Split rx processing into three states to eliminate cycle time jitter

Allow tasks with state machines to control which state determines task duration

Fix unit tests

src/main/fc/core.c

@@ -744,11 +744,6 @@ bool isAirmodeActivated()
  */
 bool processRx(timeUs_t currentTimeUs)
 {
-    static bool armedBeeperOn = false;
-#ifdef USE_TELEMETRY
-    static bool sharedPortTelemetryEnabled = false;
-#endif
-
     timeDelta_t frameAgeUs;
     timeDelta_t frameDeltaUs = rxGetFrameDelta(&frameAgeUs);
 
@@ -891,6 +886,17 @@ bool processRx(timeUs_t currentTimeUs)
     }
 #endif
 
+    return true;
+}
+
+void processRxModes(timeUs_t currentTimeUs)
+{
+    static bool armedBeeperOn = false;
+#ifdef USE_TELEMETRY
+    static bool sharedPortTelemetryEnabled = false;
+#endif
+    const throttleStatus_e throttleStatus = calculateThrottleStatus();
+
     // When armed and motors aren't spinning, do beeps and then disarm
     // board after delay so users without buzzer won't lose fingers.
     // mixTable constrains motor commands, so checking  throttleStatus is enough
@@ -1082,8 +1088,6 @@ bool processRx(timeUs_t currentTimeUs)
 #endif
 
     pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || featureIsEnabled(FEATURE_ANTI_GRAVITY));
-
-    return true;
 }
 
 static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs)

src/main/fc/core.h

@@ -76,6 +76,7 @@ void disarm(flightLogDisarmReason_e reason);
 void tryArm(void);
 
 bool processRx(timeUs_t currentTimeUs);
+void processRxModes(timeUs_t currentTimeUs);
 void updateArmingStatus(void);
 
 void taskGyroSample(timeUs_t currentTimeUs);

src/main/fc/tasks.c

@@ -158,23 +158,54 @@ static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
 }
 #endif
 
+static enum {
+    CHECK, PROCESS, MODES, UPDATE
+} rxState = CHECK;
+
+bool taskUpdateRxMainInProgress()
+{
+    return (rxState != CHECK);
+}
+
 static void taskUpdateRxMain(timeUs_t currentTimeUs)
 {
-    if (!processRx(currentTimeUs)) {
-        return;
-    }
+    switch (rxState) {
+    default:
+    case CHECK:
+        ignoreTaskTime();
+        rxState = PROCESS;
+        break;
 
-    // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
-    updateRcCommands();
-    updateArmingStatus();
+    case PROCESS:
+        ignoreTaskTime();
+        if (!processRx(currentTimeUs)) {
+            break;
+        }
+        rxState = MODES;
+        break;
+
+    case MODES:
+        processRxModes(currentTimeUs);
+        rxState = UPDATE;
+        break;
+
+    case UPDATE:
+        ignoreTaskTime();
+        // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
+        updateRcCommands();
+        updateArmingStatus();
 
 #ifdef USE_USB_CDC_HID
-    if (!ARMING_FLAG(ARMED)) {
-        sendRcDataToHid();
-    }
+        if (!ARMING_FLAG(ARMED)) {
+            sendRcDataToHid();
+        }
 #endif
+        rxState = CHECK;
+        break;
+    }
 }
 
+
 #ifdef USE_BARO
 static void taskUpdateBaro(timeUs_t currentTimeUs)
 {

src/main/fc/tasks.h

@@ -24,3 +24,6 @@
 
 void tasksInit(void);
 task_t *getTask(unsigned taskId);
+
+bool taskUpdateRxMainInProgress();
+

src/main/rx/rx.c

@@ -43,6 +43,7 @@
 
 #include "fc/rc_controls.h"
 #include "fc/rc_modes.h"
+#include "fc/tasks.h"
 
 #include "flight/failsafe.h"
 
@@ -459,6 +460,11 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
     bool signalReceived = false;
     bool useDataDrivenProcessing = true;
 
+    if (taskUpdateRxMainInProgress()) {
+        // There are more states to process
+        return true;
+    }
+
     switch (rxRuntimeState.rxProvider) {
     default:
 

src/main/scheduler/scheduler.c

@@ -50,6 +50,7 @@
 // 3 - time spent executing check function
 
 static FAST_DATA_ZERO_INIT task_t *currentTask = NULL;
+static FAST_DATA_ZERO_INIT bool ignoreCurrentTaskTime;
 
 static FAST_DATA_ZERO_INIT uint32_t totalWaitingTasks;
 static FAST_DATA_ZERO_INIT uint32_t totalWaitingTasksSamples;
@@ -210,6 +211,12 @@ timeDelta_t getTaskDeltaTimeUs(taskId_e taskId)
     }
 }
 
