Fix failsafe timings and behaviour to match Failsafe.md

src/main/cli/cli.c

@@ -6162,8 +6162,8 @@ static void cliDshotTelemetryInfo(const char *cmdName, char *cmdline)
     if (useDshotTelemetry) {
         cliPrintLinef("Dshot reads: %u", dshotTelemetryState.readCount);
         cliPrintLinef("Dshot invalid pkts: %u", dshotTelemetryState.invalidPacketCount);
-        uint32_t directionChangeCycles = dshotDMAHandlerCycleCounters.changeDirectionCompletedAt - dshotDMAHandlerCycleCounters.irqAt;
+        int32_t directionChangeCycles = cmp32(dshotDMAHandlerCycleCounters.changeDirectionCompletedAt, dshotDMAHandlerCycleCounters.irqAt);
-        uint32_t directionChangeDurationUs = clockCyclesToMicros(directionChangeCycles);
+        int32_t directionChangeDurationUs = clockCyclesToMicros(directionChangeCycles);
         cliPrintLinef("Dshot directionChange cycles: %u, micros: %u", directionChangeCycles, directionChangeDurationUs);
         cliPrintLinefeed();
 

src/main/drivers/system.c

@@ -153,7 +153,7 @@ uint32_t getCycleCounter(void)
     return DWT->CYCCNT;
 }
 
-uint32_t clockCyclesToMicros(uint32_t clockCycles)
+int32_t clockCyclesToMicros(int32_t clockCycles)
 {
     return clockCycles / usTicks;
 }

src/main/drivers/system.h

@@ -65,7 +65,7 @@ void systemReset(void);
 void systemResetToBootloader(bootloaderRequestType_e requestType);
 bool isMPUSoftReset(void);
 void cycleCounterInit(void);
-uint32_t clockCyclesToMicros(uint32_t clockCycles);
+int32_t clockCyclesToMicros(int32_t clockCycles);
 int32_t clockCyclesTo10thMicros(int32_t clockCycles);
 uint32_t clockMicrosToCycles(uint32_t micros);
 uint32_t getCycleCounter(void);

src/main/fc/core.c

@@ -744,12 +744,6 @@ bool isAirmodeActivated()
  */
 bool processRx(timeUs_t currentTimeUs)
 {
-    timeDelta_t frameAgeUs;
-    timeDelta_t frameDeltaUs = rxGetFrameDelta(&frameAgeUs);
-
-    DEBUG_SET(DEBUG_RX_TIMING, 0, MIN(frameDeltaUs / 10, INT16_MAX));
-    DEBUG_SET(DEBUG_RX_TIMING, 1, MIN(frameAgeUs / 10, INT16_MAX));
-
     if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) {
         return false;
     }

src/main/fc/rc.c

@@ -298,13 +298,18 @@ void updateRcRefreshRate(timeUs_t currentTimeUs)
     static timeUs_t lastRxTimeUs;
 
     timeDelta_t frameAgeUs;
-    timeDelta_t refreshRateUs = rxGetFrameDelta(&frameAgeUs);
+    timeDelta_t frameDeltaUs = rxGetFrameDelta(&frameAgeUs);
-    if (!refreshRateUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) {
+
-        refreshRateUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol
+    if (!frameDeltaUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) {
+        frameDeltaUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol
     }
+
+    DEBUG_SET(DEBUG_RX_TIMING, 0, MIN(frameDeltaUs / 10, INT16_MAX));
+    DEBUG_SET(DEBUG_RX_TIMING, 1, MIN(frameAgeUs / 10, INT16_MAX));
+
     lastRxTimeUs = currentTimeUs;
-    isRxRateValid = (refreshRateUs >= RC_RX_RATE_MIN_US && refreshRateUs <= RC_RX_RATE_MAX_US);
+    isRxRateValid = (frameDeltaUs >= RC_RX_RATE_MIN_US && frameDeltaUs <= RC_RX_RATE_MAX_US);
-    currentRxRefreshRate = constrain(refreshRateUs, RC_RX_RATE_MIN_US, RC_RX_RATE_MAX_US);
+    currentRxRefreshRate = constrain(frameDeltaUs, RC_RX_RATE_MIN_US, RC_RX_RATE_MAX_US);
 }
 
 uint16_t getCurrentRxRefreshRate(void)

src/main/flight/failsafe.c

@@ -65,11 +65,11 @@ PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CO
 PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
     .failsafe_throttle = 1000,                       // default throttle off.
     .failsafe_throttle_low_delay = 100,              // default throttle low delay for "just disarm" on failsafe condition
