Merge pull request #11497 from ctzsnooze/repair-commit-for-PR-11459

fix for failsafe to apply throttle, aux and recovery delay settings

src/main/flight/failsafe.c

@@ -125,6 +125,13 @@ bool failsafeIsActive(void)
     return failsafeState.active;
 }
 
+#ifdef USE_GPS_RESCUE
+bool failsafePhaseIsGpsRescue(void)
+{
+    return failsafeState.phase == FAILSAFE_GPS_RESCUE;
+}
+#endif
+
 void failsafeStartMonitoring(void)
 {
     failsafeState.monitoring = true;
@@ -255,10 +262,13 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                             && failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_GPS_RESCUE
 #endif
                             ) {
-                            // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds
+                            // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds before failsafe
+                            // protects against false arming when the Tx is powered up after the quad
                             failsafeActivate();
-                            failsafeState.phase = FAILSAFE_LANDED;      // skip auto-landing procedure
+                            // skip auto-landing procedure
-                            failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData
+                            failsafeState.phase = FAILSAFE_LANDED;
+                            // re-arm at rxDataRecoveryPeriod - default is 1.0 seconds
+                            failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
                         } else {
                             failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
                         }
@@ -289,6 +299,8 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                         case FAILSAFE_PROCEDURE_DROP_IT:
                             // Drop the craft
                             failsafeActivate();
+                            // re-arm at rxDataRecoveryPeriod - default is 1.0 seconds
+                            failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
                             // skip auto-landing procedure
                             failsafeState.phase = FAILSAFE_LANDED;
                             break;
@@ -323,6 +335,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
             case FAILSAFE_GPS_RESCUE:
                 if (receivingRxData) {
                     if (areSticksActive(failsafeConfig()->failsafe_stick_threshold)) {
+                        //  hence we must allow stick inputs during FAILSAFE_GPS_RESCUE see PR #7936 for rationale
                         failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
                         reprocessState = true;
                     }

src/main/flight/failsafe.h

@@ -107,3 +107,6 @@ void failsafeOnRxSuspend(uint32_t suspendPeriod);
 void failsafeOnRxResume(void);
 void failsafeOnValidDataReceived(void);
 void failsafeOnValidDataFailed(void);
+#ifdef USE_GPS_RESCUE
+bool failsafePhaseIsGpsRescue(void);
+#endif

src/main/rx/rx.c

@@ -103,7 +103,6 @@ static bool auxiliaryProcessingRequired = false;
 
 static bool rxSignalReceived = false;
 static bool rxFlightChannelsValid = false;
-static bool rxIsInFailsafeMode = true;
 static uint8_t rxChannelCount;
 
 static timeUs_t needRxSignalBefore = 0;
@@ -477,13 +476,13 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current
 {
     bool signalReceived = false;
     bool useDataDrivenProcessing = true;
-    // Need the next packet in 2.5 x the anticipated time
+    //  need the next packet in 2.5 x the anticipated time
     timeDelta_t needRxSignalMaxDelayUs = anticipatedDeltaTime10thUs * 2 / 8;
 
     DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 2, needRxSignalMaxDelayUs  / 10);
 
     if (taskUpdateRxMainInProgress()) {
-        // No need to check for new data as a packet is being processed already
+        //  no need to check for new data as a packet is being processed already
         return;
     }
 
@@ -495,7 +494,6 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current
     case RX_PROVIDER_PPM:
         if (isPPMDataBeingReceived()) {
             signalReceived = true;
-            rxIsInFailsafeMode = false;
             resetPPMDataReceivedState();
         }
 
@@ -503,7 +501,6 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current
     case RX_PROVIDER_PARALLEL_PWM:
         if (isPWMDataBeingReceived()) {
             signalReceived = true;
-            rxIsInFailsafeMode = false;
             useDataDrivenProcessing = false;
         }
 
@@ -514,36 +511,34 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current
     case RX_PROVIDER_SPI:
         {
             const uint8_t frameStatus = rxRuntimeState.rcFrameStatusFn(&rxRuntimeState);
-            if (frameStatus & RX_FRAME_COMPLETE) {
+            DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 1, (frameStatus & RX_FRAME_FAILSAFE));
-                rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0;
+            signalReceived = (frameStatus & RX_FRAME_COMPLETE) && !((frameStatus & RX_FRAME_FAILSAFE) || (frameStatus & RX_FRAME_DROPPED));
-                bool rxFrameDropped = (frameStatus & RX_FRAME_DROPPED) != 0;
+            setLinkQuality(signalReceived, currentDeltaTimeUs);
-                signalReceived = !(rxIsInFailsafeMode || rxFrameDropped);
+            auxiliaryProcessingRequired |= (frameStatus & RX_FRAME_PROCESSING_REQUIRED);
-                setLinkQuality(signalReceived, currentDeltaTimeUs);
-            }
-            if (frameStatus & RX_FRAME_PROCESSING_REQUIRED) {
-                auxiliaryProcessingRequired = true;
-            }
         }
 
         break;
     }
 
     if (signalReceived) {
+        //  true only when a new packet arrives
         needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
         rxSignalReceived = true; // immediately process data
     } else {
-        // no signal, wait 100ms and perform signal loss behaviour every 100ms
+        //  watch for next packet
-        if (cmpTimeUs(needRxSignalBefore, currentTimeUs) > (timeDelta_t)DELAY_10_HZ) {
+        if (cmpTimeUs(currentTimeUs, needRxSignalBefore) > 0) {
+            //  initial time to signalReceived failure is 2.5 packets then every 100ms
             rxSignalReceived = false;
-            // rcData[] needs to be processed to rcCommand
-            rxDataProcessingRequired = true;
             needRxSignalBefore = currentTimeUs + (timeDelta_t)DELAY_10_HZ;
+            //  review rcData values every 100ms in failsafe changed them
+            rxDataProcessingRequired = true;
         }
     }
 
-    if (signalReceived && useDataDrivenProcessing) {
+    DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 0, rxSignalReceived);
-        rxDataProcessingRequired = true;
+    //  process the new Rx packet when it arrives
-    }
+    rxDataProcessingRequired |= (signalReceived && useDataDrivenProcessing);
+
 }
 
 #if defined(USE_PWM) || defined(USE_PPM)
