Merge pull request #11274 from SteveCEvans/rs_state_time

Fix issues impacting RX_STATE_MODES state duration and add DEBUG_RX_STATE_TIME

src/main/build/debug.c

@@ -99,4 +99,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "TIMING_ACCURACY",
     "RX_EXPRESSLRS_SPI",
     "RX_EXPRESSLRS_PHASELOCK",
+    "RX_STATE_TIME"
 };

src/main/build/debug.h

@@ -97,6 +97,7 @@ typedef enum {
     DEBUG_TIMING_ACCURACY,
     DEBUG_RX_EXPRESSLRS_SPI,
     DEBUG_RX_EXPRESSLRS_PHASELOCK,
+    DEBUG_RX_STATE_TIME,
     DEBUG_COUNT
 } debugType_e;
 

src/main/fc/rc_adjustments.c

@@ -659,6 +659,9 @@ static uint8_t applySelectAdjustment(adjustmentFunction_e adjustmentFunction, ui
 
 static void calcActiveAdjustmentRanges(void)
 {
+    // This initialisation upsets the scheduler task duration estimation
+    schedulerIgnoreTaskExecTime();
+
     adjustmentRange_t defaultAdjustmentRange;
     memset(&defaultAdjustmentRange, 0, sizeof(defaultAdjustmentRange));
 

src/main/fc/tasks.c

@@ -182,9 +182,10 @@ bool taskUpdateRxMainInProgress()
 
 static void taskUpdateRxMain(timeUs_t currentTimeUs)
 {
-    static timeUs_t rxStateDurationFracUs[RX_STATE_COUNT];
+    static timeDelta_t rxStateDurationFracUs[RX_STATE_COUNT];
-    timeUs_t executeTimeUs;
+    timeDelta_t executeTimeUs;
     rxState_e oldRxState = rxState;
+    timeDelta_t anticipatedExecutionTime;
 
     // Where we are using a state machine call schedulerIgnoreTaskExecRate() for all states bar one
     if (rxState != RX_STATE_UPDATE) {
@@ -224,17 +225,25 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
         break;
     }
 
-    if (schedulerGetIgnoreTaskExecTime()) {
+    if (!schedulerGetIgnoreTaskExecTime()) {
-        return;
+        executeTimeUs = micros() - currentTimeUs + RX_TASK_MARGIN;
+
+        // If the scheduler has reduced the anticipatedExecutionTime due to task aging, pick that up
+        anticipatedExecutionTime = schedulerGetNextStateTime();
+        if (anticipatedExecutionTime != (rxStateDurationFracUs[oldRxState] >> RX_TASK_DECAY_SHIFT)) {
+            rxStateDurationFracUs[oldRxState] = anticipatedExecutionTime << RX_TASK_DECAY_SHIFT;
+        }
+
+        if (executeTimeUs > (rxStateDurationFracUs[oldRxState] >> RX_TASK_DECAY_SHIFT)) {
+            rxStateDurationFracUs[oldRxState] = executeTimeUs << RX_TASK_DECAY_SHIFT;
+        } else {
+            // Slowly decay the max time
+            rxStateDurationFracUs[oldRxState]--;
+        }
     }
 
-    executeTimeUs = micros() - currentTimeUs + RX_TASK_MARGIN;
+    if (debugMode == DEBUG_RX_STATE_TIME) {
-
+        debug[oldRxState] = rxStateDurationFracUs[oldRxState] >> RX_TASK_DECAY_SHIFT;
-    if (executeTimeUs > (rxStateDurationFracUs[oldRxState] >> RX_TASK_DECAY_SHIFT)) {
-        rxStateDurationFracUs[oldRxState] = executeTimeUs << RX_TASK_DECAY_SHIFT;
-    } else {
-        // Slowly decay the max time
-        rxStateDurationFracUs[oldRxState]--;
     }
 
     schedulerSetNextStateTime(rxStateDurationFracUs[rxState] >> RX_TASK_DECAY_SHIFT);

src/main/osd/osd.c

@@ -299,6 +299,13 @@ void changeOsdProfileIndex(uint8_t profileIndex)
 
 void osdAnalyzeActiveElements(void)
 {
+    /* This code results in a total RX task RX_STATE_MODES state time of ~68us on an F411 overclocked to 108MHz
+     * This upsets the scheduler task duration estimation and will break SPI RX communication. This can
+     * occur in flight, but only when the OSD profile is changed by switch so can be ignored, only causing
+     * one late task instance.
+     */
+    schedulerIgnoreTaskExecTime();
+
     osdAddActiveElements();
     osdDrawActiveElementsBackground(osdDisplayPort);
 }
@@ -999,6 +1006,8 @@ void osdDrawStats2(timeUs_t currentTimeUs)
             osdStatsEnabled = false;
             stats.armed_time = 0;
         }
+
+        schedulerIgnoreTaskExecTime();
     }
 #ifdef USE_ESC_SENSOR
     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
@@ -1298,20 +1307,22 @@ void osdUpdate(timeUs_t currentTimeUs)
         break;
     }
 
-    executeTimeUs = micros() - currentTimeUs;
+    if (!schedulerGetIgnoreTaskExecTime()) {
+        executeTimeUs = micros() - currentTimeUs;
 
 
-    // On the first pass no element groups will have been formed, so all elements will have been
+        // On the first pass no element groups will have been formed, so all elements will have been
-    // rendered which is unrepresentative, so ignore
+        // rendered which is unrepresentative, so ignore
-    if (!firstPass) {
+        if (!firstPass) {
-        if (osdCurState == OSD_STATE_UPDATE_ELEMENTS) {
+            if (osdCurState == OSD_STATE_UPDATE_ELEMENTS) {
-            if (executeTimeUs > osdElementGroupDurationUs[osdCurElementGroup]) {
+                if (executeTimeUs > osdElementGroupDurationUs[osdCurElementGroup]) {
-                osdElementGroupDurationUs[osdCurElementGroup] = executeTimeUs;
+                    osdElementGroupDurationUs[osdCurElementGroup] = executeTimeUs;
+                }
             }
-        }
 
-        if (executeTimeUs > osdStateDurationUs[osdCurState]) {
+            if (executeTimeUs > osdStateDurationUs[osdCurState]) {
-            osdStateDurationUs[osdCurState] = executeTimeUs;
+                osdStateDurationUs[osdCurState] = executeTimeUs;
+            }
         }
     }
 

src/main/scheduler/scheduler.c

@@ -92,6 +92,11 @@ static FAST_DATA_ZERO_INIT bool gyroEnabled;
 static int32_t desiredPeriodCycles;
 static uint32_t lastTargetCycles;
 
+static uint8_t skippedRxAttempts = 0;
+#ifdef USE_OSD
+static uint8_t skippedOSDAttempts = 0;
+#endif
+
 #if defined(USE_LATE_TASK_STATISTICS)
 static int16_t lateTaskCount = 0;
 static uint32_t lateTaskTotal = 0;
@@ -356,6 +361,11 @@ FAST_CODE void schedulerSetNextStateTime(timeDelta_t nextStateTime)
     taskNextStateTime = nextStateTime;
 }
 
+FAST_CODE timeDelta_t schedulerGetNextStateTime()
+{
+    return currentTask->anticipatedExecutionTime >> TASK_EXEC_TIME_SHIFT;
+}
+
 FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTimeUs)
 {
     timeUs_t taskExecutionTimeUs = 0;
@@ -564,8 +574,8 @@ FAST_CODE void scheduler(void)
                 if (task->attr->checkFunc) {
                     // Increase priority for event driven tasks
                     if (task->dynamicPriority > 0) {
-                        task->taskAgeCycles = 1 + (cmpTimeUs(currentTimeUs, task->lastSignaledAtUs) / task->attr->desiredPeriodUs);
+                        task->taskAgePeriods = 1 + (cmpTimeUs(currentTimeUs, task->lastSignaledAtUs) / task->attr->desiredPeriodUs);
-                        task->dynamicPriority = 1 + task->attr->staticPriority * task->taskAgeCycles;
+                        task->dynamicPriority = 1 + task->attr->staticPriority * task->taskAgePeriods;
                     } else if (task->attr->checkFunc(currentTimeUs, cmpTimeUs(currentTimeUs, task->lastExecutedAtUs))) {
                         const uint32_t checkFuncExecutionTimeUs = cmpTimeUs(micros(), currentTimeUs);
 #if !defined(UNIT_TEST)
@@ -576,17 +586,17 @@ FAST_CODE void scheduler(void)
                         checkFuncTotalExecutionTimeUs += checkFuncExecutionTimeUs;   // time consumed by scheduler + task
                         checkFuncMaxExecutionTimeUs = MAX(checkFuncMaxExecutionTimeUs, checkFuncExecutionTimeUs);
                         task->lastSignaledAtUs = currentTimeUs;
-                        task->taskAgeCycles = 1;
+                        task->taskAgePeriods = 1;
                         task->dynamicPriority = 1 + task->attr->staticPriority;
                     } else {
-                        task->taskAgeCycles = 0;
+                        task->taskAgePeriods = 0;
                     }
                 } else {
                     // Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods)
                     // Task age is calculated from last execution
-                    task->taskAgeCycles = (cmpTimeUs(currentTimeUs, task->lastExecutedAtUs) / task->attr->desiredPeriodUs);
+                    task->taskAgePeriods = (cmpTimeUs(currentTimeUs, task->lastExecutedAtUs) / task->attr->desiredPeriodUs);
-                    if (task->taskAgeCycles > 0) {
+                    if (task->taskAgePeriods > 0) {
-                        task->dynamicPriority = 1 + task->attr->staticPriority * task->taskAgeCycles;
+                        task->dynamicPriority = 1 + task->attr->staticPriority * task->taskAgePeriods;
                     }
                 }
 
@@ -628,6 +638,15 @@ FAST_CODE void scheduler(void)
                 }
 #endif  // USE_LATE_TASK_STATISTICS
 
+                if ((currentTask - tasks) == TASK_RX) {
+                    skippedRxAttempts = 0;
+                }
+#ifdef USE_OSD
+                else if ((currentTask - tasks) == TASK_OSD) {
+                    skippedOSDAttempts = 0;
+                }
+#endif
+
                 if ((cyclesOverdue > 0) || (-cyclesOverdue < taskGuardMinCycles)) {
                     if (taskGuardCycles < taskGuardMaxCycles) {
                         taskGuardCycles += taskGuardDeltaUpCycles;
@@ -638,7 +657,11 @@ FAST_CODE void scheduler(void)
 #if defined(USE_LATE_TASK_STATISTICS)
                 taskCount++;
 #endif  // USE_LATE_TASK_STATISTICS
-            } else if (selectedTask->taskAgeCycles > TASK_AGE_EXPEDITE_COUNT) {
+            } else if ((selectedTask->taskAgePeriods > TASK_AGE_EXPEDITE_COUNT) ||
+#ifdef USE_OSD
+                       (((selectedTask - tasks) == TASK_OSD) && (++skippedOSDAttempts > TASK_AGE_EXPEDITE_OSD)) ||
+#endif
+                       (((selectedTask - tasks) == TASK_RX) && (++skippedRxAttempts > TASK_AGE_EXPEDITE_RX))) {
                 // If a task has been unable to run, then reduce it's recorded estimated run time to ensure
                 // it's ultimate scheduling
                 selectedTask->anticipatedExecutionTime *= TASK_AGE_EXPEDITE_SCALE;

src/main/scheduler/scheduler.h

@@ -47,6 +47,8 @@
 // Decay the estimated max task duration by 1/(1 << TASK_EXEC_TIME_SHIFT) on every invocation
 #define TASK_EXEC_TIME_SHIFT            7
 
+#define TASK_AGE_EXPEDITE_RX            25  // Make RX tasks more schedulable if it's failed to be scheduled this many times
+#define TASK_AGE_EXPEDITE_OSD           25  // Make OSD tasks more schedulable if it's failed to be scheduled this many times
 #define TASK_AGE_EXPEDITE_COUNT         1   // Make aged tasks more schedulable
 #define TASK_AGE_EXPEDITE_SCALE         0.9 // By scaling their expected execution time
 
@@ -197,7 +199,7 @@ typedef struct {
 
     // Scheduling
     uint16_t dynamicPriority;           // measurement of how old task was last executed, used to avoid task starvation
-    uint16_t taskAgeCycles;
+    uint16_t taskAgePeriods;
     timeDelta_t taskLatestDeltaTimeUs;
     timeUs_t lastExecutedAtUs;          // last time of invocation
     timeUs_t lastSignaledAtUs;          // time of invocation event for event-driven tasks
@@ -230,6 +232,7 @@ void schedulerResetTaskStatistics(taskId_e taskId);
 void schedulerResetTaskMaxExecutionTime(taskId_e taskId);
 void schedulerResetCheckFunctionMaxExecutionTime(void);
 void schedulerSetNextStateTime(timeDelta_t nextStateTime);
+timeDelta_t schedulerGetNextStateTime();
 void schedulerInit(void);
 void scheduler(void);
 timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTimeUs);

src/test/Makefile

@@ -278,6 +278,8 @@ scheduler_unittest_SRC := \
 		$(USER_DIR)/common/crc.c \
 		$(USER_DIR)/common/streambuf.c
 
+scheduler_unittest_DEFINES := \
+		USE_OSD= 
 
 sensor_gyro_unittest_SRC := \
 		$(USER_DIR)/sensors/gyro.c \

src/test/unit/link_quality_unittest.cc

@@ -512,6 +512,7 @@ extern "C" {
     bool taskUpdateRxMainInProgress() { return true; }
     void schedulerIgnoreTaskStateTime(void) { }
     void schedulerIgnoreTaskExecRate(void) { }
+    bool schedulerGetIgnoreTaskExecTime() { return false; }
     void schedulerIgnoreTaskExecTime(void) { }
     void schedulerSetNextStateTime(timeDelta_t) {}
 

src/test/unit/osd_unittest.cc

@@ -1377,5 +1377,6 @@ extern "C" {
     void schedulerIgnoreTaskStateTime(void) { }
     void schedulerIgnoreTaskExecRate(void) { }
     void schedulerIgnoreTaskExecTime(void) { }
+    bool schedulerGetIgnoreTaskExecTime() { return false; }
     void schedulerSetNextStateTime(timeDelta_t) {}
 }

src/test/unit/scheduler_unittest.cc

@@ -37,6 +37,8 @@ const int TEST_UPDATE_RX_CHECK_TIME = 34;
 const int TEST_UPDATE_RX_MAIN_TIME = 1;
 const int TEST_IMU_UPDATE_TIME = 5;
 const int TEST_DISPATCH_TIME = 1;
+const int TEST_UPDATE_OSD_CHECK_TIME = 5;
+const int TEST_UPDATE_OSD_TIME = 30;
 
 #define TASK_COUNT_UNITTEST (TASK_BATTERY_VOLTAGE + 1)
 #define TASK_PERIOD_HZ(hz) (1000000 / (hz))
@@ -76,6 +78,8 @@ extern "C" {
     void taskUpdateRxMain(timeUs_t) { simulatedTime += TEST_UPDATE_RX_MAIN_TIME; }
     void imuUpdateAttitude(timeUs_t) { simulatedTime += TEST_IMU_UPDATE_TIME; }
     void dispatchProcess(timeUs_t) { simulatedTime += TEST_DISPATCH_TIME; }
+    bool osdUpdateCheck(timeUs_t, timeDelta_t) { simulatedTime += TEST_UPDATE_OSD_CHECK_TIME; return false; }
+    void osdUpdate(timeUs_t) { simulatedTime += TEST_UPDATE_OSD_TIME; }
 
     void resetGyroTaskTestFlags(void) {
         taskGyroRan = false;
@@ -156,6 +160,13 @@ extern "C" {
             .taskFunc = taskUpdateBatteryVoltage,
             .desiredPeriodUs = TASK_PERIOD_HZ(50),
             .staticPriority = TASK_PRIORITY_MEDIUM,
+        },
+        [TASK_OSD] = {
+            .taskName = "OSD",
+            .checkFunc = osdUpdateCheck,
+            .taskFunc = osdUpdate,
+            .desiredPeriodUs = TASK_PERIOD_HZ(12),
+            .staticPriority = TASK_PRIORITY_LOW,
         }
     };
 
@@ -396,7 +407,7 @@ TEST(SchedulerUnittest, TestTwoTasks)
     simulatedTime = startTime;
     tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime;
     tasks[TASK_ATTITUDE].lastExecutedAtUs = tasks[TASK_ACCEL].lastExecutedAtUs - TEST_UPDATE_ATTITUDE_TIME;
-    EXPECT_EQ(0, tasks[TASK_ATTITUDE].taskAgeCycles);
+    EXPECT_EQ(0, tasks[TASK_ATTITUDE].taskAgePeriods);
     // run the scheduler
     scheduler();
     // no tasks should have run, since neither task's desired time has elapsed