From 49e277642521c98d1b392edef0349991d0bee12a Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 12 May 2026 14:57:35 +0100 Subject: [PATCH] multifunction refactor --- src/main/fc/fc_msp_box.c | 2 - src/main/fc/multifunction.c | 253 +++++++++++++++++++++++++++++++++--- src/main/fc/multifunction.h | 2 + src/main/io/osd.c | 222 ------------------------------- 4 files changed, 239 insertions(+), 240 deletions(-) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index a20a23e0b24..65654ccd97b 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -190,9 +190,7 @@ void initActiveBoxIds(void) RESET_BOX_ID_COUNT; ADD_ACTIVE_BOX(BOXARM); ADD_ACTIVE_BOX(BOXPREARM); -#ifdef USE_MULTI_FUNCTIONS ADD_ACTIVE_BOX(BOXMULTIFUNCTION); -#endif if (sensors(SENSOR_ACC) && STATE(ALTITUDE_CONTROL)) { ADD_ACTIVE_BOX(BOXANGLE); diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c index 93c8ef51fe1..50cf3af1aea 100644 --- a/src/main/fc/multifunction.c +++ b/src/main/fc/multifunction.c @@ -26,16 +26,29 @@ #include "build/debug.h" #include "drivers/time.h" -#ifdef USE_MULTI_FUNCTIONS - +#include "fc/config.h" #include "fc/fc_core.h" #include "fc/multifunction.h" #include "fc/rc_modes.h" -#include "fc/runtime_config.h" + +#include "flight/imu.h" #include "io/osd.h" + #include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +#include "sensors/acceleration.h" +#include "sensors/battery.h" +#include "sensors/compass.h" +#include "sensors/diagnostics.h" +#include "sensors/pitotmeter.h" +#include "sensors/sensors.h" +textAttributes_t osdGetMultiFunctionMessage(char *buff); +multiFunctionWarning_t multiFunctionWarning; + +#ifdef USE_MULTI_FUNCTIONS multi_function_e selectedItem = MULTI_FUNC_NONE; uint8_t multiFunctionFlags; @@ -49,27 +62,19 @@ static void multiFunctionApply(multi_function_e selectedItem) break; case MULTI_FUNC_2: // toggle Safehome suspend #if defined(USE_SAFE_HOME) - if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) { - MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_SAFEHOMES) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_SAFEHOMES); - } + MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_SAFEHOMES) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_SAFEHOMES); #endif break; case MULTI_FUNC_3: // toggle RTH Trackback suspend - if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) { - MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_TRACKBACK) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_TRACKBACK); - } + MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_TRACKBACK) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_TRACKBACK); break; - case MULTI_FUNC_4: + case MULTI_FUNC_4: // toggle Turtle mode #ifdef USE_DSHOT - if (!ARMING_FLAG(ARMED) && STATE(MULTIROTOR)) { // toggle Turtle mode - MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? MULTI_FUNC_FLAG_DISABLE(MF_TURTLE_MODE) : MULTI_FUNC_FLAG_ENABLE(MF_TURTLE_MODE); - } + MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? MULTI_FUNC_FLAG_DISABLE(MF_TURTLE_MODE) : MULTI_FUNC_FLAG_ENABLE(MF_TURTLE_MODE); #endif break; case MULTI_FUNC_5: // emergency ARM - if (!ARMING_FLAG(ARMED)) { - emergencyArmingUpdate(true, true); - } + emergencyArmingUpdate(true, true); break; case MULTI_FUNC_6: // Calibrate compass/Zero yaw heading #if defined(USE_GPS) || defined(USE_MAG) @@ -123,4 +128,220 @@ multi_function_e multiFunctionSelection(void) return selectedItem; } +#endif // multifunction + +static bool osdCheckWarning(bool condition, uint8_t warningFlag) +{ + static timeMs_t newWarningEndTime = 0; + static uint8_t newWarningFlags = 0; // bitfield + const timeMs_t currentTimeMs = millis(); + + /* New warnings dislayed individually for 10s with blinking after which + * all current warnings displayed without blinking on 1 second cycle */ + if (condition) { // condition required to trigger warning + if (!(multiFunctionWarning.osdWarningsFlags & warningFlag)) { // check for new warnings + multiFunctionWarning.osdWarningsFlags |= warningFlag; + newWarningFlags |= warningFlag; + newWarningEndTime = currentTimeMs + 10000; + multiFunctionWarning.newWarningActive = true; + } +#ifdef USE_DEV_TOOLS + if (systemConfig()->groundTestMode) { + return true; + } #endif + if (currentTimeMs < newWarningEndTime) { + return (newWarningFlags & warningFlag); // filter out new warnings excluding older warnings + } else { + newWarningFlags = 0; + multiFunctionWarning.newWarningActive = false; + } + return true; + } else if (multiFunctionWarning.osdWarningsFlags & warningFlag) { + multiFunctionWarning.osdWarningsFlags &= ~warningFlag; + } + + return false; +} + +textAttributes_t osdGetMultiFunctionMessage(char *buff) +{ + /* Message length limit 10 char max */ + + textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; + const char *message = NULL; + +#ifdef USE_MULTI_FUNCTIONS + /* --- FUNCTIONS --- */ + multi_function_e selectedFunction = multiFunctionSelection(); + + if (selectedFunction) { + multi_function_e activeFunction = selectedFunction; + + switch (selectedFunction) { + case MULTI_FUNC_NONE: + case MULTI_FUNC_1: + if (ARMING_FLAG(ARMED)) { + message = posControl.flags.manualEmergLandActive ? "ABORT LAND" : "EMERG LAND"; + break; + } + activeFunction++; + FALLTHROUGH; + case MULTI_FUNC_2: +#if defined(USE_SAFE_HOME) + if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) { + message = MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? "USE SFHOME" : "SUS SFHOME"; + break; + } +#endif + activeFunction++; + FALLTHROUGH; + case MULTI_FUNC_3: + if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) { + message = MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? "USE TKBACK" : "SUS TKBACK"; + break; + } + activeFunction++; + FALLTHROUGH; + case MULTI_FUNC_4: +#ifdef USE_DSHOT + if (!ARMING_FLAG(ARMED) && STATE(MULTIROTOR)) { + message = MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? "END TURTLE" : "USE TURTLE"; + break; + } +#endif + activeFunction++; + FALLTHROUGH; + case MULTI_FUNC_5: + if (!ARMING_FLAG(ARMED)) { + message = "EMERG ARM "; + break; + } + activeFunction++; + FALLTHROUGH; + case MULTI_FUNC_6: + if (!ARMING_FLAG(ARMED)) { +#if defined(USE_MAG) + if (sensors(SENSOR_MAG)) { + message = "CAL COMPAS"; + break; + } +#endif +#if defined(USE_GPS) + if (isYawZeroResetAllowed()) { + message = "SET HEADIN"; + break; + } +#endif + } + activeFunction++; + break; + case MULTI_FUNC_END: + message = "*FUNC SET*"; + break; + } + + if (activeFunction != selectedFunction) { + if (selectedFunction == MULTI_FUNC_1 && activeFunction == MULTI_FUNC_END) { // no functions available so end process + message = "*NO FUNCS*"; + setMultifunctionSelection(MULTI_FUNC_NONE); + } else { + setMultifunctionSelection(activeFunction); + if (activeFunction == MULTI_FUNC_END) { // no messages to display so return + return elemAttr; + } + } + } + + strcpy(buff, message); + + return elemAttr; + } +#endif // MULTIFUNCTION - functions only, warnings always defined + + /* --- WARNINGS --- */ + const char *messages[8]; + uint8_t messageCount = 0; + bool warningCondition = false; + uint8_t warningFlagID = 1; + + // Low Battery Voltage + const batteryState_e batteryVoltageState = checkBatteryVoltageState(); + warningCondition = batteryVoltageState == BATTERY_CRITICAL || batteryVoltageState == BATTERY_WARNING; + if (osdCheckWarning(warningCondition, warningFlagID)) { + messages[messageCount++] = batteryVoltageState == BATTERY_CRITICAL ? "VBATT LAND" : "VBATT LOW "; + } + + // Low Battery Capacity + if (batteryUsesCapacityThresholds()) { + const batteryState_e batteryState = getBatteryState(); + warningCondition = batteryState == BATTERY_CRITICAL || batteryState == BATTERY_WARNING; + if (osdCheckWarning(warningCondition, warningFlagID <<= 1)) { + messages[messageCount++] = batteryState == BATTERY_CRITICAL ? "BATT EMPTY" : "BATT DYING"; + } + } +#if defined(USE_GPS) + // GPS Fix and Failure + if (feature(FEATURE_GPS)) { + if (osdCheckWarning(!STATE(GPS_FIX), warningFlagID <<= 1)) { + bool gpsFailed = getHwGPSStatus() == HW_SENSOR_UNAVAILABLE; + messages[messageCount++] = gpsFailed ? "GPS FAILED" : "NO GPS FIX"; + } + } + + // RTH sanity (warning if RTH heads 200m further away from home than closest point) + warningCondition = NAV_Status.state == MW_NAV_STATE_RTH_ENROUTE && !posControl.flags.rthTrackbackActive && + (posControl.homeDistance - posControl.rthSanityChecker.minimalDistanceToHome) > 20000; + if (osdCheckWarning(warningCondition, warningFlagID <<= 1)) { + messages[messageCount++] = "RTH SANITY"; + } + + // Altitude sanity (warning if significant mismatch between estimated and GPS altitude) + if (osdCheckWarning(posControl.flags.gpsCfEstimatedAltitudeMismatch, warningFlagID <<= 1)) { + messages[messageCount++] = "ALT SANITY"; + } +#endif + +#if defined(USE_MAG) + // Magnetometer failure + if (requestedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { + hardwareSensorStatus_e magStatus = getHwCompassStatus(); + if (osdCheckWarning(magStatus == HW_SENSOR_UNAVAILABLE || magStatus == HW_SENSOR_UNHEALTHY, warningFlagID <<= 1)) { + messages[messageCount++] = "MAG FAILED"; + } + } +#endif + +#if defined(USE_PITOT) + // Pitot sensor validation failure (blocked/failed pitot tube) + if (sensors(SENSOR_PITOT) && detectedSensors[SENSOR_INDEX_PITOT] != PITOT_VIRTUAL) { + if (osdCheckWarning(pitotHasFailed(), warningFlagID <<= 1)) { + messages[messageCount++] = "PITOT FAIL"; + } + } +#endif + + // Vibration levels TODO - needs better vibration measurement to be useful + // const float vibrationLevel = accGetVibrationLevel(); + // warningCondition = vibrationLevel > 1.5f; + // if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) { + // messages[messageCount++] = vibrationLevel > 2.5f ? "BAD VIBRTN" : "VIBRATION!"; + // } + +#ifdef USE_DEV_TOOLS + if (osdCheckWarning(systemConfig()->groundTestMode, warningFlagID <<= 1)) { + messages[messageCount++] = "GRD TEST !"; + } +#endif + + if (messageCount) { + message = messages[(millis() / 1000) % messageCount]; // display each warning on 1s cycle + strcpy(buff, message); + if (multiFunctionWarning.newWarningActive) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } + + return elemAttr; +} + diff --git a/src/main/fc/multifunction.h b/src/main/fc/multifunction.h index 709e229d6c1..cdbdd17ea0f 100644 --- a/src/main/fc/multifunction.h +++ b/src/main/fc/multifunction.h @@ -25,6 +25,7 @@ #pragma once #include +#include "drivers/display.h" typedef struct multiFunctionWarning_s { uint8_t osdWarningsFlags; // bitfield @@ -61,3 +62,4 @@ typedef enum { multi_function_e multiFunctionSelection(void); void setMultifunctionSelection(multi_function_e item); #endif +textAttributes_t osdGetMultiFunctionMessage(char *buff); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ab99cbb5fa5..47eaa6e29ce 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -84,8 +84,6 @@ #include "fc/multifunction.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" -#include "fc/rc_modes.h" -#include "fc/runtime_config.h" #include "fc/settings.h" #include "flight/imu.h" @@ -107,7 +105,6 @@ #include "sensors/boardalignment.h" #include "sensors/compass.h" #include "sensors/diagnostics.h" -#include "sensors/sensors.h" #include "sensors/pitotmeter.h" #include "sensors/temperature.h" #include "sensors/esc_sensor.h" @@ -203,10 +200,6 @@ static bool fullRedraw = false; static uint8_t armState; -// Multifunction -static textAttributes_t osdGetMultiFunctionMessage(char *buff); -multiFunctionWarning_t multiFunctionWarning; - typedef struct osdMapData_s { uint32_t scale; char referenceSymbol; @@ -6387,221 +6380,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter return elemAttr; } -static bool osdCheckWarning(bool condition, uint8_t warningFlag) -{ - static timeMs_t newWarningEndTime = 0; - static uint8_t newWarningFlags = 0; // bitfield - const timeMs_t currentTimeMs = millis(); - - /* New warnings dislayed individually for 10s with blinking after which - * all current warnings displayed without blinking on 1 second cycle */ - if (condition) { // condition required to trigger warning - if (!(multiFunctionWarning.osdWarningsFlags & warningFlag)) { // check for new warnings - multiFunctionWarning.osdWarningsFlags |= warningFlag; - newWarningFlags |= warningFlag; - newWarningEndTime = currentTimeMs + 10000; - multiFunctionWarning.newWarningActive = true; - } -#ifdef USE_DEV_TOOLS - if (systemConfig()->groundTestMode) { - return true; - } -#endif - if (currentTimeMs < newWarningEndTime) { - return (newWarningFlags & warningFlag); // filter out new warnings excluding older warnings - } else { - newWarningFlags = 0; - multiFunctionWarning.newWarningActive = false; - } - return true; - } else if (multiFunctionWarning.osdWarningsFlags & warningFlag) { - multiFunctionWarning.osdWarningsFlags &= ~warningFlag; - } - - return false; -} - -static textAttributes_t osdGetMultiFunctionMessage(char *buff) -{ - /* Message length limit 10 char max */ - - textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; - const char *message = NULL; - -#ifdef USE_MULTI_FUNCTIONS - /* --- FUNCTIONS --- */ - multi_function_e selectedFunction = multiFunctionSelection(); - - if (selectedFunction) { - multi_function_e activeFunction = selectedFunction; - - switch (selectedFunction) { - case MULTI_FUNC_NONE: - case MULTI_FUNC_1: - if (ARMING_FLAG(ARMED)) { - message = posControl.flags.manualEmergLandActive ? "ABORT LAND" : "EMERG LAND"; - break; - } - activeFunction++; - FALLTHROUGH; - case MULTI_FUNC_2: -#if defined(USE_SAFE_HOME) - if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) { - message = MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? "USE SFHOME" : "SUS SFHOME"; - break; - } -#endif - activeFunction++; - FALLTHROUGH; - case MULTI_FUNC_3: - if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) { - message = MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? "USE TKBACK" : "SUS TKBACK"; - break; - } - activeFunction++; - FALLTHROUGH; - case MULTI_FUNC_4: -#ifdef USE_DSHOT - if (!ARMING_FLAG(ARMED) && STATE(MULTIROTOR)) { - message = MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? "END TURTLE" : "USE TURTLE"; - break; - } -#endif - activeFunction++; - FALLTHROUGH; - case MULTI_FUNC_5: - if (!ARMING_FLAG(ARMED)) { - message = "EMERG ARM "; - break; - } - activeFunction++; - FALLTHROUGH; - case MULTI_FUNC_6: - if (!ARMING_FLAG(ARMED)) { -#if defined(USE_MAG) - if (sensors(SENSOR_MAG)) { - message = "CAL COMPAS"; - break; - } -#endif -#if defined(USE_GPS) - if (isYawZeroResetAllowed()) { - message = "SET HEADIN"; - break; - } -#endif - } - activeFunction++; - break; - case MULTI_FUNC_END: - message = "*FUNC SET*"; - break; - } - - if (activeFunction != selectedFunction) { - if (selectedFunction == MULTI_FUNC_1 && activeFunction == MULTI_FUNC_END) { // no functions available so end process - message = "*NO FUNCS*"; - setMultifunctionSelection(MULTI_FUNC_NONE); - } else { - setMultifunctionSelection(activeFunction); - if (activeFunction == MULTI_FUNC_END) { // no messages to display so return - return elemAttr; - } - } - } - - strcpy(buff, message); - - return elemAttr; - } -#endif // MULTIFUNCTION - functions only, warnings always defined - - /* --- WARNINGS --- */ - const char *messages[8]; - uint8_t messageCount = 0; - bool warningCondition = false; - uint8_t warningFlagID = 1; - - // Low Battery Voltage - const batteryState_e batteryVoltageState = checkBatteryVoltageState(); - warningCondition = batteryVoltageState == BATTERY_CRITICAL || batteryVoltageState == BATTERY_WARNING; - if (osdCheckWarning(warningCondition, warningFlagID)) { - messages[messageCount++] = batteryVoltageState == BATTERY_CRITICAL ? "VBATT LAND" : "VBATT LOW "; - } - - // Low Battery Capacity - if (batteryUsesCapacityThresholds()) { - const batteryState_e batteryState = getBatteryState(); - warningCondition = batteryState == BATTERY_CRITICAL || batteryState == BATTERY_WARNING; - if (osdCheckWarning(warningCondition, warningFlagID <<= 1)) { - messages[messageCount++] = batteryState == BATTERY_CRITICAL ? "BATT EMPTY" : "BATT DYING"; - } - } -#if defined(USE_GPS) - // GPS Fix and Failure - if (feature(FEATURE_GPS)) { - if (osdCheckWarning(!STATE(GPS_FIX), warningFlagID <<= 1)) { - bool gpsFailed = getHwGPSStatus() == HW_SENSOR_UNAVAILABLE; - messages[messageCount++] = gpsFailed ? "GPS FAILED" : "NO GPS FIX"; - } - } - - // RTH sanity (warning if RTH heads 200m further away from home than closest point) - warningCondition = NAV_Status.state == MW_NAV_STATE_RTH_ENROUTE && !posControl.flags.rthTrackbackActive && - (posControl.homeDistance - posControl.rthSanityChecker.minimalDistanceToHome) > 20000; - if (osdCheckWarning(warningCondition, warningFlagID <<= 1)) { - messages[messageCount++] = "RTH SANITY"; - } - - // Altitude sanity (warning if significant mismatch between estimated and GPS altitude) - if (osdCheckWarning(posControl.flags.gpsCfEstimatedAltitudeMismatch, warningFlagID <<= 1)) { - messages[messageCount++] = "ALT SANITY"; - } -#endif - -#if defined(USE_MAG) - // Magnetometer failure - if (requestedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { - hardwareSensorStatus_e magStatus = getHwCompassStatus(); - if (osdCheckWarning(magStatus == HW_SENSOR_UNAVAILABLE || magStatus == HW_SENSOR_UNHEALTHY, warningFlagID <<= 1)) { - messages[messageCount++] = "MAG FAILED"; - } - } -#endif - -#if defined(USE_PITOT) - // Pitot sensor validation failure (blocked/failed pitot tube) - if (sensors(SENSOR_PITOT) && detectedSensors[SENSOR_INDEX_PITOT] != PITOT_VIRTUAL) { - if (osdCheckWarning(pitotHasFailed(), warningFlagID <<= 1)) { - messages[messageCount++] = "PITOT FAIL"; - } - } -#endif - - // Vibration levels TODO - needs better vibration measurement to be useful - // const float vibrationLevel = accGetVibrationLevel(); - // warningCondition = vibrationLevel > 1.5f; - // if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) { - // messages[messageCount++] = vibrationLevel > 2.5f ? "BAD VIBRTN" : "VIBRATION!"; - // } - -#ifdef USE_DEV_TOOLS - if (osdCheckWarning(systemConfig()->groundTestMode, warningFlagID <<= 1)) { - messages[messageCount++] = "GRD TEST !"; - } -#endif - - if (messageCount) { - message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; // display each warning on 1s cycle - strcpy(buff, message); - if (multiFunctionWarning.newWarningActive) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - } - - return elemAttr; -} - void osdDrawCustomItem(uint8_t item){ osdDrawSingleElement(item); }