-    .failsafe_delay = 4,                             // 0,4sec
+    .failsafe_delay = 10,                                // 1 sec, can regain control on signal recovery, at idle in drop mode
-    .failsafe_off_delay = 10,                        // 1sec
+    .failsafe_off_delay = 10,                            // 1 sec in landing phase, if enabled
-    .failsafe_switch_mode = 0,                       // default failsafe switch action is identical to rc link loss
+    .failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE1, // 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_recovery_delay = 20,                   // 2 sec of valid rx data (plus 200ms) needed to allow recovering from failsafe procedure
+    .failsafe_recovery_delay = 10,                       // 1 sec of valid rx data needed to allow recovering from failsafe procedure
     .failsafe_stick_threshold = 30                   // 30 percent of stick deflection to exit GPS Rescue procedure
 );
 
@@ -86,8 +86,12 @@ const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = {
  */
 void failsafeReset(void)
 {
-    failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND;
+    failsafeState.rxDataFailurePeriod = failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND; // time to start stage2
-    failsafeState.rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY + failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND;
+    failsafeState.rxDataRecoveryPeriod = failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND;
+    if (failsafeState.rxDataRecoveryPeriod < PERIOD_RXDATA_RECOVERY){
+        // PERIOD_RXDATA_RECOVERY (200ms) is the minimum allowed RxData recovery time
+        failsafeState.rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY;
+    }
     failsafeState.validRxDataReceivedAt = 0;
     failsafeState.validRxDataFailedAt = 0;
     failsafeState.throttleLowPeriod = 0;
@@ -138,26 +142,12 @@ static void failsafeActivate(void)
     failsafeState.phase = FAILSAFE_LANDING;
 
     ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
+
     failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
 
     failsafeState.events++;
 }
 
-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;
-    }
-    rcData[THROTTLE] = failsafeConfig()->failsafe_throttle;
-}
-
 bool failsafeIsReceivingRxData(void)
 {
     return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP);
@@ -177,42 +167,61 @@ void failsafeOnRxResume(void)
 void failsafeOnValidDataReceived(void)
 {
     failsafeState.validRxDataReceivedAt = millis();
-    if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > failsafeState.rxDataRecoveryPeriod) {
+    if ((cmp32(failsafeState.validRxDataReceivedAt, failsafeState.validRxDataFailedAt) > (int32_t)failsafeState.rxDataRecoveryPeriod) ||
+        (cmp32(failsafeState.validRxDataFailedAt, failsafeState.validRxDataReceivedAt) > 0)) {
+        // rxDataRecoveryPeriod defaults to 1.0s with minimum of PERIOD_RXDATA_RECOVERY (200ms)
         failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;
+        // Allow arming now we have an RX link
         unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
     }
 }
 
 void failsafeOnValidDataFailed(void)
 {
-    setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); // To prevent arming with no RX link
     failsafeState.validRxDataFailedAt = millis();
-    if ((failsafeState.validRxDataFailedAt - failsafeState.validRxDataReceivedAt) > failsafeState.rxDataFailurePeriod) {
+    if ((cmp32(failsafeState.validRxDataFailedAt, failsafeState.validRxDataReceivedAt) > (int32_t)failsafeState.rxDataFailurePeriod)) {
+        // rxDataFailurePeriod is stage 1 guard time
         failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
+        // Prevent arming with no RX link
+        setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
     }
 }
 
 void failsafeCheckDataFailurePeriod(void)
+// forces link down directly from scheduler?
 {
     if (cmp32(millis(), failsafeState.validRxDataReceivedAt) > (int32_t)failsafeState.rxDataFailurePeriod) {
-        setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); // To prevent arming with no RX link
         failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
+        // Prevent arming with no RX link
+        setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
     }
 }
 
+uint32_t failsafeFailurePeriodMs(void)
+{
+    return failsafeState.rxDataFailurePeriod;
+}
+
 FAST_CODE_NOINLINE void failsafeUpdateState(void)
