From c05ad2ec9b7d3a0915ef9748a160a0cc28931069 Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Tue, 11 Jan 2022 20:09:55 +0000 Subject: [PATCH 1/4] Fix issues impacting RX_STATE_MODES state duration and add DEBUG_RX_STATE_TIME --- src/main/build/debug.c | 1 + src/main/build/debug.h | 1 + src/main/fc/rc_adjustments.c | 3 +++ src/main/fc/tasks.c | 15 +++++++++++++-- src/main/osd/osd.c | 7 +++++++ src/main/scheduler/scheduler.c | 5 +++++ src/main/scheduler/scheduler.h | 1 + 7 files changed, 31 insertions(+), 2 deletions(-) diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 2ac1b2cbb2..d3ae5fb3f4 100644 --- a/src/main/build/debug.c +++ b/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" }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index acde8b9003..8494550257 100644 --- a/src/main/build/debug.h +++ b/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; diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 51b7a0e439..ad4cbf0529 100644 --- a/src/main/fc/rc_adjustments.c +++ b/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)); diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 0d8e2c1b5c..463954b4e0 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -182,9 +182,10 @@ bool taskUpdateRxMainInProgress() static void taskUpdateRxMain(timeUs_t currentTimeUs) { - static timeUs_t rxStateDurationFracUs[RX_STATE_COUNT]; - timeUs_t executeTimeUs; + static timeDelta_t rxStateDurationFracUs[RX_STATE_COUNT]; + 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) { @@ -230,6 +231,12 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs) 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 { @@ -237,6 +244,10 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs) rxStateDurationFracUs[oldRxState]--; } + if (debugMode == DEBUG_RX_STATE_TIME) { + debug[oldRxState] = rxStateDurationFracUs[oldRxState] >> RX_TASK_DECAY_SHIFT; + } + schedulerSetNextStateTime(rxStateDurationFracUs[rxState] >> RX_TASK_DECAY_SHIFT); } diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c index cf9f04935b..7a86267d57 100644 --- a/src/main/osd/osd.c +++ b/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); } diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index d1e552a943..a2e24de9c0 100644 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -356,6 +356,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; diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 2a8fbd662e..66330822ee 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -230,6 +230,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); From 4e47a792d8688b5ff2217c0f75262d282b68886c Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Wed, 12 Jan 2022 21:51:58 +0000 Subject: [PATCH 2/4] Add TASK_AGE_EXPEDITE_RX to ensure RX task is never starved of time --- src/main/scheduler/scheduler.c | 23 +++++++++++++++-------- src/main/scheduler/scheduler.h | 3 ++- src/test/unit/scheduler_unittest.cc | 2 +- 3 files changed, 18 insertions(+), 10 deletions(-) diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index a2e24de9c0..fc18976160 100644 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -92,6 +92,8 @@ static FAST_DATA_ZERO_INIT bool gyroEnabled; static int32_t desiredPeriodCycles; static uint32_t lastTargetCycles; +static uint8_t skippedRxAttempts = 0; + #if defined(USE_LATE_TASK_STATISTICS) static int16_t lateTaskCount = 0; static uint32_t lateTaskTotal = 0; @@ -569,8 +571,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->dynamicPriority = 1 + task->attr->staticPriority * task->taskAgeCycles; + task->taskAgePeriods = 1 + (cmpTimeUs(currentTimeUs, task->lastSignaledAtUs) / task->attr->desiredPeriodUs); + 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) @@ -581,17 +583,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); - if (task->taskAgeCycles > 0) { - task->dynamicPriority = 1 + task->attr->staticPriority * task->taskAgeCycles; + task->taskAgePeriods = (cmpTimeUs(currentTimeUs, task->lastExecutedAtUs) / task->attr->desiredPeriodUs); + if (task->taskAgePeriods > 0) { + task->dynamicPriority = 1 + task->attr->staticPriority * task->taskAgePeriods; } } @@ -633,6 +635,10 @@ FAST_CODE void scheduler(void) } #endif // USE_LATE_TASK_STATISTICS + if ((currentTask - tasks) == TASK_RX) { + skippedRxAttempts = 0; + } + if ((cyclesOverdue > 0) || (-cyclesOverdue < taskGuardMinCycles)) { if (taskGuardCycles < taskGuardMaxCycles) { taskGuardCycles += taskGuardDeltaUpCycles; @@ -643,7 +649,8 @@ 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) || + (((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; diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 66330822ee..b3678649b3 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -47,6 +47,7 @@ // 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_COUNT 1 // Make aged tasks more schedulable #define TASK_AGE_EXPEDITE_SCALE 0.9 // By scaling their expected execution time @@ -197,7 +198,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 diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc index 172a0f73cb..bc20c0ce48 100644 --- a/src/test/unit/scheduler_unittest.cc +++ b/src/test/unit/scheduler_unittest.cc @@ -396,7 +396,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 From e7b61a928f0662a93fcbd38cd3a1e26a77c55e31 Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Fri, 14 Jan 2022 03:22:52 +0000 Subject: [PATCH 3/4] Still update duration of next anticipated state even is current execution time is ignored --- src/main/fc/tasks.c | 28 +++++++++++++--------------- src/main/osd/osd.c | 24 ++++++++++++++---------- 2 files changed, 27 insertions(+), 25 deletions(-) diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 463954b4e0..19021c65ce 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -225,23 +225,21 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs) break; } - if (schedulerGetIgnoreTaskExecTime()) { - return; - } + if (!schedulerGetIgnoreTaskExecTime()) { + executeTimeUs = micros() - currentTimeUs + RX_TASK_MARGIN; - 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 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]--; + if (executeTimeUs > (rxStateDurationFracUs[oldRxState] >> RX_TASK_DECAY_SHIFT)) { + rxStateDurationFracUs[oldRxState] = executeTimeUs << RX_TASK_DECAY_SHIFT; + } else { + // Slowly decay the max time + rxStateDurationFracUs[oldRxState]--; + } } if (debugMode == DEBUG_RX_STATE_TIME) { diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c index 7a86267d57..342e62fbda 100644 --- a/src/main/osd/osd.c +++ b/src/main/osd/osd.c @@ -1006,6 +1006,8 @@ void osdDrawStats2(timeUs_t currentTimeUs) osdStatsEnabled = false; stats.armed_time = 0; } + + schedulerIgnoreTaskExecTime(); } #ifdef USE_ESC_SENSOR if (featureIsEnabled(FEATURE_ESC_SENSOR)) { @@ -1305,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 - // rendered which is unrepresentative, so ignore - if (!firstPass) { - if (osdCurState == OSD_STATE_UPDATE_ELEMENTS) { - if (executeTimeUs > osdElementGroupDurationUs[osdCurElementGroup]) { - osdElementGroupDurationUs[osdCurElementGroup] = executeTimeUs; + // On the first pass no element groups will have been formed, so all elements will have been + // rendered which is unrepresentative, so ignore + if (!firstPass) { + if (osdCurState == OSD_STATE_UPDATE_ELEMENTS) { + if (executeTimeUs > osdElementGroupDurationUs[osdCurElementGroup]) { + osdElementGroupDurationUs[osdCurElementGroup] = executeTimeUs; + } } - } - if (executeTimeUs > osdStateDurationUs[osdCurState]) { - osdStateDurationUs[osdCurState] = executeTimeUs; + if (executeTimeUs > osdStateDurationUs[osdCurState]) { + osdStateDurationUs[osdCurState] = executeTimeUs; + } } } From db904b4a75393bb8e4f5f716bad89e7c5cfeaffc Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Fri, 14 Jan 2022 03:42:10 +0000 Subject: [PATCH 4/4] Add expediting of OSD task --- src/main/scheduler/scheduler.c | 11 +++++++++++ src/main/scheduler/scheduler.h | 1 + src/test/Makefile | 2 ++ src/test/unit/link_quality_unittest.cc | 1 + src/test/unit/osd_unittest.cc | 1 + src/test/unit/scheduler_unittest.cc | 11 +++++++++++ 6 files changed, 27 insertions(+) diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index fc18976160..c1540c8cf7 100644 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -93,6 +93,9 @@ 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; @@ -638,6 +641,11 @@ FAST_CODE void scheduler(void) 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) { @@ -650,6 +658,9 @@ FAST_CODE void scheduler(void) taskCount++; #endif // USE_LATE_TASK_STATISTICS } 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 diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index b3678649b3..ee679f6ec5 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -48,6 +48,7 @@ #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 diff --git a/src/test/Makefile b/src/test/Makefile index a2c105ed99..75c0e79be7 100644 --- a/src/test/Makefile +++ b/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 \ diff --git a/src/test/unit/link_quality_unittest.cc b/src/test/unit/link_quality_unittest.cc index 38399218cd..b564005fc0 100644 --- a/src/test/unit/link_quality_unittest.cc +++ b/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) {} diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 7439c30efd..8d1664b8af 100644 --- a/src/test/unit/osd_unittest.cc +++ b/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) {} } diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc index bc20c0ce48..cbd955e575 100644 --- a/src/test/unit/scheduler_unittest.cc +++ b/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, } };