Merge pull request #11552 from breadoven/abo_multifunction_refactor

Multifunction refactor

Author: breadoven

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);

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;
+}
+

src/main/fc/multifunction.h

@@ -25,6 +25,7 @@
 #pragma once
 
 #include <stdbool.h>
+#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);