+// triggered directly, and ONLY, by the cheduler, at 10ms = PERIOD_RXDATA_FAILURE - intervals
 {
     if (!failsafeIsMonitoring()) {
         return;
     }
 
     bool receivingRxData = failsafeIsReceivingRxData();
+    // should be true when FAILSAFE_RXLINK_UP
+    // FAILSAFE_RXLINK_UP is set in failsafeOnValidDataReceived
+    // failsafeOnValidDataReceived runs from detectAndApplySignalLossBehaviour
+
     bool armed = ARMING_FLAG(ARMED);
     bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
     beeperMode_e beeperMode = BEEPER_SILENCE;
 
-    if (failsafeSwitchIsOn && failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2) {
+    if (failsafeSwitchIsOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2)) {
-        receivingRxData = false; // force Stage2
+        // Aux switch set to failsafe stage2 emulates loss of signal without waiting
+        failsafeOnValidDataFailed();
+        receivingRxData = false;
     }
 
     // Beep RX lost only if we are not seeing data and we have been armed earlier
@@ -232,12 +241,13 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                     if (THROTTLE_HIGH == calculateThrottleStatus()) {
                         failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
                     }
-                    // Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming)
+                    if (failsafeSwitchIsOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL)) {
-                    if (failsafeSwitchIsOn && failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL) {
+                        // Failsafe switch is configured as KILL switch and is switched ON
-                        // KillswitchEvent: failsafe switch is configured as KILL switch and is switched ON
                         failsafeActivate();
-                        failsafeState.phase = FAILSAFE_LANDED;      // skip auto-landing procedure
+                        // Skip auto-landing procedure
-                        failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_1_SECONDS;    // require 1 seconds of valid rxData
+                        failsafeState.phase = FAILSAFE_LANDED;
+                        // Require 3 seconds of valid rxData
+                        failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS;
                         reprocessState = true;
                     } else if (!receivingRxData) {
                         if (millis() > failsafeState.throttleLowPeriod
@@ -279,7 +289,8 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                         case FAILSAFE_PROCEDURE_DROP_IT:
                             // Drop the craft
                             failsafeActivate();
-                            failsafeState.phase = FAILSAFE_LANDED;      // skip auto-landing procedure
+                            // skip auto-landing procedure
+                            failsafeState.phase = FAILSAFE_LANDED;
                             break;
 #ifdef USE_GPS_RESCUE
                         case FAILSAFE_PROCEDURE_GPS_RESCUE:
@@ -296,15 +307,16 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                 if (receivingRxData) {
                     failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
                     reprocessState = true;
-                }
+                } else {
-                if (armed) {
+                    if (armed) {
-                    failsafeApplyControlInput();
+                        beeperMode = BEEPER_RX_LOST_LANDING;
-                    beeperMode = BEEPER_RX_LOST_LANDING;
+                    }
-                }
+                    if (failsafeShouldHaveCausedLandingByNow() || crashRecoveryModeActive() || !armed) {
-                if (failsafeShouldHaveCausedLandingByNow() || crashRecoveryModeActive() || !armed) {
+                        // require 3 seconds of valid rxData
-                    failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData
+                        failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS;
-                    failsafeState.phase = FAILSAFE_LANDED;
+                        failsafeState.phase = FAILSAFE_LANDED;
-                    reprocessState = true;
+                        reprocessState = true;
+                    }
                 }
                 break;
 #ifdef USE_GPS_RESCUE
@@ -314,19 +326,22 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                         failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
                         reprocessState = true;
                     }
-                }
-                if (armed) {
-                    failsafeApplyControlInput();
-                    beeperMode = BEEPER_RX_LOST_LANDING;
                 } else {
-                    failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData
+                    if (armed) {
-                    failsafeState.phase = FAILSAFE_LANDED;
+                        ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
-                    reprocessState = true;
+                        beeperMode = BEEPER_RX_LOST_LANDING;
+                    } else {
+                        // require 3 seconds of valid rxData
+                        failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS;
+                        failsafeState.phase = FAILSAFE_LANDED;
+                        reprocessState = true;
+                    }
                 }
                 break;
 #endif
             case FAILSAFE_LANDED:
-                setArmingDisabled(ARMING_DISABLED_FAILSAFE); // To prevent accidently rearming by an intermittent rx link
+                // Prevent accidently rearming by an intermittent rx link
+                setArmingDisabled(ARMING_DISABLED_FAILSAFE);
                 disarm(DISARM_REASON_FAILSAFE);
                 failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData
                 failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING;
@@ -337,12 +352,9 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                 // Monitoring the rx link to allow rearming when it has become good for > `receivingRxDataPeriodPreset` time.
                 if (receivingRxData) {
                     if (millis() > failsafeState.receivingRxDataPeriod) {
-                        // rx link is good now, when arming via ARM switch, it must be OFF first
+                        // rx link is good now
-                        if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) {
+                        failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
-                            unsetArmingDisabled(ARMING_DISABLED_FAILSAFE);
+                        reprocessState = true;
-                            failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
-                            reprocessState = true;
-                        }
                     }
                 } else {
                     failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset;
@@ -357,6 +369,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                 failsafeState.phase = FAILSAFE_IDLE;
                 failsafeState.active = false;
                 DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
+                unsetArmingDisabled(ARMING_DISABLED_FAILSAFE);
                 reprocessState = true;
                 break;
 

src/main/flight/failsafe.h

@@ -28,18 +28,17 @@
 #define PERIOD_OF_1_SECONDS            1 * MILLIS_PER_SECOND
 #define PERIOD_OF_3_SECONDS            3 * MILLIS_PER_SECOND
 #define PERIOD_OF_30_SECONDS          30 * MILLIS_PER_SECOND
-#define PERIOD_RXDATA_FAILURE        200    // millis
+#define PERIOD_RXDATA_FAILURE        10     // millis
 #define PERIOD_RXDATA_RECOVERY       200    // millis
 
-
 typedef struct failsafeConfig_s {
     uint16_t failsafe_throttle;             // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
     uint16_t failsafe_throttle_low_delay;   // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure".
     uint8_t failsafe_delay;                 // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
     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_switch_mode;           // failsafe switch action is 0: Stage 1, 1: Disarms instantly, 2: Stage 2
     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
+    uint16_t failsafe_recovery_delay;       // Time (in 0.1sec) of valid rx data (min 200ms PERIOD_RXDATA_RECOVERY) to allow recovering from failsafe procedure
     uint8_t failsafe_stick_threshold;       // Stick deflection percentage to exit GPS Rescue procedure
 } failsafeConfig_t;
 
@@ -106,6 +105,5 @@ bool failsafeIsReceivingRxData(void);
 void failsafeCheckDataFailurePeriod(void);
 void failsafeOnRxSuspend(uint32_t suspendPeriod);
 void failsafeOnRxResume(void);
-
 void failsafeOnValidDataReceived(void);
 void failsafeOnValidDataFailed(void);

src/main/osd/osd.c

@@ -67,6 +67,7 @@
 #if defined(USE_DYN_NOTCH_FILTER)
 #include "flight/dyn_notch_filter.h"
 #endif
+#include "flight/failsafe.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/position.h"
@@ -1037,6 +1038,7 @@ STATIC_UNIT_TESTED bool osdProcessStats1(timeUs_t currentTimeUs)
             resumeRefreshAt = osdShowArmed() + currentTimeUs;
         } else if (isSomeStatEnabled()
                    && !suppressStatsDisplay
+                   && !failsafeIsActive()
                    && (!(getArmingDisableFlags() & (ARMING_DISABLED_RUNAWAY_TAKEOFF | ARMING_DISABLED_CRASH_DETECTED))
                        || !VISIBLE(osdElementConfig()->item_pos[OSD_WARNINGS]))) { // suppress stats if runaway takeoff triggered disarm and WARNINGS element is visible
             osdStatsEnabled = true;

src/main/rx/rx.c

@@ -43,6 +43,7 @@
 
 #include "fc/rc_controls.h"
 #include "fc/rc_modes.h"
+#include "fc/runtime_config.h"
 #include "fc/tasks.h"
 
 #include "flight/failsafe.h"
@@ -105,17 +106,15 @@ static bool rxFlightChannelsValid = false;
 static bool rxIsInFailsafeMode = true;
 static uint8_t rxChannelCount;
 
-static timeUs_t rxNextUpdateAtUs = 0;
+static timeUs_t needRxSignalBefore = 0;
-static uint32_t needRxSignalBefore = 0;
+static timeUs_t suspendRxSignalUntil = 0;
-static uint32_t needRxSignalMaxDelayUs;
-static uint32_t suspendRxSignalUntil = 0;
 static uint8_t  skipRxSamples = 0;
 
 static float rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
 float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];           // interval [1000;2000]
-uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT];
+uint32_t validRxSignalTimeout[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 
-#define MAX_INVALID_PULS_TIME    300
+#define MAX_INVALID_PULSE_TIME 300                  // hold time in millisecons after bad channel or Rx link loss
 #define PPM_AND_PWM_SAMPLE_COUNT 3
 
 #define DELAY_50_HZ (1000000 / 50)
@@ -284,11 +283,10 @@ void rxInit(void)
     rxRuntimeState.rcProcessFrameFn = nullProcessFrame;
     rxRuntimeState.lastRcFrameTimeUs = 0;
     rcSampleIndex = 0;
-    needRxSignalMaxDelayUs = DELAY_10_HZ;
 
     for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
         rcData[i] = rxConfig()->midrc;
-        rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME;
+        validRxSignalTimeout[i] = millis() + MAX_INVALID_PULSE_TIME;
     }
 
     rcData[THROTTLE] = (featureIsEnabled(FEATURE_3D)) ? rxConfig()->midrc : rxConfig()->rx_min_usec;
@@ -330,7 +328,6 @@ void rxInit(void)
 #ifdef USE_RX_MSP
     case RX_PROVIDER_MSP:
         rxMspInit(rxConfig(), &rxRuntimeState);
-        needRxSignalMaxDelayUs = DELAY_5_HZ;
 
         break;
 #endif
