GPS Rescue Bugfix, add a failsafe debug, refactor stick deflection (#12195)

* simple failsafe debug

* simplify areSticksActive

use getRcDeflectionAbs and fix unitTest

* bugfix to require stick input to re-take control

* small refactor

src/main/build/debug.c

@@ -107,4 +107,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "ATTITUDE",
     "VTX_MSP",
     "GPS_DOP",
+    "FAILSAFE",
 };

src/main/build/debug.h

@@ -105,6 +105,7 @@ typedef enum {
     DEBUG_ATTITUDE,
     DEBUG_VTX_MSP,
     DEBUG_GPS_DOP,
+    DEBUG_FAILSAFE,
     DEBUG_COUNT
 } debugType_e;
 

src/main/fc/core.c

@@ -683,18 +683,7 @@ static bool canUpdateVTX(void)
 bool areSticksActive(uint8_t stickPercentLimit)
 {
     for (int axis = FD_ROLL; axis <= FD_YAW; axis ++) {
-        const uint8_t deadband = axis == FD_YAW ? rcControlsConfig()->yaw_deadband : rcControlsConfig()->deadband;
+        if (getRcDeflectionAbs(axis) * 100.f >= stickPercentLimit) {
-        uint8_t stickPercent = 0;
-        if ((rcData[axis] >= PWM_RANGE_MAX) || (rcData[axis] <= PWM_RANGE_MIN)) {
-            stickPercent = 100;
-        } else {
-            if (rcData[axis] > (rxConfig()->midrc + deadband)) {
-                stickPercent = ((rcData[axis] - rxConfig()->midrc - deadband) * 100) / (PWM_RANGE_MAX - rxConfig()->midrc - deadband);
-            } else if (rcData[axis] < (rxConfig()->midrc - deadband)) {
-                stickPercent = ((rxConfig()->midrc - deadband - rcData[axis]) * 100) / (rxConfig()->midrc - deadband - PWM_RANGE_MIN);
-            }
-        }
-        if (stickPercent >= stickPercentLimit) {
             return true;
         }
     }

src/main/flight/failsafe.c

@@ -104,6 +104,7 @@ void failsafeReset(void)
     failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
     failsafeState.phase = FAILSAFE_IDLE;
     failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
+    failsafeState.failsafeSwitchWasOn = false;
 }
 
 void failsafeInit(void)
@@ -217,17 +218,19 @@ uint32_t failsafeFailurePeriodMs(void)
 }
 
 FAST_CODE_NOINLINE void failsafeUpdateState(void)
-// triggered directly, and ONLY, by the cheduler, at 10ms = PERIOD_RXDATA_FAILURE - intervals
+// triggered directly, and ONLY, by the scheduler, at 10ms = PERIOD_RXDATA_FAILURE - intervals
 {
     if (!failsafeIsMonitoring()) {
         return;
     }
 
     bool receivingRxData = failsafeIsReceivingRxData();
-    // true when FAILSAFE_RXLINK_UP
+    // returns state of FAILSAFE_RXLINK_UP
-    // FAILSAFE_RXLINK_UP is set in failsafeOnValidDataReceived
+    // FAILSAFE_RXLINK_UP is set in failsafeOnValidDataReceived, after the various Stage 1 and recovery delays
     // failsafeOnValidDataReceived runs from detectAndApplySignalLossBehaviour
 
+    DEBUG_SET(DEBUG_FAILSAFE, 2, receivingRxData); // from Rx alone, not considering switch
+
     bool armed = ARMING_FLAG(ARMED);
     bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
     beeperMode_e beeperMode = BEEPER_SILENCE;
@@ -238,7 +241,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
         receivingRxData = false;
     }
 
-    // Beep RX lost only if we are not seeing data and we have been armed earlier
+    // Beep RX lost only if we are not seeing data and are armed or have been armed earlier
     if (!receivingRxData && (armed || ARMING_FLAG(WAS_EVER_ARMED))) {
         beeperMode = BEEPER_RX_LOST;
     }