+// Called by tasks executing what are known to be short states
+void ignoreTaskTime()
+{
+    ignoreCurrentTaskTime = true;
+}
+
 void schedulerSetCalulateTaskStatistics(bool calculateTaskStatisticsToUse)
 {
     calculateTaskStatistics = calculateTaskStatisticsToUse;
@@ -281,6 +288,7 @@ FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTi
 
     if (selectedTask) {
         currentTask = selectedTask;
+        ignoreCurrentTaskTime = false;
         selectedTask->taskLatestDeltaTimeUs = cmpTimeUs(currentTimeUs, selectedTask->lastExecutedAtUs);
 #if defined(USE_TASK_STATISTICS)
         float period = currentTimeUs - selectedTask->lastExecutedAtUs;
@@ -295,10 +303,12 @@ FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTi
             const timeUs_t currentTimeBeforeTaskCallUs = micros();
             selectedTask->taskFunc(currentTimeBeforeTaskCallUs);
             taskExecutionTimeUs = micros() - currentTimeBeforeTaskCallUs;
-            selectedTask->movingSumExecutionTimeUs += taskExecutionTimeUs - selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
-            selectedTask->movingSumDeltaTimeUs += selectedTask->taskLatestDeltaTimeUs - selectedTask->movingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
+            if (!ignoreCurrentTaskTime) {
+                selectedTask->movingSumExecutionTimeUs += taskExecutionTimeUs - selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
+                selectedTask->movingSumDeltaTimeUs += selectedTask->taskLatestDeltaTimeUs - selectedTask->movingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
+                selectedTask->maxExecutionTimeUs = MAX(selectedTask->maxExecutionTimeUs, taskExecutionTimeUs);
+            }
             selectedTask->totalExecutionTimeUs += taskExecutionTimeUs;   // time consumed by scheduler + task
-            selectedTask->maxExecutionTimeUs = MAX(selectedTask->maxExecutionTimeUs, taskExecutionTimeUs);
             selectedTask->movingAverageCycleTimeUs += 0.05f * (period - selectedTask->movingAverageCycleTimeUs);
         } else
 #endif

src/main/scheduler/scheduler.h

@@ -187,6 +187,7 @@ void getTaskInfo(taskId_e taskId, taskInfo_t *taskInfo);
 void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs);
 void setTaskEnabled(taskId_e taskId, bool newEnabledState);
 timeDelta_t getTaskDeltaTimeUs(taskId_e taskId);
+void ignoreTaskTime();
 void schedulerSetCalulateTaskStatistics(bool calculateTaskStatistics);
 void schedulerResetTaskStatistics(taskId_e taskId);
 void schedulerResetTaskMaxExecutionTime(taskId_e taskId);

src/main/sensors/barometer.c

@@ -51,6 +51,8 @@
 
 #include "sensors/sensors.h"
 
+#include "scheduler/scheduler.h"
+
 #include "barometer.h"
 
 baro_t baro;                        // barometer access functions
@@ -349,7 +351,7 @@ static uint32_t recalculateBarometerTotal(uint32_t pressureTotal, int32_t newPre
         nextSampleIndex = 0;
         baroReady = true;
     } else {
-	    nextSampleIndex = (currentSampleIndex + 1);
+        nextSampleIndex = (currentSampleIndex + 1);
     }
     barometerSamples[currentSampleIndex] = applyBarometerMedianFilter(newPressureReading);
 
@@ -385,6 +387,12 @@ uint32_t baroUpdate(void)
         debug[0] = state;
     }
 
+    // Tell the scheduler to ignore how long this task takes unless the pressure is being read
+    // as that takes the longest
+    if (state != BAROMETER_NEEDS_PRESSURE_READ) {
+           ignoreTaskTime();
+    }
+
     switch (state) {
         default:
         case BAROMETER_NEEDS_TEMPERATURE_START:
@@ -414,6 +422,8 @@ uint32_t baroUpdate(void)
         case BAROMETER_NEEDS_PRESSURE_READ:
             if (baro.dev.read_up(&baro.dev)) {
                 state = BAROMETER_NEEDS_PRESSURE_SAMPLE;
+            } else {
+                ignoreTaskTime();
             }
         break;
 

src/test/unit/link_quality_unittest.cc

@@ -459,6 +459,7 @@ extern "C" {
     void failsafeOnValidDataReceived(void) { }
     void failsafeOnValidDataFailed(void) { }
     void pinioBoxTaskControl(void) { }
+    bool taskUpdateRxMainInProgress() { return true; }
 
     void rxPwmInit(rxRuntimeState_t *rxRuntimeState, rcReadRawDataFnPtr *callback)
     {

src/test/unit/rx_ranges_unittest.cc

@@ -107,6 +107,7 @@ extern "C" {
 
 void failsafeOnRxSuspend(uint32_t ) {}
 void failsafeOnRxResume(void) {}
+bool taskUpdateRxMainInProgress() { return true; }
 
 uint32_t micros(void) { return 0; }
 uint32_t millis(void) { return 0; }

src/test/unit/rx_rx_unittest.cc

@@ -235,6 +235,7 @@ extern "C" {
     void xBusInit(const rxConfig_t *, rxRuntimeState_t *) {}
     void rxMspInit(const rxConfig_t *, rxRuntimeState_t *) {}
     void rxPwmInit(const rxConfig_t *, rxRuntimeState_t *) {}
+    bool taskUpdateRxMainInProgress() { return true; }
     float pt1FilterGain(float f_cut, float dT)
     {
         UNUSED(f_cut);