Merge pull request #7331 from joelucid/scheduler_fix

Schedule time based tasks such that average frequency approaches 1/de…

src/main/fc/config.c

@@ -62,6 +62,8 @@
 #include "sensors/battery.h"
 #include "sensors/gyro.h"
 
+#include "scheduler/scheduler.h"
+
 pidProfile_t *currentPidProfile;
 
 #ifndef RX_SPI_DEFAULT_PROTOCOL
@@ -88,6 +90,7 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
     .boardIdentifier = TARGET_BOARD_IDENTIFIER,
     .hseMhz = SYSTEM_HSE_VALUE,  // Not used for non-F4 targets
     .configured = false,
+    .schedulerOptimizeRate = false,
 );
 
 uint8_t getCurrentPidProfileIndex(void)
@@ -121,6 +124,7 @@ void resetConfigs(void)
 
 static void activateConfig(void)
 {
+    schedulerOptimizeRate(systemConfig()->schedulerOptimizeRate);
     loadPidProfile();
     loadControlRateProfile();
 

src/main/fc/config.h

@@ -44,6 +44,7 @@ typedef struct systemConfig_s {
     char boardIdentifier[sizeof(TARGET_BOARD_IDENTIFIER) + 1];
     uint8_t hseMhz; // Not used for non-F4 targets
     uint8_t configured;
+    uint8_t schedulerOptimizeRate;
 } systemConfig_t;
 
 PG_DECLARE(systemConfig_t, systemConfig);

src/main/interface/cli.c

@@ -3766,7 +3766,7 @@ static void cliTasks(char *cmdline)
             int taskFrequency;
             int subTaskFrequency = 0;
             if (taskId == TASK_GYROPID) {
-                subTaskFrequency = taskInfo.latestDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)taskInfo.latestDeltaTime));
+                subTaskFrequency = taskInfo.movingAverageCycleTime == 0.0f ? 0.0f : (int)(1000000.0f / (taskInfo.movingAverageCycleTime));
                 taskFrequency = subTaskFrequency / pidConfig()->pid_process_denom;
                 if (pidConfig()->pid_process_denom > 1) {
                     cliPrintf("%02d - (%15s) ", taskId, taskInfo.taskName);

src/main/interface/settings.c

@@ -1175,6 +1175,7 @@ const clivalue_t valueTable[] = {
     { "cpu_overclock",              VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OVERCLOCK }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, cpu_overclock) },
 #endif
     { "pwr_on_arm_grace",           VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, 30 }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, powerOnArmingGraceTime) },
+    { "scheduler_optimize_rate",    VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, schedulerOptimizeRate) },
 
 // PG_VTX_CONFIG
 #ifdef USE_VTX_COMMON

src/main/scheduler/scheduler.c

@@ -53,10 +53,11 @@ static FAST_RAM_ZERO_INIT uint32_t totalWaitingTasksSamples;
 static FAST_RAM_ZERO_INIT bool calculateTaskStatistics;
 FAST_RAM_ZERO_INIT uint16_t averageSystemLoadPercent = 0;
 
-
 static FAST_RAM_ZERO_INIT int taskQueuePos = 0;
 STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT int taskQueueSize = 0;
 
+static FAST_RAM int periodCalculationBasisOffset = offsetof(cfTask_t, lastExecutedAt);
+
 // No need for a linked list for the queue, since items are only inserted at startup
 
 STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT cfTask_t* taskQueueArray[TASK_COUNT + 1]; // extra item for NULL pointer at end of queue
@@ -164,6 +165,7 @@ void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
     taskInfo->totalExecutionTime = cfTasks[taskId].totalExecutionTime;
     taskInfo->averageExecutionTime = cfTasks[taskId].movingSumExecutionTime / MOVING_SUM_COUNT;
     taskInfo->latestDeltaTime = cfTasks[taskId].taskLatestDeltaTime;
+    taskInfo->movingAverageCycleTime = cfTasks[taskId].movingAverageCycleTime;
 #endif
 }
 
@@ -243,6 +245,16 @@ void schedulerInit(void)
     queueAdd(&cfTasks[TASK_SYSTEM]);
 }
 
