diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 3d95fadc2..758027b94 100644 --- a/src/main/fc/config.c +++ b/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(); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 168b52c5e..8413f38dd 100644 --- a/src/main/fc/config.h +++ b/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); diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index df78f5f3a..da5cadf9f 100644 --- a/src/main/interface/cli.c +++ b/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); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index faff32989..b6b8d26f2 100644 --- a/src/main/interface/settings.c +++ b/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 diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index e665eeb9e..d7308b5be 100644 --- a/src/main/scheduler/scheduler.c +++ b/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 { diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index c7de1b714..2260c903e 100644 --- a/src/main/scheduler/scheduler.h +++ b/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