@@ -473,13 +470,17 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
     UNUSED(currentTimeUs);
     UNUSED(currentDeltaTimeUs);
 
-    return taskUpdateRxMainInProgress() || rxDataProcessingRequired || auxiliaryProcessingRequired || !failsafeIsReceivingRxData();
+    return taskUpdateRxMainInProgress() || rxDataProcessingRequired || auxiliaryProcessingRequired;
 }
 
-FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
+FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs, timeDelta_t anticipatedDeltaTime10thUs)
 {
     bool signalReceived = false;
     bool useDataDrivenProcessing = true;
+    // 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
@@ -495,7 +496,6 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current
         if (isPPMDataBeingReceived()) {
             signalReceived = true;
             rxIsInFailsafeMode = false;
-            needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
             resetPPMDataReceivedState();
         }
 
@@ -504,7 +504,6 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current
         if (isPWMDataBeingReceived()) {
             signalReceived = true;
             rxIsInFailsafeMode = false;
-            needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
             useDataDrivenProcessing = false;
         }
 
@@ -519,13 +518,8 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current
                 rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0;
                 bool rxFrameDropped = (frameStatus & RX_FRAME_DROPPED) != 0;
                 signalReceived = !(rxIsInFailsafeMode || rxFrameDropped);
-                if (signalReceived) {
-                    needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
-                }
-
                 setLinkQuality(signalReceived, currentDeltaTimeUs);
             }
-
             if (frameStatus & RX_FRAME_PROCESSING_REQUIRED) {
                 auxiliaryProcessingRequired = true;
             }
@@ -535,12 +529,19 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current
     }
 
     if (signalReceived) {
-        rxSignalReceived = true;
+        needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
-    } else if (currentTimeUs >= needRxSignalBefore) {
+        rxSignalReceived = true; // immediately process data
-        rxSignalReceived = false;
+    } else {
+        // no signal, wait 100ms and perform signal loss behaviour every 100ms
+        if (cmpTimeUs(needRxSignalBefore, currentTimeUs) > (timeDelta_t)DELAY_10_HZ) {
+            rxSignalReceived = false;
+            // rcData[] needs to be processed to rcCommand
+            rxDataProcessingRequired = true;
+            needRxSignalBefore = currentTimeUs + (timeDelta_t)DELAY_10_HZ;
+        }
     }
 
-    if ((signalReceived && useDataDrivenProcessing) || cmpTimeUs(currentTimeUs, rxNextUpdateAtUs) > 0) {
+    if (signalReceived && useDataDrivenProcessing) {
         rxDataProcessingRequired = true;
     }
 }
@@ -645,50 +646,83 @@ static void readRxChannelsApplyRanges(void)
 static void detectAndApplySignalLossBehaviour(void)
 {
     const uint32_t currentTimeMs = millis();
-
+    const bool failsafeAuxSwitch = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
-    const bool useValueFromRx = rxSignalReceived && !rxIsInFailsafeMode;
+    bool validRxPacket = rxSignalReceived && !failsafeAuxSwitch;
+    // becomes false when a packet is bad or we use a failsafe switch - logged to blackbox
+    rxFlightChannelsValid = true;
+    // becomes false when one or more flight channels has bad Rx data for longer than hold time
 
     DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 0, rxSignalReceived);
     DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 1, rxIsInFailsafeMode);
 
-    rxFlightChannelsValid = true;
     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 validPulse = useValueFromRx && isPulseValid(sample);