@@ -643,51 +638,57 @@ static void readRxChannelsApplyRanges(void)
     }
 }
 
-static void detectAndApplySignalLossBehaviour(void)
+void detectAndApplySignalLossBehaviour(void)
 {
     const uint32_t currentTimeMs = millis();
     const bool failsafeAuxSwitch = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
-    bool validRxPacket = rxSignalReceived && !failsafeAuxSwitch;
+#ifdef USE_GPS_RESCUE
-    // becomes false when a packet is bad or we use a failsafe switch - logged to blackbox
+    const bool gpsRescue = failsafePhaseIsGpsRescue();
-    rxFlightChannelsValid = true;
+#endif
-    // becomes false when one or more flight channels has bad Rx data for longer than hold time
+    rxFlightChannelsValid = rxSignalReceived && !failsafeAuxSwitch;
-
+    //  set rxFlightChannelsValid false when a packet is bad or we use a failsafe switch
-    DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 0, rxSignalReceived);
-    DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 1, rxIsInFailsafeMode);
 
     for (int channel = 0; channel < rxChannelCount; channel++) {
         float sample = rcRaw[channel];
-        const bool thisChannelValid = validRxPacket && isPulseValid(sample);  // for all or just one channel alone
+        const bool thisChannelValid = rxFlightChannelsValid && isPulseValid(sample);
+          //  if the packet is bad, we don't allow any channels to be good
 
         if (thisChannelValid) {
-            // reset invalid pulse period timer for each good-channel
+            //  reset the invalid pulse period timer for every good channel
             validRxSignalTimeout[channel] = currentTimeMs + MAX_INVALID_PULSE_TIME;
         }
-        // set failsafe and hold values
+
-        if (failsafeIsActive() && ARMING_FLAG(ARMED)) {
+       if (ARMING_FLAG(ARMED) && failsafeIsActive()) {
-            // STAGE 2 failsafe is active, and armed.  Apply failsafe values until failsafe ends or disarmed
+            //  apply failsafe values, until failsafe ends, or disarmed, unless in GPS Return
             if (channel < NON_AUX_CHANNEL_COUNT) {
+#ifdef USE_GPS_RESCUE
+                if (gpsRescue) {
+                    continue;
+                } 
+#endif 
                 if (channel == THROTTLE ) {
                     sample = failsafeConfig()->failsafe_throttle;
                 } else {
                     sample = rxConfig()->midrc;
                 }
             } else if (!failsafeAuxSwitch) {
-                //  Aux channels as Set in Configurator, unless failsafe initiated by switch
+                //  aux channels as Set in Configurator, unless failsafe initiated by switch
                 sample = getRxfailValue(channel);
             }
         } else {
+            //  in STAGE 1 failsafe or HOLD period.
             if (!thisChannelValid) {
                 if (cmp32(currentTimeMs, validRxSignalTimeout[channel]) < 0) {
-                    //  HOLD PERIOD for any invalid channels
+                    //  HOLD PERIOD is MAX_INVALID_PULSE_TIME or 300ms for invalid channels/packets
                 } else {
-                    //  STAGE 1 failsafe settings apply to any channels invalid for more than hold time
+                    //  in STAGE 1 failsafe now that hold time has expired
                     if (channel < NON_AUX_CHANNEL_COUNT) {
                         rxFlightChannelsValid = false;
+                        //  some flight channels were bad, so flag them
                         sample = getRxfailValue(channel);
-                        //  affected RPYT values will be set as per Stage 1 Configurator settings
+                        //  affected RPYT values will get Stage 1 values
                     } else if (!failsafeAuxSwitch) {
-                        //  Aux channels as Set in Configurator, unless failsafe initiated by switch
+                        //  for switch initiated failsafe, only the flight channels get Stage 1 values
                         sample = getRxfailValue(channel);
                     }
                 }
@@ -700,26 +701,22 @@ static void detectAndApplySignalLossBehaviour(void)
             rcData[channel] = calculateChannelMovingAverage(channel, sample);
         } else
 #endif
+
         {
-            // set rcData to either clean raw incoming values, or failsafe values
+            //  set rcData to either clean raw incoming values, or failsafe-modified values
             rcData[channel] = sample;
         }
     }
 
-    if (!rxFlightChannelsValid) {
+    if (rxFlightChannelsValid) {
-        // show RXLOSS in OSD if any RPYT channel, or any packets, are lost for more than hold time
+        //  clear RXLOSS in OSD
-        setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
-    }
-
-    if (validRxPacket && rxFlightChannelsValid) {
-        //  failsafe switches are off, packet is good, no invalid channel for more than hold time
-        //  --> start the timer to exit stage 2 failsafe
         unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
+        //  --> start the timer to exit stage 2 failsafe
         failsafeOnValidDataReceived();
     } else {
-        //  failsafe switch is on, or a bad packet, or at least one invalid channel for more than hold time
+        //  show RXLOSS in OSD (may be a little aggressive ? if so move up to Stage 1)
+        setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
         //  -> start timer to enter stage2 failsafe
-        //  note stage 2 onset will be delayed by hold time if we only have some bad channels in otherwise good packets 
         failsafeOnValidDataFailed();
     }
 
@@ -747,8 +744,8 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
         return true;
     }
 
-    readRxChannelsApplyRanges();
+    readRxChannelsApplyRanges();            // returns rcRaw
-    detectAndApplySignalLossBehaviour();
+    detectAndApplySignalLossBehaviour();    // returns rcData
 
     rcSampleIndex++;
 
@@ -973,4 +970,4 @@ timeDelta_t rxGetFrameDelta(timeDelta_t *frameAgeUs)
 timeUs_t rxFrameTimeUs(void)
 {
     return rxRuntimeState.lastRcFrameTimeUs;
-}
+}

src/test/unit/flight_failsafe_unittest.cc

@@ -290,7 +290,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
 
     // given
     failsafeOnValidDataFailed();                    // set last invalid sample at current time
-    sysTickUptime += PERIOD_RXDATA_RECOVERY + 1;    // adjust time to point just past the recovery time to
+    sysTickUptime += PERIOD_RXDATA_RECOVERY;    // adjust time to point just past the recovery time to
     failsafeOnValidDataReceived();                  // cause a recovered link
 
     // when
@@ -303,7 +303,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
     EXPECT_TRUE(isArmingDisabled());
 
     // given
-    sysTickUptime += PERIOD_OF_3_SECONDS + 1;       // adjust time to point just past the required additional recovery time
+    sysTickUptime += PERIOD_OF_1_SECONDS + 1;       // adjust time to point just past the required additional recovery time
     failsafeOnValidDataReceived();
 
     // when
@@ -401,7 +401,7 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Drop)
     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 
     // given
-    sysTickUptime += PERIOD_OF_3_SECONDS + 1;       // adjust time to point just past the required additional recovery time
+    sysTickUptime += PERIOD_OF_1_SECONDS + 1;       // adjust time to point just past the required additional recovery time
     deactivateBoxFailsafe();
     failsafeOnValidDataReceived();                  // inactive box failsafe gives valid data