From b6aa1394f32ec1055943c91cf86974774b362d9a Mon Sep 17 00:00:00 2001 From: Christian Date: Thu, 13 Aug 2026 17:43:57 +0200 Subject: [PATCH 1/9] cms: add safe in-flight CMS access via AUX switch with NAV mode guards - Allow CMS to open while armed via BOXUSER4 (placeholder for a dedicated BOXCMS mode to be added by maintainers) - Restrict access to NAV modes only (POSHOLD, CRUISE, RTH, ALTHOLD) - Add 3-second activation countdown on OSD (MENU IN X.X) - Latch switch state to prevent re-open while switch stays ON - Force-close CMS if safety condition lost (disarm, failsafe, mode exit) - Block YAW stick gestures while armed to avoid accidental activation --- src/main/cms/cms.c | 144 +++++++++++++++++++++++++++++++++++++++--- src/main/cms/cms.h | 5 ++ src/main/fc/fc_core.c | 21 ++++++ 3 files changed, 162 insertions(+), 8 deletions(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index a32ab0af886..7211d55bb18 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -61,6 +61,7 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" #include "fc/settings.h" +#include "fc/rc_modes.h" #include "flight/mixer.h" #include "flight/servos.h" @@ -183,6 +184,42 @@ static uint8_t linesPerMenuItem; static cms_key_e externKey = CMS_KEY_NONE; bool cmsInMenu = false; +static bool cmsOpenedInFlight = false; // true when menu was opened via BOXUSER4 while armed +static bool cmsMenuSwitchLatched = false; +static uint32_t cmsOpenCountdownStartTime = 0; +static timeMs_t cmsLastInputMs = 0; + +uint32_t cmsGetOpenCountdownRemaining(void) +{ + if (cmsOpenCountdownStartTime == 0) { + return 0; + } + uint32_t elapsed = millis() - cmsOpenCountdownStartTime; + if (elapsed >= 3000) { + return 0; + } + return 3000 - elapsed; +} + +uint32_t cmsGetInactivityCloseCountdownRemaining(void) +{ + if (!cmsOpenedInFlight || cmsLastInputMs == 0) { + return 0; + } + uint32_t elapsed = millis() - cmsLastInputMs; + if (elapsed < 5000) { + return 0; + } + if (elapsed >= 15000) { + return 0; + } + return 15000 - elapsed; +} + +bool cmsIsMenuSwitchLatched(void) +{ + return cmsMenuSwitchLatched; +} typedef struct cmsCtx_s { const CMS_Menu *menu; // menu for this context @@ -794,13 +831,21 @@ void cmsMenuOpen(void) { if (!cmsInMenu) { // New open - setServoOutputEnabled(false); + if (!ARMING_FLAG(ARMED)) { + setServoOutputEnabled(false); + } pCurrentDisplay = cmsDisplayPortSelectCurrent(); if (!pCurrentDisplay) return; cmsInMenu = true; - currentCtx = (cmsCtx_t){ &menuMain, 0, 0 }; - ENABLE_ARMING_FLAG(ARMING_DISABLED_CMS_MENU); + cmsOpenedInFlight = ARMING_FLAG(ARMED); + cmsLastInputMs = millis(); + if (cmsOpenedInFlight) { + currentCtx = (cmsCtx_t){ &menuMainInFlight, 0, 0 }; + } else { + currentCtx = (cmsCtx_t){ &menuMain, 0, 0 }; + ENABLE_ARMING_FLAG(ARMING_DISABLED_CMS_MENU); + } } else { // Switch display displayPort_t *pNextDisplay = cmsDisplayPortSelectNext(); @@ -872,6 +917,10 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) case CMS_EXIT_SAVEREBOOT: case CMS_POPUP_SAVE: case CMS_POPUP_SAVEREBOOT: + if (cmsOpenedInFlight) { + // Save and reboot are not allowed while armed - treat as simple exit + break; + } cmsTraverseGlobalExit(&menuMain); @@ -899,9 +948,11 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) displayRelease(pDisplay); currentCtx.menu = NULL; - setServoOutputEnabled(true); + if (!cmsOpenedInFlight) { + setServoOutputEnabled(true); + } - if ((exitType == CMS_EXIT_SAVEREBOOT) || (exitType == CMS_POPUP_SAVEREBOOT)) { + if (!cmsOpenedInFlight && ((exitType == CMS_EXIT_SAVEREBOOT) || (exitType == CMS_POPUP_SAVEREBOOT))) { processDelayedSave(); displayClearScreen(pDisplay); displayWrite(pDisplay, 5, 3, "REBOOTING..."); @@ -911,7 +962,14 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) fcReboot(false); } - DISABLE_ARMING_FLAG(ARMING_DISABLED_CMS_MENU); + if (!cmsOpenedInFlight) { + DISABLE_ARMING_FLAG(ARMING_DISABLED_CMS_MENU); + } else { + // Latch the switch so it doesn't reopen immediately if still ON + cmsMenuSwitchLatched = IS_RC_MODE_ACTIVE(BOXUSER4); + } + + cmsOpenedInFlight = false; return 0; } @@ -1262,9 +1320,9 @@ static uint16_t cmsScanKeys(timeMs_t currentTimeMs, timeMs_t lastCalledMs, int16 key = CMS_KEY_LEFT; } else if (IS_HI(ROLL)) { key = CMS_KEY_RIGHT; - } else if (IS_LO(YAW)) { + } else if (IS_LO(YAW) && !cmsOpenedInFlight) { key = CMS_KEY_ESC; - } else if (IS_HI(YAW)) { + } else if (IS_HI(YAW) && !cmsOpenedInFlight) { key = CMS_KEY_SAVEMENU; } @@ -1276,6 +1334,7 @@ static uint16_t cmsScanKeys(timeMs_t currentTimeMs, timeMs_t lastCalledMs, int16 } else { // The 'key' is being pressed; keep counting ++holdCount; + cmsLastInputMs = currentTimeMs; } if (rcDelayMs > 0) { @@ -1330,6 +1389,15 @@ static uint16_t cmsScanKeys(timeMs_t currentTimeMs, timeMs_t lastCalledMs, int16 return rcDelayMs; } +static bool cmsIsNavModeActive(void) +{ + return FLIGHT_MODE(NAV_POSHOLD_MODE) || + FLIGHT_MODE(NAV_RTH_MODE) || + FLIGHT_MODE(NAV_WP_MODE) || + FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || + FLIGHT_MODE(NAV_ALTHOLD_MODE); +} + void cmsUpdate(uint32_t currentTimeUs) { #ifdef USE_RCDEVICE @@ -1345,13 +1413,52 @@ void cmsUpdate(uint32_t currentTimeUs) const timeMs_t currentTimeMs = currentTimeUs / 1000; + if (!IS_RC_MODE_ACTIVE(BOXUSER4)) { + cmsMenuSwitchLatched = false; + cmsOpenCountdownStartTime = 0; + } + 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 } + // In-flight menu via BOXUSER4 mode - requires armed state, a NAV mode, + // and no active failsafe. Without a NAV mode the stick override would + // leave the aircraft with zeroed control inputs. + else if (IS_RC_MODE_ACTIVE(BOXUSER4) && ARMING_FLAG(ARMED) + && cmsIsNavModeActive() && !FLIGHT_MODE(FAILSAFE_MODE)) { + + if (!cmsMenuSwitchLatched) { + if (cmsOpenCountdownStartTime == 0) { + cmsOpenCountdownStartTime = millis(); + } else if (millis() - cmsOpenCountdownStartTime >= 3000) { + cmsMenuOpen(); + cmsOpenCountdownStartTime = 0; + rcDelayMs = BUTTON_PAUSE; + } + } + } else { + cmsOpenCountdownStartTime = 0; + } } else { + // Close menu immediately if opened in-flight and any safety condition is lost: + // - BOXUSER4 switch deactivated (user wants to exit) + // - Aircraft disarmed + // - Failsafe activated (pilot must regain situational awareness) + // - NAV mode lost (stick override would leave aircraft without stabilization) + if (cmsOpenedInFlight && (!IS_RC_MODE_ACTIVE(BOXUSER4) || !ARMING_FLAG(ARMED) + || FLIGHT_MODE(FAILSAFE_MODE) || !cmsIsNavModeActive())) { + cmsMenuExit(pCurrentDisplay, (void *)CMS_EXIT); + return; + } + + if (cmsOpenedInFlight && (currentTimeMs - cmsLastInputMs >= 15000)) { + cmsMenuExit(pCurrentDisplay, (void *)CMS_EXIT); + return; + } + displayBeginTransaction(pCurrentDisplay, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); // Check if we're yielding and its's time to stop it @@ -1369,6 +1476,27 @@ void cmsUpdate(uint32_t currentTimeUs) // Check again, the keypress might have produced a yield if (cmsYieldUntil == 0) { cmsDrawMenu(pCurrentDisplay, currentTimeUs); + + static bool wasDrawingCountdown = false; + if (cmsOpenedInFlight) { + uint32_t elapsed = currentTimeMs - cmsLastInputMs; + if (elapsed >= 5000) { + uint32_t remaining = 15000 - elapsed; + unsigned sec = remaining / 1000; + char buf[22]; + tfp_sprintf(buf, " CLOSING IN %u ", sec); + int col = (pCurrentDisplay->cols - strlen(buf)) / 2; + if (col < 0) col = 0; + displayWrite(pCurrentDisplay, col, pCurrentDisplay->rows - 1, buf); + wasDrawingCountdown = true; + } else if (wasDrawingCountdown) { + // Clear the bottom row + char buf[32] = " "; + buf[pCurrentDisplay->cols > 0 && pCurrentDisplay->cols < 32 ? pCurrentDisplay->cols : 30] = '\0'; + displayWrite(pCurrentDisplay, 0, pCurrentDisplay->rows - 1, buf); + wasDrawingCountdown = false; + } + } } } diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h index 7084df8b50c..4946b722d83 100644 --- a/src/main/cms/cms.h +++ b/src/main/cms/cms.h @@ -33,6 +33,11 @@ displayPort_t *cmsDisplayPortGetCurrent(void); void cmsMenuOpen(void); long cmsMenuChange(displayPort_t *pPort, const CMS_Menu *menu, const OSD_Entry *from); long cmsMenuExit(displayPort_t *pPort, const void *ptr); + +uint32_t cmsGetOpenCountdownRemaining(void); +uint32_t cmsGetInactivityCloseCountdownRemaining(void); +bool cmsIsMenuSwitchLatched(void); + void cmsYieldDisplay(displayPort_t *pPort, timeMs_t duration); void cmsUpdate(uint32_t currentTimeUs); void cmsSetExternKey(cms_key_e extKey); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3223aca497e..4f1a3aa55e5 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -41,6 +41,9 @@ #include "sensors/sensors.h" #include "sensors/diagnostics.h" #include "sensors/boardalignment.h" +#ifdef USE_CMS +#include "cms/cms.h" +#endif #include "sensors/acceleration.h" #include "sensors/barometer.h" #include "sensors/compass.h" @@ -390,6 +393,24 @@ static void processPilotAndFailSafeActions(float dT) failsafeApplyControlInput(); } else { +#ifdef USE_CMS + // In-flight CMS menu: override stick commands with neutral values + // so the aircraft continues flying in its current nav mode. + // Throttle passes through since nav modes manage it automatically. + // IMPORTANT: Do not skip failsafeUpdateRcCommandValues() - the failsafe + // system must keep receiving updates to detect RC link loss. + if (cmsInMenu && ARMING_FLAG(ARMED)) { + rcCommand[ROLL] = 0; + rcCommand[PITCH] = 0; + rcCommand[YAW] = 0; + rcCommand[THROTTLE] = throttleStickMixedValue(); + + if (isRXDataNew) { + failsafeUpdateRcCommandValues(); + } + return; + } +#endif // Compute ROLL PITCH and YAW command. // Only recompute when the RX task has delivered new data (~50 Hz). { From 0e35b4d78d57f4c269d7eaea7acc34dabcf3578b Mon Sep 17 00:00:00 2001 From: Christian Date: Thu, 13 Aug 2026 17:44:15 +0200 Subject: [PATCH 2/9] osd: add in-flight CMS OSD feedback elements - Display hardware-blinking warning (USE NAV MODES FOR MENU) when pilot attempts CMS activation outside of valid NAV modes - Display opening countdown (MENU IN X.X) during 3s delay - Display inactivity auto-close countdown (CLOSING IN X) in the last 10 seconds before the 15s timeout triggers - Clean up countdown text immediately when pilot resumes input to avoid ghost characters on the OSD screen --- src/main/io/osd.c | 14 +++++++++++++- src/main/io/osd.h | 1 + 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index aa0a01ce71b..a20d3f9c599 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -84,6 +84,7 @@ #include "fc/multifunction.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/settings.h" #include "flight/imu.h" @@ -6233,6 +6234,17 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } else { /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active and landed state inactive */ /* ADDS MAXIMUM OF 5 MESSAGES TO TOTAL */ +#ifdef USE_CMS + uint32_t menuCountdownMs = cmsGetOpenCountdownRemaining(); + if (menuCountdownMs > 0) { + unsigned sec = menuCountdownMs / 1000; + unsigned dec = (menuCountdownMs % 1000) / 100; + tfp_sprintf(messageBuf, "MENU IN %u.%u", sec, dec); + ADD_MSG(messageBuf); + } else if (IS_RC_MODE_ACTIVE(BOXUSER4) && !cmsInMenu && !cmsIsMenuSwitchLatched()) { + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_MENU_NAV_REQ)); + } +#endif #ifdef USE_GEOZONE char buf[12], buf1[12]; switch (geozone.messageState) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */ @@ -6413,7 +6425,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (messageCount > 0) { message = messages[OSD_ALTERNATING_CHOICES(systemMessageCycleTime(messageCount, messages), messageCount)]; - if (message == failsafeInfoMessage) { + if (message == failsafeInfoMessage || message == OSD_MESSAGE_STR(OSD_MSG_MENU_NAV_REQ)) { // failsafeInfoMessage is not useful for recovering // a lost model, but might help avoiding a crash. // Blink to grab user attention. diff --git a/src/main/io/osd.h b/src/main/io/osd.h index e55aeb22461..37242c9818c 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -124,6 +124,7 @@ #define OSD_MSG_ANGLEHOLD_PITCH "(ANGLEHOLD PITCH)" #define OSD_MSG_ANGLEHOLD_LEVEL "(ANGLEHOLD LEVEL)" #define OSD_MSG_MOVE_STICKS "MOVE STICKS TO ABORT" +#define OSD_MSG_MENU_NAV_REQ "USE NAV MODES FOR MENU" #ifdef USE_DEV_TOOLS #define OSD_MSG_GRD_TEST_MODE "GRD TEST > MOTORS DISABLED" From 35d366c8646b48a6358858217b2b55d0042ad4c4 Mon Sep 17 00:00:00 2001 From: Christian Date: Thu, 13 Aug 2026 17:44:36 +0200 Subject: [PATCH 3/9] cms: add cloned in-flight menus excluding dangerous ground-only items - Introduce menuMainInFlight and menuFeaturesInFlight as static const clones of the standard menus, omitting SAVE+REBOOT, BLACKBOX, and MIXER and SERVOS entries - Select the appropriate root menu in cmsMenuOpen() based on the cmsOpenedInFlight flag set at open time - Ground menu (disarmed) is completely unaffected and remains 100 percent identical to stock INAV behavior - Avoids dynamic entry hiding which would break CMS pagination math --- src/main/cms/cms_menu_builtin.c | 57 +++++++++++++++++++++++++++++++++ src/main/cms/cms_menu_builtin.h | 1 + 2 files changed, 58 insertions(+) diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c index e9849df8619..5312b225efe 100644 --- a/src/main/cms/cms_menu_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -157,4 +157,61 @@ const CMS_Menu menuMain = { .onGlobalExit = NULL, .entries = menuMainEntries, }; + +static const OSD_Entry menuFeaturesInFlightEntries[] = +{ + OSD_LABEL_ENTRY("--- FEATURES ---"), + OSD_SUBMENU_ENTRY("NAVIGATION", &cmsx_menuNavigation), +#if defined(USE_VTX_CONTROL) + OSD_SUBMENU_ENTRY("VTX", &cmsx_menuVtxControl), +#endif // VTX_CONTROL +#ifdef USE_LED_STRIP + OSD_SUBMENU_ENTRY("LED STRIP", &cmsx_menuLedstrip), +#endif // LED_STRIP + + OSD_BACK_AND_END_ENTRY, +}; + +static const CMS_Menu menuFeaturesInFlight = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "MENUFEAT_IF", + .GUARD_type = OME_MENU, +#endif + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuFeaturesInFlightEntries, +}; + +static const OSD_Entry menuMainInFlightEntries[] = +{ + OSD_LABEL_ENTRY("-- MAIN --"), + + OSD_SUBMENU_ENTRY("PID TUNING", &cmsx_menuImu), + OSD_SUBMENU_ENTRY("FEATURES", &menuFeaturesInFlight), +#if defined(USE_OSD) && defined(CMS_MENU_OSD) + OSD_SUBMENU_ENTRY("OSD", &cmsx_menuOsd), +#endif + OSD_SUBMENU_ENTRY("BATTERY", &cmsx_menuBattery), + OSD_SUBMENU_ENTRY("FC+FW INFO", &menuInfo), + OSD_SUBMENU_ENTRY("MISC", &cmsx_menuMisc), + + {"EXIT" , {.func = cmsMenuExit}, (void*)CMS_EXIT, OME_OSD_Exit, 0}, +#ifdef CMS_MENU_DEBUG + OSD_SUBMENU_ENTRY("ERR SAMPLE", &menuInfoEntries[0]), +#endif + + OSD_END_ENTRY, +}; + +const CMS_Menu menuMainInFlight = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "MENUMAIN_IF", + .GUARD_type = OME_MENU, +#endif + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuMainInFlightEntries, +}; #endif diff --git a/src/main/cms/cms_menu_builtin.h b/src/main/cms/cms_menu_builtin.h index 35be458fe76..508c69f9f94 100644 --- a/src/main/cms/cms_menu_builtin.h +++ b/src/main/cms/cms_menu_builtin.h @@ -20,3 +20,4 @@ #include "cms/cms_types.h" extern const CMS_Menu menuMain; +extern const CMS_Menu menuMainInFlight; From b838bbbe15b8fe02b7114adfbf0626e7dd315cd7 Mon Sep 17 00:00:00 2001 From: Christian Date: Thu, 13 Aug 2026 17:45:10 +0200 Subject: [PATCH 4/9] cms: add explicit SET confirmation submenus to PID tuning menus - Decouple PID RAM writeback from the BACK button in cmsx_menuPid, cmsx_menuPidAltMag and cmsx_menuPidGpsnav by setting onExit to NULL - Add a SET submenu entry before BACK in all three PID lists - Each SET submenu shows a CONFIRM page with YES and NO options YES commits the edited values to RAM via the original writeback function and returns to the PID menu via MENU_CHAIN_BACK NO simply goes back discarding all pending changes - This prevents accidental mid-flight PID commits when the pilot presses BACK or the auto-close timer fires --- src/main/cms/cms_menu_imu.c | 75 +++++++++++++++++++++++++++++++++++-- 1 file changed, 72 insertions(+), 3 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 2747697d222..67fd2e0d991 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -161,6 +161,28 @@ static const CMS_Menu cmsx_menuEzTune = { .entries = cmsx_menuEzTuneEntries }; +static long cmsx_PidWriteback_Confirm(displayPort_t *displayPort, const void *ptr) +{ + UNUSED(displayPort); + UNUSED(ptr); + cmsx_PidWriteback(NULL); + return MENU_CHAIN_BACK; +} + +static const OSD_Entry cmsx_menuPidConfirmEntries[] = { + OSD_LABEL_ENTRY("--- CONFIRM ---"), + OSD_FUNC_CALL_ENTRY("YES", cmsx_PidWriteback_Confirm), + { "NO", {.func = NULL}, NULL, OME_Back, 0 }, + OSD_END_ENTRY +}; + +static const CMS_Menu cmsx_menuPidConfirm = { + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = cmsx_menuPidConfirmEntries, +}; + static const OSD_Entry cmsx_menuPidEntries[] = { OSD_LABEL_DATA_ENTRY("-- PID --", profileIndexString), @@ -180,6 +202,7 @@ static const OSD_Entry cmsx_menuPidEntries[] = RPY_PIDFF_ENTRY("YAW D", &cmsx_pidYaw.D), RPY_PIDFF_ENTRY("YAW FF", &cmsx_pidYaw.FF), + OSD_SUBMENU_ENTRY("SET", &cmsx_menuPidConfirm), OSD_BACK_AND_END_ENTRY, }; @@ -189,7 +212,7 @@ static const CMS_Menu cmsx_menuPid = { .GUARD_type = OME_MENU, #endif .onEnter = cmsx_PidOnEnter, - .onExit = cmsx_PidWriteback, + .onExit = NULL, .onGlobalExit = NULL, .entries = cmsx_menuPidEntries }; @@ -218,6 +241,28 @@ static long cmsx_menuPidAltMag_onExit(const OSD_Entry *self) return 0; } +static long cmsx_menuPidAltMag_onExit_Confirm(displayPort_t *displayPort, const void *ptr) +{ + UNUSED(displayPort); + UNUSED(ptr); + cmsx_menuPidAltMag_onExit(NULL); + return MENU_CHAIN_BACK; +} + +static const OSD_Entry cmsx_menuPidAltMagConfirmEntries[] = { + OSD_LABEL_ENTRY("--- CONFIRM ---"), + OSD_FUNC_CALL_ENTRY("YES", cmsx_menuPidAltMag_onExit_Confirm), + { "NO", {.func = NULL}, NULL, OME_Back, 0 }, + OSD_END_ENTRY +}; + +static const CMS_Menu cmsx_menuPidAltMagConfirm = { + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = cmsx_menuPidAltMagConfirmEntries, +}; + static const OSD_Entry cmsx_menuPidAltMagEntries[] = { OSD_LABEL_DATA_ENTRY("-- ALT&MAG --", profileIndexString), @@ -235,6 +280,7 @@ static const OSD_Entry cmsx_menuPidAltMagEntries[] = OTHER_PIDFF_ENTRY("MAG P", &cmsx_pidHead.P), + OSD_SUBMENU_ENTRY("SET", &cmsx_menuPidAltMagConfirm), OSD_BACK_AND_END_ENTRY, }; @@ -244,7 +290,7 @@ static const CMS_Menu cmsx_menuPidAltMag = { .GUARD_type = OME_MENU, #endif .onEnter = cmsx_menuPidAltMag_onEnter, - .onExit = cmsx_menuPidAltMag_onExit, + .onExit = NULL, .onGlobalExit = NULL, .entries = cmsx_menuPidAltMagEntries, }; @@ -271,6 +317,28 @@ static long cmsx_menuPidGpsnav_onExit(const OSD_Entry *self) return 0; } +static long cmsx_menuPidGpsnav_onExit_Confirm(displayPort_t *displayPort, const void *ptr) +{ + UNUSED(displayPort); + UNUSED(ptr); + cmsx_menuPidGpsnav_onExit(NULL); + return MENU_CHAIN_BACK; +} + +static const OSD_Entry cmsx_menuPidGpsnavConfirmEntries[] = { + OSD_LABEL_ENTRY("--- CONFIRM ---"), + OSD_FUNC_CALL_ENTRY("YES", cmsx_menuPidGpsnav_onExit_Confirm), + { "NO", {.func = NULL}, NULL, OME_Back, 0 }, + OSD_END_ENTRY +}; + +static const CMS_Menu cmsx_menuPidGpsnavConfirm = { + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = cmsx_menuPidGpsnavConfirmEntries, +}; + static const OSD_Entry cmsx_menuPidGpsnavEntries[] = { OSD_LABEL_DATA_ENTRY("-- GPSNAV --", profileIndexString), @@ -284,6 +352,7 @@ static const OSD_Entry cmsx_menuPidGpsnavEntries[] = OTHER_PIDFF_ENTRY("VEL D", &cmsx_pidVelXY.D), OTHER_PIDFF_ENTRY("VEL FF", &cmsx_pidVelXY.FF), + OSD_SUBMENU_ENTRY("SET", &cmsx_menuPidGpsnavConfirm), OSD_BACK_AND_END_ENTRY, }; @@ -293,7 +362,7 @@ static const CMS_Menu cmsx_menuPidGpsnav = { .GUARD_type = OME_MENU, #endif .onEnter = cmsx_menuPidGpsnav_onEnter, - .onExit = cmsx_menuPidGpsnav_onExit, + .onExit = NULL, .onGlobalExit = NULL, .entries = cmsx_menuPidGpsnavEntries, }; From d47ab453c61e3cf178b197c7467ca575c9d9ae0f Mon Sep 17 00:00:00 2001 From: Christian Date: Thu, 13 Aug 2026 17:45:36 +0200 Subject: [PATCH 5/9] cms: fix in-flight VTX save and add live battery profile re-init - cms_menu_vtx: guard saveConfigAndNotify() with !ARMING_FLAG(ARMED) so that confirming VTX settings in flight applies the new band, channel and power to the hardware and RAM immediately without triggering an EEPROM write or the saving settings OSD message and without causing ESC beeps on landing Ground behavior (disarmed) remains unchanged - cms_menu_battery: call batteryInit() on exit from battery settings and battery menu only when ARMING_FLAG(ARMED) is set, so that changes to cell count and voltage thresholds take effect instantly in RAM without requiring SAVE+REBOOT during flight Ground menu behavior (disarmed) remains completely unaffected --- src/main/cms/cms.c | 16 +++++++--------- src/main/cms/cms_menu_battery.c | 17 ++++++++++++++++- src/main/cms/cms_menu_vtx.c | 5 ++++- 3 files changed, 27 insertions(+), 11 deletions(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 7211d55bb18..cef695df397 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -950,19 +950,17 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) if (!cmsOpenedInFlight) { setServoOutputEnabled(true); - } - if (!cmsOpenedInFlight && ((exitType == CMS_EXIT_SAVEREBOOT) || (exitType == CMS_POPUP_SAVEREBOOT))) { - processDelayedSave(); - displayClearScreen(pDisplay); - displayWrite(pDisplay, 5, 3, "REBOOTING..."); + if ((exitType == CMS_EXIT_SAVEREBOOT) || (exitType == CMS_POPUP_SAVEREBOOT)) { + processDelayedSave(); + displayClearScreen(pDisplay); + displayWrite(pDisplay, 5, 3, "REBOOTING..."); - displayResync(pDisplay); // Was max7456RefreshAll(); why at this timing? + displayResync(pDisplay); // Was max7456RefreshAll(); why at this timing? - fcReboot(false); - } + fcReboot(false); + } - if (!cmsOpenedInFlight) { DISABLE_ARMING_FLAG(ARMING_DISABLED_CMS_MENU); } else { // Latch the switch so it doesn't reopen immediately if still ON diff --git a/src/main/cms/cms_menu_battery.c b/src/main/cms/cms_menu_battery.c index c6abcdace1b..201be0a3e1b 100644 --- a/src/main/cms/cms_menu_battery.c +++ b/src/main/cms/cms_menu_battery.c @@ -32,6 +32,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/runtime_config.h" #include "fc/settings.h" #include "sensors/battery.h" @@ -62,6 +63,9 @@ static long cmsx_menuBattery_onExit(const OSD_Entry *self) setConfigBatteryProfile(battProfileIndex); activateBatteryProfile(); + if (ARMING_FLAG(ARMED)) { + batteryInit(); + } if (featureProfAutoswitchEnabled) { featureSet(FEATURE_BAT_PROFILE_AUTOSWITCH); @@ -93,6 +97,17 @@ static long cmsx_menuBattSettings_onEnter(const OSD_Entry *from) return 0; } +static long cmsx_menuBattSettings_onExit(const OSD_Entry *self) +{ + UNUSED(self); + + if (ARMING_FLAG(ARMED)) { + batteryInit(); + } + + return 0; +} + static const OSD_Entry menuBattSettingsEntries[]= { OSD_LABEL_DATA_ENTRY("-- BATT SETTINGS --", battProfileIndexString), @@ -118,7 +133,7 @@ static CMS_Menu cmsx_menuBattSettings = { .GUARD_type = OME_MENU, #endif .onEnter = cmsx_menuBattSettings_onEnter, - .onExit = NULL, + .onExit = cmsx_menuBattSettings_onExit, .onGlobalExit = NULL, .entries = menuBattSettingsEntries }; diff --git a/src/main/cms/cms_menu_vtx.c b/src/main/cms/cms_menu_vtx.c index 249683fe879..a56656ec8a1 100644 --- a/src/main/cms/cms_menu_vtx.c +++ b/src/main/cms/cms_menu_vtx.c @@ -36,6 +36,7 @@ #include "drivers/vtx_common.h" #include "fc/config.h" +#include "fc/runtime_config.h" #include "io/vtx_string.h" #include "io/vtx.h" @@ -166,7 +167,9 @@ static long cms_Vtx_Commence(displayPort_t *pDisp, const void *self) vtxSettingsConfigMutable()->channel = vtxChan; vtxSettingsConfigMutable()->power = vtxPower; - saveConfigAndNotify(); + if (!ARMING_FLAG(ARMED)) { + saveConfigAndNotify(); + } return MENU_CHAIN_BACK; } From 3128dbc2b857d30c8a08faf5e4573a725ee94eae Mon Sep 17 00:00:00 2001 From: Christian Date: Thu, 13 Aug 2026 20:16:51 +0200 Subject: [PATCH 6/9] cms: filter out non-live items from in-flight menus and fix profile switch delays - Introduce cmsx_menuImuInFlight, cmsx_menuBatteryInFlight, and cmsx_menuMiscInFlight omitting submenus that cannot be updated live in flight (EZTUNE, FILTERING, MECHANICS, PROF AUTOSWITCH, FS PROCEDURE) - Add immediate schedulePidGainsUpdate(), navigationUsePIDs(), and activateControlConfig() to cmsx_profileIndexOnChange() for seamless in-flight profile switching - Disarmed ground menus remain 100 percent complete and untouched --- src/main/cms/cms_menu_battery.c | 21 +++++++++++++++++++++ src/main/cms/cms_menu_battery.h | 1 + src/main/cms/cms_menu_builtin.c | 6 +++--- src/main/cms/cms_menu_imu.c | 32 ++++++++++++++++++++++++++++++++ src/main/cms/cms_menu_imu.h | 1 + src/main/cms/cms_menu_misc.c | 30 ++++++++++++++++++++++++++++++ src/main/cms/cms_menu_misc.h | 1 + 7 files changed, 89 insertions(+), 3 deletions(-) diff --git a/src/main/cms/cms_menu_battery.c b/src/main/cms/cms_menu_battery.c index 201be0a3e1b..4115773845d 100644 --- a/src/main/cms/cms_menu_battery.c +++ b/src/main/cms/cms_menu_battery.c @@ -162,4 +162,25 @@ CMS_Menu cmsx_menuBattery = { .entries = menuBatteryEntries }; +static OSD_Entry menuBatteryInFlightEntries[]= +{ + OSD_LABEL_ENTRY("-- BATTERY --"), + + OSD_UINT8_CALLBACK_ENTRY("PROF", cmsx_onBatteryProfileIndexChange, (&(const OSD_UINT8_t){ &battDispProfileIndex, 1, MAX_BATTERY_PROFILE_COUNT, 1})), + OSD_SUBMENU_ENTRY("SETTINGS", &cmsx_menuBattSettings), + + OSD_BACK_AND_END_ENTRY, +}; + +const CMS_Menu cmsx_menuBatteryInFlight = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "XBATT_IF", + .GUARD_type = OME_MENU, +#endif + .onEnter = cmsx_menuBattery_onEnter, + .onExit = cmsx_menuBattery_onExit, + .onGlobalExit = NULL, + .entries = menuBatteryInFlightEntries +}; + #endif // CMS diff --git a/src/main/cms/cms_menu_battery.h b/src/main/cms/cms_menu_battery.h index ca4e10643b0..0dd869063e5 100644 --- a/src/main/cms/cms_menu_battery.h +++ b/src/main/cms/cms_menu_battery.h @@ -18,3 +18,4 @@ #pragma once extern CMS_Menu cmsx_menuBattery; +extern const CMS_Menu cmsx_menuBatteryInFlight; diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c index 5312b225efe..e308eda6a27 100644 --- a/src/main/cms/cms_menu_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -187,14 +187,14 @@ static const OSD_Entry menuMainInFlightEntries[] = { OSD_LABEL_ENTRY("-- MAIN --"), - OSD_SUBMENU_ENTRY("PID TUNING", &cmsx_menuImu), + OSD_SUBMENU_ENTRY("PID TUNING", &cmsx_menuImuInFlight), OSD_SUBMENU_ENTRY("FEATURES", &menuFeaturesInFlight), #if defined(USE_OSD) && defined(CMS_MENU_OSD) OSD_SUBMENU_ENTRY("OSD", &cmsx_menuOsd), #endif - OSD_SUBMENU_ENTRY("BATTERY", &cmsx_menuBattery), + OSD_SUBMENU_ENTRY("BATTERY", &cmsx_menuBatteryInFlight), OSD_SUBMENU_ENTRY("FC+FW INFO", &menuInfo), - OSD_SUBMENU_ENTRY("MISC", &cmsx_menuMisc), + OSD_SUBMENU_ENTRY("MISC", &cmsx_menuMiscInFlight), {"EXIT" , {.func = cmsMenuExit}, (void*)CMS_EXIT, OME_OSD_Exit, 0}, #ifdef CMS_MENU_DEBUG diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 67fd2e0d991..482adc23a2c 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -101,6 +101,9 @@ static long cmsx_profileIndexOnChange(displayPort_t *displayPort, const void *pt profileIndex = tmpProfileIndex - 1; profileIndexString[1] = '0' + tmpProfileIndex; setConfigProfile(profileIndex); + schedulePidGainsUpdate(); + navigationUsePIDs(); + activateControlConfig(); return 0; } @@ -586,4 +589,33 @@ const CMS_Menu cmsx_menuImu = { .onGlobalExit = NULL, .entries = cmsx_menuImuEntries, }; + +static const OSD_Entry cmsx_menuImuInFlightEntries[] = +{ + OSD_LABEL_ENTRY("-- PID TUNING --"), + + // Profile dependent + OSD_UINT8_CALLBACK_ENTRY("PID PROF", cmsx_profileIndexOnChange, (&(const OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_PROFILE_COUNT, 1})), + OSD_SUBMENU_ENTRY("PID", &cmsx_menuPid), + OSD_SUBMENU_ENTRY("PID ALTMAG", &cmsx_menuPidAltMag), + OSD_SUBMENU_ENTRY("PID GPSNAV", &cmsx_menuPidGpsnav), + + // Rate profile dependent + OSD_UINT8_CALLBACK_ENTRY("RATE PROF", cmsx_profileIndexOnChange, (&(const OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_CONTROL_PROFILE_COUNT, 1})), + OSD_SUBMENU_ENTRY("RATE", &cmsx_menuRateProfile), + OSD_SUBMENU_ENTRY("MANU RATE", &cmsx_menuManualRateProfile), + + OSD_BACK_AND_END_ENTRY, +}; + +const CMS_Menu cmsx_menuImuInFlight = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "XIMU_IF", + .GUARD_type = OME_MENU, +#endif + .onEnter = cmsx_menuImu_onEnter, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = cmsx_menuImuInFlightEntries, +}; #endif // CMS diff --git a/src/main/cms/cms_menu_imu.h b/src/main/cms/cms_menu_imu.h index 8219e4166e6..d80766b3129 100644 --- a/src/main/cms/cms_menu_imu.h +++ b/src/main/cms/cms_menu_imu.h @@ -18,3 +18,4 @@ #pragma once extern const CMS_Menu cmsx_menuImu; +extern const CMS_Menu cmsx_menuImuInFlight; diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index 248dcc9edcf..5c8b114bcec 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -70,4 +70,34 @@ const CMS_Menu cmsx_menuMisc = { .entries = menuMiscEntries }; +static const OSD_Entry menuMiscInFlightEntries[]= +{ + OSD_LABEL_ENTRY("-- MISC --"), + + OSD_SETTING_ENTRY("THR IDLE", SETTING_THROTTLE_IDLE), +#ifdef USE_DEV_TOOLS + OSD_SETTING_ENTRY("GROUND TEST MODE", SETTING_GROUND_TEST_MODE), +#endif +#ifdef USE_OSD +#ifdef USE_ADC + OSD_SETTING_ENTRY("OSD VOLT DECIMALS", SETTING_OSD_MAIN_VOLTAGE_DECIMALS), + OSD_SETTING_ENTRY("STATS ENERGY UNIT", SETTING_OSD_STATS_ENERGY_UNIT), +#endif // ADC + OSD_SETTING_ENTRY("STATS PAGE SWAP TIME", SETTING_OSD_STATS_PAGE_AUTO_SWAP_TIME), +#endif // OSD + + OSD_BACK_AND_END_ENTRY, +}; + +const CMS_Menu cmsx_menuMiscInFlight = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "XMISC_IF", + .GUARD_type = OME_MENU, +#endif + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuMiscInFlightEntries +}; + #endif // CMS diff --git a/src/main/cms/cms_menu_misc.h b/src/main/cms/cms_menu_misc.h index fa776acf004..747f0f13ac6 100644 --- a/src/main/cms/cms_menu_misc.h +++ b/src/main/cms/cms_menu_misc.h @@ -18,3 +18,4 @@ #pragma once extern const CMS_Menu cmsx_menuMisc; +extern const CMS_Menu cmsx_menuMiscInFlight; From 67b6e4bad41baff06d76a6fe90ea9055d35f581d Mon Sep 17 00:00:00 2001 From: Christian Date: Fri, 14 Aug 2026 11:24:26 +0200 Subject: [PATCH 7/9] cms: drop standalone NAV_COURSE_HOLD_MODE from in-flight menu active check - Require altitude-holding modes (POSHOLD, RTH, WP, ALTHOLD) to ensure altitude stabilization while accessing the CMS in flight. - Standalone Course Hold only locks heading without controlling pitch/altitude. CRUISE mode remains supported as it activates NAV_ALTHOLD_MODE alongside heading hold. --- src/main/cms/cms.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index cef695df397..3e0a166ab04 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -1392,7 +1392,6 @@ static bool cmsIsNavModeActive(void) return FLIGHT_MODE(NAV_POSHOLD_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || - FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || FLIGHT_MODE(NAV_ALTHOLD_MODE); } From b95bdc7a9d4f446e751ffa08c10096be2a5c7743 Mon Sep 17 00:00:00 2001 From: Christian Date: Fri, 14 Aug 2026 19:14:40 +0200 Subject: [PATCH 8/9] cms: add onExit stack unwinding on in-flight auto-close and non-zeroing battery refresh - Unwind menuStack and invoke per-menu onExit callbacks during in-flight CMS_EXIT so transient state (e.g. OSD layout preview override) is cleanly cleared when the menu collapses due to timeout, switch-off, or emergency - Introduce batteryUpdateThresholdsAndCells() to seamlessly recompute cell count and voltage thresholds in RAM without resetting batteryState or zeroing batteryFullVoltage, preventing transient throttle scaling glitches in mixer --- src/main/cms/cms.c | 10 ++++++++++ src/main/cms/cms_menu_battery.c | 4 ++-- src/main/sensors/battery.c | 21 +++++++++++++++++++++ src/main/sensors/battery.h | 1 + 4 files changed, 34 insertions(+), 2 deletions(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 3e0a166ab04..d3b1826bb4b 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -940,6 +940,16 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) break; case CMS_EXIT: + if (cmsOpenedInFlight) { + if (currentCtx.menu && currentCtx.menu->onExit) { + currentCtx.menu->onExit((OSD_Entry *)NULL); + } + for (int i = menuStackIdx - 1; i >= 0; i--) { + if (menuStack[i].menu && menuStack[i].menu->onExit) { + menuStack[i].menu->onExit((OSD_Entry *)NULL); + } + } + } break; } diff --git a/src/main/cms/cms_menu_battery.c b/src/main/cms/cms_menu_battery.c index 4115773845d..9a5a45e1359 100644 --- a/src/main/cms/cms_menu_battery.c +++ b/src/main/cms/cms_menu_battery.c @@ -64,7 +64,7 @@ static long cmsx_menuBattery_onExit(const OSD_Entry *self) setConfigBatteryProfile(battProfileIndex); activateBatteryProfile(); if (ARMING_FLAG(ARMED)) { - batteryInit(); + batteryUpdateThresholdsAndCells(); } if (featureProfAutoswitchEnabled) { @@ -102,7 +102,7 @@ static long cmsx_menuBattSettings_onExit(const OSD_Entry *self) UNUSED(self); if (ARMING_FLAG(ARMED)) { - batteryInit(); + batteryUpdateThresholdsAndCells(); } return 0; diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index e2e23cf21d4..0e78219a6a5 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -207,6 +207,27 @@ void batteryInit(void) batteryCriticalVoltage = 0; } +void batteryUpdateThresholdsAndCells(void) +{ + if (batteryState == BATTERY_NOT_PRESENT) { + return; + } + + if (currentBatteryProfile->cells > 0) { + batteryCellCount = currentBatteryProfile->cells; + } else if (currentBatteryProfile->voltage.cellDetect > 0) { + batteryCellCount = (vbat / currentBatteryProfile->voltage.cellDetect) + 1; + if (batteryCellCount == 7 || batteryCellCount == 9 || batteryCellCount == 11) { + batteryCellCount += 1; + } + batteryCellCount = MIN(batteryCellCount, 12); + } + + batteryFullVoltage = batteryCellCount * currentBatteryProfile->voltage.cellMax; + batteryWarningVoltage = batteryCellCount * currentBatteryProfile->voltage.cellWarning; + batteryCriticalVoltage = batteryCellCount * currentBatteryProfile->voltage.cellMin; +} + #ifdef USE_ADC // profileDetect() profile sorting compare function static int profile_compare(profile_comp_t *a, profile_comp_t *b) { diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 0ecde181b37..3455d515685 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -69,6 +69,7 @@ bool batteryUsesCapacityThresholds(void); void batteryInit(void); void setBatteryProfile(uint8_t profileIndex); void activateBatteryProfile(void); +void batteryUpdateThresholdsAndCells(void); void batteryDisableProfileAutoswitch(void); bool isBatteryVoltageConfigured(void); From 3689d8cff7da00b8422b8222109433c33398c249 Mon Sep 17 00:00:00 2001 From: Christian Date: Sun, 16 Aug 2026 00:48:53 +0200 Subject: [PATCH 9/9] cms: implement dual-axis panic detection for in-flight menu - Add cmsDetectPanicStickMovement to detect Roll+Pitch >100 PWM sustained for 150ms. - Trigger immediate menu exit on erratic stick input to ensure pilot control override. --- src/main/cms/cms.c | 47 +++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 46 insertions(+), 1 deletion(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index d3b1826bb4b..76d9c0d0eda 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -1405,6 +1405,49 @@ static bool cmsIsNavModeActive(void) FLIGHT_MODE(NAV_ALTHOLD_MODE); } +static bool cmsDetectPanicStickMovement(timeMs_t currentTimeMs) +{ + // Detect panicking pilot by checking for simultaneous multi-axis stick deflection. + // + // Normal CMS menu navigation is strictly single-axis: + // - Pitch only for scrolling items (Roll stays near center, max ~65 PWM crosstalk) + // - Roll only for changing values (Pitch stays near center) + // - Spring bounce after release is single-axis only + // + // A panicking pilot grabs the stick and moves it erratically, which always + // deflects both Roll AND Pitch simultaneously with significant force. + // + // Trigger: both Roll and Pitch deflected >100 PWM from center for 3 consecutive + // samples at 50ms intervals (150ms sustained). This gives zero false positives + // on real navigation data while catching all panic patterns within ~200ms. + + static uint8_t dualAxisCount = 0; + static timeMs_t lastCheckMs = 0; + + // Sample at ~20 Hz + if (currentTimeMs - lastCheckMs < 50) { + return false; + } + lastCheckMs = currentTimeMs; + + const int16_t rollDev = ABS((int16_t)rxGetChannelValue(ROLL) - 1500); + const int16_t pitchDev = ABS((int16_t)rxGetChannelValue(PITCH) - 1500); + + #define PANIC_DUAL_AXIS_THRESHOLD 100 // PWM deviation from center + + if (rollDev > PANIC_DUAL_AXIS_THRESHOLD && pitchDev > PANIC_DUAL_AXIS_THRESHOLD) { + dualAxisCount++; + if (dualAxisCount >= 3) { + dualAxisCount = 0; + return true; + } + } else { + dualAxisCount = 0; + } + + return false; +} + void cmsUpdate(uint32_t currentTimeUs) { #ifdef USE_RCDEVICE @@ -1455,8 +1498,10 @@ void cmsUpdate(uint32_t currentTimeUs) // - Aircraft disarmed // - Failsafe activated (pilot must regain situational awareness) // - NAV mode lost (stick override would leave aircraft without stabilization) + // - Panic / rapid / multi-axis stick movement detected (immediate evasive override) if (cmsOpenedInFlight && (!IS_RC_MODE_ACTIVE(BOXUSER4) || !ARMING_FLAG(ARMED) - || FLIGHT_MODE(FAILSAFE_MODE) || !cmsIsNavModeActive())) { + || FLIGHT_MODE(FAILSAFE_MODE) || !cmsIsNavModeActive() + || cmsDetectPanicStickMovement(currentTimeMs))) { cmsMenuExit(pCurrentDisplay, (void *)CMS_EXIT); return; }