diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index a32ab0af886..76d9c0d0eda 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); @@ -891,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; } @@ -899,19 +958,26 @@ 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)) { - 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); + 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); } - DISABLE_ARMING_FLAG(ARMING_DISABLED_CMS_MENU); + cmsOpenedInFlight = false; return 0; } @@ -1262,9 +1328,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 +1342,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 +1397,57 @@ 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_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 @@ -1345,13 +1463,54 @@ 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) + // - 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() + || cmsDetectPanicStickMovement(currentTimeMs))) { + 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 +1528,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/cms/cms_menu_battery.c b/src/main/cms/cms_menu_battery.c index c6abcdace1b..9a5a45e1359 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)) { + batteryUpdateThresholdsAndCells(); + } 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)) { + batteryUpdateThresholdsAndCells(); + } + + 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 }; @@ -147,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 e9849df8619..e308eda6a27 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_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_menuBatteryInFlight), + OSD_SUBMENU_ENTRY("FC+FW INFO", &menuInfo), + OSD_SUBMENU_ENTRY("MISC", &cmsx_menuMiscInFlight), + + {"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; diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 2747697d222..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; } @@ -161,6 +164,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 +205,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 +215,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 +244,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 +283,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 +293,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 +320,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 +355,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 +365,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, }; @@ -517,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; 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; } 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). { 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" 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);