static void cmsUpdate(uint32_t currentTimeUs)
{
static int16_t rcDelayMs = BUTTON_TIME;
static int holdCount = 1;
static int repeatCount = 1;
static int repeatBase = 0;
static uint32_t lastCalledMs = 0;
static uint32_t lastCmsHeartBeatMs = 0;
const uint32_t currentTimeMs = currentTimeUs / 1000;
if (!cmsInMenu) {
// Detect menu invocation
if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) {
cmsMenuOpen();
rcDelayMs = BUTTON_PAUSE; // Tends to overshoot if BUTTON_TIME
}
} else {
//
// Scan 'key' first
//
uint8_t key = KEY_NONE;
if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) {
key = KEY_MENU;
}
else if (IS_HI(PITCH)) {
key = KEY_UP;
}
else if (IS_LO(PITCH)) {
key = KEY_DOWN;
}
else if (IS_LO(ROLL)) {
key = KEY_LEFT;
}
else if (IS_HI(ROLL)) {
key = KEY_RIGHT;
}
else if (IS_HI(YAW) || IS_LO(YAW))
{
key = KEY_ESC;
}
if (key == KEY_NONE) {
// No 'key' pressed, reset repeat control
holdCount = 1;
repeatCount = 1;
repeatBase = 0;
} else {
// The 'key' is being pressed; keep counting
++holdCount;
}
if (rcDelayMs > 0) {
rcDelayMs -= (currentTimeMs - lastCalledMs);
} else if (key) {
rcDelayMs = cmsHandleKeyWithRepeat(pCurrentDisplay, key, repeatCount);
// Key repeat effect is implemented in two phases.
// First phldase is to decrease rcDelayMs reciprocal to hold time.
// When rcDelayMs reached a certain limit (scheduling interval),
// repeat rate will not raise anymore, so we call key handler
// multiple times (repeatCount).
//
// XXX Caveat: Most constants are adjusted pragmatically.
// XXX Rewrite this someday, so it uses actual hold time instead
// of holdCount, which depends on the scheduling interval.
if (((key == KEY_LEFT) || (key == KEY_RIGHT)) && (holdCount > 20)) {
// Decrease rcDelayMs reciprocally
rcDelayMs /= (holdCount - 20);
// When we reach the scheduling limit,
if (rcDelayMs <= 50) {
// start calling handler multiple times.
if (repeatBase == 0)
repeatBase = holdCount;
repeatCount = repeatCount + (holdCount - repeatBase) / 5;
if (repeatCount > 5) {
repeatCount= 5;
}
}
}
}
cmsDrawMenu(pCurrentDisplay, currentTimeUs);
if (currentTimeMs > lastCmsHeartBeatMs + 500) {
// Heart beat for external CMS display device @ 500msec
// (Timeout @ 1000msec)
displayHeartbeat(pCurrentDisplay);
lastCmsHeartBeatMs = currentTimeMs;
}
}
lastCalledMs = currentTimeMs;
}
STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
{
static timeUs_t lastTimeUs = 0;
static bool osdStatsEnabled = false;
static bool osdStatsVisible = false;
static timeUs_t osdStatsRefreshTimeUs;
static uint16_t endBatteryVoltage;
// detect arm/disarm
if (armState != ARMING_FLAG(ARMED)) {
if (ARMING_FLAG(ARMED)) {
osdStatsEnabled = false;
osdStatsVisible = false;
osdResetStats();
osdShowArmed();
resumeRefreshAt = currentTimeUs + (REFRESH_1S / 2);
} else if (isSomeStatEnabled()
&& (!(getArmingDisableFlags() & ARMING_DISABLED_RUNAWAY_TAKEOFF)
|| !VISIBLE(osdConfig()->item_pos[OSD_WARNINGS]))) { // suppress stats if runaway takeoff triggered disarm and WARNINGS element is visible
osdStatsEnabled = true;
resumeRefreshAt = currentTimeUs + (60 * REFRESH_1S);
endBatteryVoltage = getBatteryVoltage();
}
armState = ARMING_FLAG(ARMED);
}
if (ARMING_FLAG(ARMED)) {
osdUpdateStats();
timeUs_t deltaT = currentTimeUs - lastTimeUs;
flyTime += deltaT;
stats.armed_time += deltaT;
} else if (osdStatsEnabled) { // handle showing/hiding stats based on OSD disable switch position
if (displayIsGrabbed(osdDisplayPort)) {
osdStatsEnabled = false;
resumeRefreshAt = 0;
stats.armed_time = 0;
} else {
if (IS_RC_MODE_ACTIVE(BOXOSD) && osdStatsVisible) {
osdStatsVisible = false;
displayClearScreen(osdDisplayPort);
} else if (!IS_RC_MODE_ACTIVE(BOXOSD)) {
if (!osdStatsVisible) {
osdStatsVisible = true;
osdStatsRefreshTimeUs = 0;
}
if (currentTimeUs >= osdStatsRefreshTimeUs) {
osdStatsRefreshTimeUs = currentTimeUs + REFRESH_1S;
osdShowStats(endBatteryVoltage);
}
}
}
}
lastTimeUs = currentTimeUs;
if (resumeRefreshAt) {
if (cmp32(currentTimeUs, resumeRefreshAt) < 0) {
// in timeout period, check sticks for activity to resume display.
if (IS_HI(THROTTLE) || IS_HI(PITCH)) {
resumeRefreshAt = currentTimeUs;
}
displayHeartbeat(osdDisplayPort);
return;
} else {
displayClearScreen(osdDisplayPort);
resumeRefreshAt = 0;
osdStatsEnabled = false;
stats.armed_time = 0;
}
}
blinkState = (currentTimeUs / 200000) % 2;
#ifdef USE_ESC_SENSOR
if (feature(FEATURE_ESC_SENSOR)) {
escDataCombined = getEscSensorData(ESC_SENSOR_COMBINED);
}
#endif
#ifdef USE_CMS
if (!displayIsGrabbed(osdDisplayPort)) {
osdUpdateAlarms();
osdDrawElements();
displayHeartbeat(osdDisplayPort);
#ifdef OSD_CALLS_CMS
} else {
cmsUpdate(currentTimeUs);
#endif
}
#endif
lastArmState = ARMING_FLAG(ARMED);
}