@@ -250,12 +253,13 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
 
         switch (failsafeState.phase) {
             case FAILSAFE_IDLE:
+                failsafeState.failsafeSwitchWasOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
                 if (armed) {
                     // Track throttle command below minimum time
                     if (calculateThrottleStatus() != THROTTLE_LOW) {
                         failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
                     }
-                    if (failsafeSwitchIsOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL)) {
+                    if (failsafeState.failsafeSwitchWasOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL)) {
                         // Failsafe switch is configured as KILL switch and is switched ON
                         failsafeState.active = true;
                         failsafeState.events++;
@@ -286,8 +290,8 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                         reprocessState = true;
                     }
                 } else {
-                    // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode)
+                    // When NOT armed, enable failsafe mode to show warnings in OSD
-                    if (failsafeSwitchIsOn) {
+                    if (failsafeState.failsafeSwitchWasOn) {
                         ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
                     } else {
                         DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
@@ -308,8 +312,6 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                             //  Enter Stage 2 with settings for landing mode
                             ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
                             failsafeState.phase = FAILSAFE_LANDING;
-                            failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
-                            //  allow re-arming 1 second after Rx recovery
                             failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
                             break;
 
@@ -317,21 +319,20 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                             ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
                             failsafeState.phase = FAILSAFE_LANDED;
                             //  go directly to FAILSAFE_LANDED
-                            failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
-                            //  allow re-arming 1 second after Rx recovery
                             break;
 #ifdef USE_GPS_RESCUE
                         case FAILSAFE_PROCEDURE_GPS_RESCUE:
                             ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
                             failsafeState.phase = FAILSAFE_GPS_RESCUE;
-                            failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
-                            //  allow re-arming 1 second after Rx recovery
                             break;
 #endif
                     }
-                    if (failsafeSwitchIsOn) {
+                    if (failsafeState.failsafeSwitchWasOn) {
                         failsafeState.receivingRxDataPeriodPreset = 0;
-                        // allow immediate recovery if failsafe is triggered by a switch
+                        // recover immediately if failsafe was triggered by a switch
+                    } else {
+                        failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
+                        // recover from true link loss failsafe 1 second after RC Link recovers
                     }
                 }
                 reprocessState = true;
@@ -358,8 +359,10 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
 #ifdef USE_GPS_RESCUE
             case FAILSAFE_GPS_RESCUE:
                 if (receivingRxData) {
-                    if (areSticksActive(failsafeConfig()->failsafe_stick_threshold) || !failsafeSwitchIsOn) {
+                    if (areSticksActive(failsafeConfig()->failsafe_stick_threshold) || failsafeState.failsafeSwitchWasOn) {
-                        //  this test requires stick inputs to be received during GPS Rescue see PR #7936 for rationale
+                        // exits the rescue immediately if failsafe was initiated by switch, otherwise 
+                        // requires stick input to exit the rescue after a true Rx loss failsafe
+                        // NB this test requires stick inputs to be received during GPS Rescue see PR #7936 for rationale
                         failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
                         reprocessState = true;
                     }
@@ -414,6 +417,10 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
             default:
                 break;
         }
+
+    DEBUG_SET(DEBUG_FAILSAFE, 0, failsafeState.failsafeSwitchWasOn);
+    DEBUG_SET(DEBUG_FAILSAFE, 3, failsafeState.phase);
+
     } while (reprocessState);
 
     if (beeperMode != BEEPER_SILENCE) {

src/main/flight/failsafe.h

@@ -90,6 +90,7 @@ typedef struct failsafeState_s {
     uint32_t receivingRxDataPeriodPreset;   // preset for the required period of valid rxData
     failsafePhase_e phase;
     failsafeRxLinkState_e rxLinkState;
+    bool failsafeSwitchWasOn;
 } failsafeState_t;
 
 void failsafeInit(void);

src/main/rx/rx.c

@@ -564,6 +564,7 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current
         }
     }
 
+    DEBUG_SET(DEBUG_FAILSAFE, 1, rxSignalReceived);
     DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 0, rxSignalReceived);
 }
 

src/test/unit/arming_prevention_unittest.cc

@@ -1150,5 +1150,5 @@ extern "C" {
         UNUSED(pitchLpf);
         return 0.0f;
     }
-
+    void getRcDeflectionAbs(void) {}
 }