When GPS Rescue is engaged after a Failsafe, pilot has to manually sw… (#7936)

When GPS Rescue is engaged after a Failsafe, pilot has to manually sw…

src/main/cli/settings.c

@@ -747,6 +747,8 @@ const clivalue_t valueTable[] = {
     { "failsafe_switch_mode",       VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FAILSAFE_SWITCH_MODE }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_switch_mode) },
     { "failsafe_throttle_low_delay",VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 300 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_throttle_low_delay) },
     { "failsafe_procedure",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FAILSAFE }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_procedure) },
+    { "failsafe_recovery_delay",    VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_recovery_delay) },
+    { "failsafe_stick_threshold",   VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_stick_threshold) },
 
 // PG_BOARDALIGNMENT_CONFIG
     { "align_board_roll",           VAR_INT16  | MASTER_VALUE, .config.minmax = { -180, 360 }, PG_BOARD_ALIGNMENT, offsetof(boardAlignment_t, rollDegrees) },

src/main/fc/config.c

@@ -319,9 +319,12 @@ static void validateAndFixConfig(void)
         || true
 #endif
         ) {
+
+#ifdef USE_GPS_RESCUE
         if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE) {
             failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT;
         }
+#endif
 
         if (isModeActivationConditionPresent(BOXGPSRESCUE)) {
             removeModeActivationCondition(BOXGPSRESCUE);

src/main/fc/core.c

@@ -579,7 +579,7 @@ static bool canUpdateVTX(void)
 }
 #endif
 
-#ifdef USE_RUNAWAY_TAKEOFF
+#if defined(USE_RUNAWAY_TAKEOFF) || defined(USE_GPS_RESCUE)
 // determine if the R/P/Y stick deflection exceeds the specified limit - integer math is good enough here.
 bool areSticksActive(uint8_t stickPercentLimit)
 {
@@ -601,8 +601,9 @@ bool areSticksActive(uint8_t stickPercentLimit)
     }
     return false;
 }
+#endif
 
-
+#ifdef USE_RUNAWAY_TAKEOFF
 // allow temporarily disabling runaway takeoff prevention if we are connected
 // to the configurator and the ARMING_DISABLED_MSP flag is cleared.
 void runawayTakeoffTemporaryDisable(uint8_t disableFlag)

src/main/fc/core.h

@@ -70,6 +70,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs);
 bool isFlipOverAfterCrashActive(void);
 int8_t calculateThrottlePercent(void);
 uint8_t calculateThrottlePercentAbs(void);
+bool areSticksActive(uint8_t stickPercentLimit);
 void runawayTakeoffTemporaryDisable(uint8_t disableFlag);
 bool isAirmodeActivated();
 timeUs_t getLastDisarmTimeUs(void);

src/main/flight/failsafe.c

@@ -69,13 +69,17 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
     .failsafe_delay = 4,                             // 0,4sec
     .failsafe_off_delay = 10,                        // 1sec
     .failsafe_switch_mode = 0,                       // default failsafe switch action is identical to rc link loss