+        if (thisChannelValid) {
-
+            // reset invalid pulse period timer for each good-channel
-        if (validPulse) {
+            validRxSignalTimeout[channel] = currentTimeMs + MAX_INVALID_PULSE_TIME;
-            rcInvalidPulsPeriod[channel] = currentTimeMs + MAX_INVALID_PULS_TIME;
+        }
+        // set failsafe and hold values
+        if (failsafeIsActive() && ARMING_FLAG(ARMED)) {
+            // STAGE 2 failsafe is active, and armed.  Apply failsafe values until failsafe ends or disarmed
+            if (channel < NON_AUX_CHANNEL_COUNT) {
+                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
+                sample = getRxfailValue(channel);
+            }
         } else {
-            if (cmp32(currentTimeMs, rcInvalidPulsPeriod[channel]) < 0) {
+            if (!thisChannelValid) {
-                continue;           // skip to next channel to hold channel value MAX_INVALID_PULS_TIME
+                if (cmp32(currentTimeMs, validRxSignalTimeout[channel]) < 0) {
-            } else {
+                    //  HOLD PERIOD for any invalid channels
-                sample = getRxfailValue(channel);   // after that apply rxfail value
+                } else {
-                if (channel < NON_AUX_CHANNEL_COUNT) {
+                    //  STAGE 1 failsafe settings apply to any channels invalid for more than hold time
-                    rxFlightChannelsValid = false;
+                    if (channel < NON_AUX_CHANNEL_COUNT) {
+                        rxFlightChannelsValid = false;
+                        sample = getRxfailValue(channel);
+                        //  affected RPYT values will be set as per Stage 1 Configurator settings
+                    } else if (!failsafeAuxSwitch) {
+                        //  Aux channels as Set in Configurator, unless failsafe initiated by switch
+                        sample = getRxfailValue(channel);
+                    }
                 }
             }
         }
+
 #if defined(USE_PWM) || defined(USE_PPM)
         if (rxRuntimeState.rxProvider == RX_PROVIDER_PARALLEL_PWM || rxRuntimeState.rxProvider == RX_PROVIDER_PPM) {
-            // smooth output for PWM and PPM
+            //  smooth output for PWM and PPM using moving average
             rcData[channel] = calculateChannelMovingAverage(channel, sample);
         } else
 #endif
         {
+            // set rcData to either clean raw incoming values, or failsafe values
             rcData[channel] = sample;
         }
     }
 
-    if (rxFlightChannelsValid && !IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
+    if (!rxFlightChannelsValid) {
+        // show RXLOSS in OSD if any RPYT channel, or any packets, are lost for more than hold time
+        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);
         failsafeOnValidDataReceived();
     } else {
-        rxIsInFailsafeMode = true;
+        //  failsafe switch is on, or a bad packet, or at least one invalid channel for more than hold time
+        //  -> 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();
-        for (int channel = 0; channel < rxChannelCount; channel++) {
-            rcData[channel] = getRxfailValue(channel);
-        }
     }
+
     DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 3, rcData[THROTTLE]);
 }
 
@@ -703,7 +737,6 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
     }
 
     rxDataProcessingRequired = false;
-    rxNextUpdateAtUs = currentTimeUs + DELAY_15_HZ;
 
     // only proceed when no more samples to skip and suspend period is over
     if (skipRxSamples || currentTimeUs <= suspendRxSignalUntil) {

src/main/rx/rx.h

@@ -177,7 +177,7 @@ extern rxRuntimeState_t rxRuntimeState; //!!TODO remove this extern, only needed
 void rxInit(void);
 void rxProcessPending(bool state);
 bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
-void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
+void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs, timeDelta_t anticipatedDeltaTime10thUs);
 bool rxIsReceivingSignal(void);
 bool rxAreFlightChannelsValid(void);
 bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs);

src/main/scheduler/scheduler.c

@@ -494,7 +494,7 @@ FAST_CODE void scheduler(void)
             }
             DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 0, clockCyclesTo10thMicros(cmpTimeCycles(nowCycles, lastTargetCycles)));
 #endif
-            currentTimeUs = clockCyclesToMicros(nowCycles);
+            currentTimeUs = micros();
             taskExecutionTimeUs += schedulerExecuteTask(gyroTask, currentTimeUs);
 
             if (gyroFilterReady()) {
@@ -504,15 +504,15 @@ FAST_CODE void scheduler(void)
                 taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_PID), currentTimeUs);
             }
             if (rxFrameReady()) {
-                // Check to for incoming RX data. Don't do this in the checker as that is called repeatedly within
+                // Check for incoming RX data. Don't do this in the checker as that is called repeatedly within
                 // a given gyro loop, and ELRS takes a long time to process this and so can only be safely processed
                 // before the checkers
-                rxFrameCheck(currentTimeUs, cmpTimeUs(currentTimeUs, getTask(TASK_RX)->lastExecutedAtUs));
+                rxFrameCheck(currentTimeUs, cmpTimeUs(currentTimeUs, getTask(TASK_RX)->lastExecutedAtUs), getTask(TASK_RX)->movingSumDeltaTime10thUs / TASK_STATS_MOVING_SUM_COUNT);
             }
 
             // Check for failsafe conditions without reliance on the RX task being well behaved
             if (cmp32(millis(), lastFailsafeCheckMs) > PERIOD_RXDATA_FAILURE) {
-                // This is very low cost taking less that 4us every 200ms
+                // This is very low cost taking less that 4us every 10ms
                 failsafeCheckDataFailurePeriod();
                 failsafeUpdateState();
                 lastFailsafeCheckMs = millis();

src/main/scheduler/scheduler.h

@@ -28,7 +28,7 @@
 #define TASK_PERIOD_MS(ms) ((ms) * 1000)
 #define TASK_PERIOD_US(us) (us)
 
-#define TASK_STATS_MOVING_SUM_COUNT     64
+#define TASK_STATS_MOVING_SUM_COUNT     8
 
 #define LOAD_PERCENTAGE_ONE             100
 

src/main/target/SITL/target.c

@@ -334,7 +334,7 @@ uint32_t millis(void) {
     return millis64() & 0xFFFFFFFF;
 }
 
-uint32_t clockCyclesToMicros(uint32_t clockCycles)
+int32_t clockCyclesToMicros(int32_t clockCycles)
 {
     return clockCycles;
 }

src/test/unit/flight_failsafe_unittest.cc

@@ -183,7 +183,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndStartsLanding)
     failsafeOnValidDataReceived();                  // set last valid sample at current time
 
     // when