+void schedulerOptimizeRate(bool optimizeRate)
+{
+    periodCalculationBasisOffset = optimizeRate ? offsetof(cfTask_t, lastDesiredAt) : offsetof(cfTask_t, lastExecutedAt);
+}
+
+inline static timeUs_t getPeriodCalculationBasis(const cfTask_t* task)
+{
+    return *(timeUs_t*)((uint8_t*)task + periodCalculationBasisOffset);
+}
+
 FAST_CODE void scheduler(void)
 {
     // Cache currentTime
@@ -251,7 +263,7 @@ FAST_CODE void scheduler(void)
     // Check for realtime tasks
     bool outsideRealtimeGuardInterval = true;
     for (const cfTask_t *task = queueFirst(); task != NULL && task->staticPriority >= TASK_PRIORITY_REALTIME; task = queueNext()) {
-        const timeUs_t nextExecuteAt = task->lastExecutedAt + task->desiredPeriod;
+        const timeUs_t nextExecuteAt = getPeriodCalculationBasis(task) + task->desiredPeriod;
         if ((timeDelta_t)(currentTimeUs - nextExecuteAt) >= 0) {
             outsideRealtimeGuardInterval = false;
             break;
@@ -299,7 +311,7 @@ FAST_CODE void scheduler(void)
         } else {
             // Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods)
             // Task age is calculated from last execution
-            task->taskAgeCycles = ((currentTimeUs - task->lastExecutedAt) / task->desiredPeriod);
+            task->taskAgeCycles = ((currentTimeUs - getPeriodCalculationBasis(task)) / task->desiredPeriod);
             if (task->taskAgeCycles > 0) {
                 task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
                 waitingTasks++;
@@ -326,7 +338,9 @@ FAST_CODE void scheduler(void)
     if (selectedTask) {
         // Found a task that should be run
         selectedTask->taskLatestDeltaTime = currentTimeUs - selectedTask->lastExecutedAt;
+        float period = currentTimeUs - selectedTask->lastExecutedAt;
         selectedTask->lastExecutedAt = currentTimeUs;
+        selectedTask->lastDesiredAt += (cmpTimeUs(currentTimeUs, selectedTask->lastDesiredAt) / selectedTask->desiredPeriod) * selectedTask->desiredPeriod;
         selectedTask->dynamicPriority = 0;
 
         // Execute task
@@ -338,6 +352,7 @@ FAST_CODE void scheduler(void)
             selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / MOVING_SUM_COUNT;
             selectedTask->totalExecutionTime += taskExecutionTime;   // time consumed by scheduler + task
             selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
+            selectedTask->movingAverageCycleTime += 0.05f * (period - selectedTask->movingAverageCycleTime);
         } else
 #endif
         {

src/main/scheduler/scheduler.h

@@ -21,6 +21,7 @@
 #pragma once
 
 #include "common/time.h"
+#include "fc/config.h"
 
 #define TASK_PERIOD_HZ(hz) (1000000 / (hz))
 #define TASK_PERIOD_MS(ms) ((ms) * 1000)
@@ -53,6 +54,7 @@ typedef struct {
     timeUs_t     maxExecutionTime;
     timeUs_t     totalExecutionTime;
     timeUs_t     averageExecutionTime;
+    float        movingAverageCycleTime;
 } cfTaskInfo_t;
 
 typedef enum {
@@ -157,9 +159,11 @@ typedef struct {
     timeDelta_t taskLatestDeltaTime;
     timeUs_t lastExecutedAt;        // last time of invocation
     timeUs_t lastSignaledAt;        // time of invocation event for event-driven tasks
+    timeUs_t lastDesiredAt;         // time of last desired execution
 
 #if defined(USE_TASK_STATISTICS)
     // Statistics
+    float    movingAverageCycleTime;
     timeUs_t movingSumExecutionTime;  // moving sum over 32 samples
     timeUs_t maxExecutionTime;
     timeUs_t totalExecutionTime;    // total time consumed by task since boot
@@ -181,6 +185,7 @@ void schedulerResetTaskMaxExecutionTime(cfTaskId_e taskId);
 void schedulerInit(void);
 void scheduler(void);
 void taskSystemLoad(timeUs_t currentTime);
+void schedulerOptimizeRate(bool optimizeRate);
 
 #define LOAD_PERCENTAGE_ONE 100