-    .failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT // default full failsafe procedure is 0: auto-landing
+    .failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT,// default full failsafe procedure is 0: auto-landing
+    .failsafe_recovery_delay = 20,                   // 2 sec of valid rx data (plus 200ms) needed to allow recovering from failsafe procedure
+    .failsafe_stick_threshold = 30                   // 30 percent of stick deflection to exit GPS Rescue procedure
 );
 
 const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = {
     "AUTO-LAND",
     "DROP",
-    "GPS-RESCUE"
+#ifdef USE_GPS_RESCUE
+    "GPS-RESCUE",
+#endif
 };
 
 /*
@@ -84,6 +88,7 @@ const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = {
 void failsafeReset(void)
 {
     failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND;
+    failsafeState.rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY + failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND;
     failsafeState.validRxDataReceivedAt = 0;
     failsafeState.validRxDataFailedAt = 0;
     failsafeState.throttleLowPeriod = 0;
@@ -141,11 +146,12 @@ static void failsafeActivate(void)
 
 static void failsafeApplyControlInput(void)
 {
+#ifdef USE_GPS_RESCUE
     if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE) {
         ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
-
         return;
     }
+#endif
 
     for (int i = 0; i < 3; i++) {
         rcData[i] = rxConfig()->midrc;
@@ -172,7 +178,7 @@ void failsafeOnRxResume(void)
 void failsafeOnValidDataReceived(void)
 {
     failsafeState.validRxDataReceivedAt = millis();
-    if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > PERIOD_RXDATA_RECOVERY) {
+    if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > failsafeState.rxDataRecoveryPeriod) {
         failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;
         unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
     }
@@ -227,7 +233,11 @@ void failsafeUpdateState(void)
                         failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_1_SECONDS;    // require 1 seconds of valid rxData
                         reprocessState = true;
                     } else if (!receivingRxData) {
-                        if (millis() > failsafeState.throttleLowPeriod && failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_GPS_RESCUE) {
+                        if (millis() > failsafeState.throttleLowPeriod 
+#ifdef USE_GPS_RESCUE
+                            && failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_GPS_RESCUE
+#endif
+                            ) {
                             // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds
                             failsafeActivate();
                             failsafeState.phase = FAILSAFE_LANDED;      // skip auto-landing procedure
@@ -263,13 +273,13 @@ void failsafeUpdateState(void)
                             // Drop the craft
                             failsafeActivate();
                             failsafeState.phase = FAILSAFE_LANDED;      // skip auto-landing procedure
-                            failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData
                             break;
+#ifdef USE_GPS_RESCUE
                         case FAILSAFE_PROCEDURE_GPS_RESCUE:
                             failsafeActivate();
                             failsafeState.phase = FAILSAFE_GPS_RESCUE;
-                            failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS;
                             break;
+#endif
                     }
                 }
                 reprocessState = true;
@@ -290,10 +300,13 @@ void failsafeUpdateState(void)
                     reprocessState = true;
                 }
                 break;
+#ifdef USE_GPS_RESCUE
             case FAILSAFE_GPS_RESCUE:
                 if (receivingRxData) {
-                    failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
+                    if (areSticksActive(failsafeConfig()->failsafe_stick_threshold)) {
-                    reprocessState = true;
+                        failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
+                        reprocessState = true;
+                    }
                 }
                 if (armed) {
                     failsafeApplyControlInput();
@@ -304,6 +317,7 @@ void failsafeUpdateState(void)
                     reprocessState = true;
                 }
                 break;
+#endif
             case FAILSAFE_LANDED:
                 setArmingDisabled(ARMING_DISABLED_FAILSAFE); // To prevent accidently rearming by an intermittent rx link
                 disarm();

src/main/flight/failsafe.h

@@ -39,6 +39,8 @@ typedef struct failsafeConfig_s {
     uint8_t failsafe_off_delay;             // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200)
     uint8_t failsafe_switch_mode;           // failsafe switch action is 0: stage1 (identical to rc link loss), 1: disarms instantly, 2: stage2
     uint8_t failsafe_procedure;             // selected full failsafe procedure is 0: auto-landing, 1: Drop it
+    uint16_t failsafe_recovery_delay;       // Time (in 0.1sec) of valid rx data (plus 200ms) needed to allow recovering from failsafe procedure
+    uint8_t failsafe_stick_threshold;       // Stick deflection percentage to exit GPS Rescue procedure
 } failsafeConfig_t;
 
 PG_DECLARE(failsafeConfig_t, failsafeConfig);
@@ -61,7 +63,9 @@ typedef enum {
 typedef enum {
     FAILSAFE_PROCEDURE_AUTO_LANDING = 0,
     FAILSAFE_PROCEDURE_DROP_IT,
+#ifdef USE_GPS_RESCUE
     FAILSAFE_PROCEDURE_GPS_RESCUE,
+#endif
     FAILSAFE_PROCEDURE_COUNT   // must be last
 } failsafeProcedure_e;
 
@@ -78,6 +82,7 @@ typedef struct failsafeState_s {
     bool monitoring;
     bool active;
     uint32_t rxDataFailurePeriod;
+    uint32_t rxDataRecoveryPeriod; 
     uint32_t validRxDataReceivedAt;
     uint32_t validRxDataFailedAt;
     uint32_t throttleLowPeriod;             // throttle stick must have been below 'min_check' for this period

src/test/Makefile

@@ -129,6 +129,9 @@ flight_failsafe_unittest_SRC := \
 		$(USER_DIR)/fc/runtime_config.c \
 		$(USER_DIR)/flight/failsafe.c
 
+flight_failsafe_unittest_DEFINES := \
+		USE_GPS_RESCUE=
+
 
 flight_imu_unittest_SRC := \
 		$(USER_DIR)/common/bitarray.c \

src/test/unit/flight_failsafe_unittest.cc

@@ -589,6 +589,11 @@ bool isUsingSticksForArming(void)
     return isUsingSticksToArm;
 }
 
+bool areSticksActive(uint8_t stickPercentLimit) {
+    UNUSED(stickPercentLimit);
+    return false;
+}
+
 void beeperConfirmationBeeps(uint8_t beepCount) { UNUSED(beepCount); }
 
 bool crashRecoveryModeActive(void) { return false; }