-    for (sysTickUptime = 0; sysTickUptime < (uint32_t)(PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND); sysTickUptime++) {
+    for (sysTickUptime = 0; sysTickUptime < (uint32_t)(failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND); sysTickUptime++) {
         failsafeOnValidDataFailed();
 
         failsafeUpdateState();
@@ -237,7 +237,7 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
     EXPECT_TRUE(isArmingDisabled());
 
     // given
-    sysTickUptime += PERIOD_OF_30_SECONDS + 1;      // adjust time to point just past the required additional recovery time
+    sysTickUptime += PERIOD_OF_3_SECONDS + 1;      // adjust time to point just past the required additional recovery time
     failsafeOnValidDataReceived();
 
     // when
@@ -361,7 +361,7 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeKill)
     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 
     // given
-    sysTickUptime += PERIOD_OF_1_SECONDS + 1;       // adjust time to point just past the required additional recovery time
+    sysTickUptime += PERIOD_OF_3_SECONDS + 1;       // adjust time to point just past the required additional recovery time
     failsafeOnValidDataReceived();
 
     // when
@@ -435,8 +435,7 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Land)
     failsafeUpdateState();                          // should activate stage2 immediately
 
     // then
-    EXPECT_TRUE(failsafeIsActive());
+    EXPECT_TRUE(failsafeIsActive());               // stick induced failsafe allows re-arming
-    EXPECT_TRUE(isArmingDisabled());
     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
     EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
 
@@ -456,7 +455,7 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Land)
     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 
     // given
-    sysTickUptime += PERIOD_OF_30_SECONDS + 1;       // adjust time to point just past the required additional recovery time
+    sysTickUptime += PERIOD_OF_3_SECONDS + 1;       // adjust time to point just past the required additional recovery time
 
     // and
     deactivateBoxFailsafe();
@@ -497,7 +496,7 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected
     sysTickUptime = 0;                              // restart time from 0
     failsafeOnValidDataReceived();                  // set last valid sample at current time
 
