Move failsafe processing out of the RX task into the scheduler as a r…

…eal-time task

src/main/fc/core.c

@@ -767,7 +767,6 @@ bool processRx(timeUs_t currentTimeUs)
     if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
         failsafeStartMonitoring();
     }
-    failsafeUpdateState();
 
     const throttleStatus_e throttleStatus = calculateThrottleStatus();
     const uint8_t throttlePercent = calculateThrottlePercentAbs();

src/main/flight/failsafe.c

@@ -192,6 +192,14 @@ void failsafeOnValidDataFailed(void)
     }
 }
 
+void failsafeCheckDataFailurePeriod(void)
+{
+    if ((millis() - failsafeState.validRxDataReceivedAt) > failsafeState.rxDataFailurePeriod) {
+        setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); // To prevent arming with no RX link
+        failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
+    }
+}
+
 void failsafeUpdateState(void)
 {
     if (!failsafeIsMonitoring()) {

src/main/flight/failsafe.h

@@ -103,6 +103,7 @@ failsafePhase_e failsafePhase(void);
 bool failsafeIsMonitoring(void);
 bool failsafeIsActive(void);
 bool failsafeIsReceivingRxData(void);
+void failsafeCheckDataFailurePeriod(void);
 void failsafeOnRxSuspend(uint32_t suspendPeriod);
 void failsafeOnRxResume(void);
 

src/main/scheduler/scheduler.c

@@ -42,6 +42,8 @@
 #include "fc/core.h"
 #include "fc/tasks.h"
 
+#include "flight/failsafe.h"
+
 #include "scheduler.h"
 
 #include "sensors/gyro_init.h"
@@ -104,6 +106,8 @@ static int16_t taskCount = 0;
 static uint32_t nextTimingCycles;
 #endif
 
+static timeUs_t lastFailsafeCheck = 0;
+
 // No need for a linked list for the queue, since items are only inserted at startup
 
 STATIC_UNIT_TESTED FAST_DATA_ZERO_INIT task_t* taskQueueArray[TASK_COUNT + 1]; // extra item for NULL pointer at end of queue
@@ -499,6 +503,14 @@ FAST_CODE void scheduler(void)
                 taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_PID), currentTimeUs);
             }
 
+            // Check for failsafe conditions without reliance on the RX task being well behaved
+            if (millis() - lastFailsafeCheck > PERIOD_RXDATA_FAILURE) {
+                // This is very low cost taking less that 4us every 200ms
+                failsafeCheckDataFailurePeriod();
+                failsafeUpdateState();
+                lastFailsafeCheck = millis();
+            }
+
 #if defined(USE_LATE_TASK_STATISTICS)
             // % CPU busy
             DEBUG_SET(DEBUG_TIMING_ACCURACY, 0, getAverageSystemLoadPercent());

src/test/unit/scheduler_unittest.cc

@@ -70,6 +70,7 @@ extern "C" {
     // set up micros() to simulate time
     uint32_t simulatedTime = 0;
     uint32_t micros(void) { return simulatedTime; }
+    uint32_t millis(void) { return simulatedTime/1000; }
     uint32_t clockCyclesToMicros(uint32_t x) { return x/10;}
     int32_t clockCyclesTo10thMicros(int32_t x) { return x;}
     uint32_t clockMicrosToCycles(uint32_t x) { return x*10;}
@@ -78,6 +79,8 @@ extern "C" {
     // set up tasks to take a simulated representative time to execute
     bool gyroFilterReady(void) { return taskFilterReady; }
     bool pidLoopReady(void) { return taskPidReady; }
+    void failsafeCheckDataFailurePeriod(void) {}
+    void failsafeUpdateState(void) {}
     void taskGyroSample(timeUs_t) { simulatedTime += TEST_GYRO_SAMPLE_TIME; taskGyroRan = true; }
     void taskFiltering(timeUs_t) { simulatedTime += TEST_FILTERING_TIME; taskFilterRan = true; }
     void taskMainPidLoop(timeUs_t) { simulatedTime += TEST_PID_LOOP_TIME; taskPidRan = true; }