-    // when
+    // enter stage 1 failsafe
     for (sysTickUptime = 0; sysTickUptime < PERIOD_RXDATA_FAILURE; sysTickUptime++) {
         failsafeOnValidDataFailed();
 
@@ -520,10 +519,20 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected
     EXPECT_FALSE(failsafeIsActive());
     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
     EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
-    EXPECT_TRUE(isArmingDisabled());
+    EXPECT_FALSE(isArmingDisabled());               // arming not blocked in stage 1
 
-    // given
+    // add enough time to enter stage 2
-    // enough valid data is received
+    sysTickUptime += failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND + 1;
+    failsafeOnValidDataFailed();
+
+    // when
+    failsafeUpdateState();
+
+    // then
+    EXPECT_TRUE(isArmingDisabled());                // arming blocked until recovery time
+    EXPECT_FALSE(failsafeIsActive());               // failsafe is still not active
+
+    // given valid data is received for the recovery time, allow re-arming
     uint32_t sysTickTarget = sysTickUptime + PERIOD_RXDATA_RECOVERY;
     for (; sysTickUptime < sysTickTarget; sysTickUptime++) {
         failsafeOnValidDataReceived();
@@ -546,6 +555,7 @@ extern "C" {
 float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 float rcCommand[4];
 int16_t debug[DEBUG16_VALUE_COUNT];
+uint8_t debugMode = 0;
 bool isUsingSticksToArm = true;
 
 PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0);

src/test/unit/link_quality_unittest.cc

@@ -45,6 +45,7 @@ extern "C" {
     #include "fc/rc_modes.h"
     #include "fc/runtime_config.h"
 
+    #include "flight/failsafe.h"
     #include "flight/imu.h"
     #include "flight/mixer.h"
     #include "flight/pid.h"
@@ -86,6 +87,7 @@ extern "C" {
     PG_REGISTER(pilotConfig_t, pilotConfig, PG_PILOT_CONFIG, 0);
     PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
     PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
+    PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0);
 
     timeUs_t simulationTime = 0;
 
@@ -502,6 +504,7 @@ extern "C" {
     void persistentObjectWrite(persistentObjectId_e, uint32_t) {}
     void failsafeOnRxSuspend(uint32_t ) {}
     void failsafeOnRxResume(void) {}
+    uint32_t failsafeFailurePeriodMs(void) { return 400; }
     void featureDisableImmediate(uint32_t) { }
     bool rxMspFrameComplete(void) { return false; }
     bool isPPMDataBeingReceived(void) { return false; }

src/test/unit/rx_ranges_unittest.cc

@@ -25,11 +25,13 @@ extern "C" {
     #include "build/debug.h"
     #include "drivers/io.h"
     #include "common/maths.h"
+    #include "flight/failsafe.h"
     #include "pg/pg.h"
     #include "pg/pg_ids.h"
     #include "pg/rx.h"
     #include "fc/rc_controls.h"
     #include "fc/rc_modes.h"
+    #include "fc/runtime_config.h"
     #include "rx/rx.h"
 }
 
@@ -39,10 +41,12 @@ extern "C" {
 extern "C" {
 
 PG_REGISTER(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0);
+PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0);
 
 boxBitmask_t rcModeActivationMask;
 int16_t debug[DEBUG16_VALUE_COUNT];
 uint8_t debugMode = 0;
+uint8_t armingFlags = 0;
 
 extern float applyRxChannelRangeConfiguraton(float sample, const rxChannelRangeConfig_t *range);
 }
@@ -107,8 +111,11 @@ extern "C" {
 
 void failsafeOnRxSuspend(uint32_t ) {}
 void failsafeOnRxResume(void) {}
+bool failsafeIsActive(void) { return false; }
 bool failsafeIsReceivingRxData(void) { return true; }
 bool taskUpdateRxMainInProgress() { return true; }
+void setArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
+void unsetArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
 
 uint32_t micros(void) { return 0; }
 uint32_t millis(void) { return 0; }
@@ -225,6 +232,11 @@ void failsafeOnValidDataFailed(void)
 {
 }
 
+uint32_t failsafeFailurePeriodMs(void)
+{
+    return 400;
+}
+
 float pt1FilterGain(float f_cut, float dT)
 {
     UNUSED(f_cut);

src/test/unit/rx_rx_unittest.cc

@@ -27,6 +27,8 @@ extern "C" {
     #include "build/debug.h"
     #include "drivers/io.h"
     #include "fc/rc_controls.h"
+    #include "fc/runtime_config.h"
+    #include "flight/failsafe.h"
     #include "rx/rx.h"
     #include "fc/rc_modes.h"
     #include "common/maths.h"
@@ -39,6 +41,7 @@ extern "C" {
     boxBitmask_t rcModeActivationMask;
     int16_t debug[DEBUG16_VALUE_COUNT];
     uint8_t debugMode = 0;
+    uint8_t armingFlags = 0;
 
     bool isPulseValid(uint16_t pulseDuration);
 
@@ -47,6 +50,7 @@ extern "C" {
     );
 
     PG_REGISTER(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0);
+    PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0);
 }
 
 #include "unittest_macros.h"
@@ -210,7 +214,9 @@ extern "C" {
 
     void failsafeOnRxSuspend(uint32_t ) {}
     void failsafeOnRxResume(void) {}
+    bool failsafeIsActive(void) { return false; }
     bool failsafeIsReceivingRxData(void) { return true; }
+    uint32_t failsafeFailurePeriodMs(void) { return 400; }
 
     uint32_t micros(void) { return 0; }
     uint32_t millis(void) { return 0; }
@@ -236,6 +242,8 @@ extern "C" {
     void xBusInit(const rxConfig_t *, rxRuntimeState_t *) {}
     void rxMspInit(const rxConfig_t *, rxRuntimeState_t *) {}
     void rxPwmInit(const rxConfig_t *, rxRuntimeState_t *) {}
+    void setArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
+    void unsetArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
     bool taskUpdateRxMainInProgress() { return true; }
     float pt1FilterGain(float f_cut, float dT)
     {

src/test/unit/scheduler_unittest.cc

@@ -74,7 +74,7 @@ extern "C" {
     uint32_t simulatedTime = 0;
     uint32_t micros(void) { return simulatedTime; }
     uint32_t millis(void) { return simulatedTime/1000; } // Note simplistic mapping suitable only for short unit tests
-    uint32_t clockCyclesToMicros(uint32_t x) { return x/10;}
+    int32_t clockCyclesToMicros(int32_t x) { return x/10;}
     int32_t clockCyclesTo10thMicros(int32_t x) { return x;}
     uint32_t clockMicrosToCycles(uint32_t x) { return x*10;}
     uint32_t getCycleCounter(void) {return simulatedTime * 10;}