1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00

Merge pull request #11033 from SteveCEvans/late_led_strip

Fix peaky task duration estimation
This commit is contained in:
ctzsnooze 2021-12-20 10:27:05 +11:00 committed by GitHub
commit db242cb161
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
23 changed files with 101 additions and 92 deletions

View file

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

View file

@ -109,7 +109,7 @@ PG_RESET_TEMPLATE(pilotConfig_t, pilotConfig,
.displayName = { 0 }, .displayName = { 0 },
); );
PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 2); PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 3);
PG_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
.pidProfileIndex = 0, .pidProfileIndex = 0,
@ -122,7 +122,6 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
.boardIdentifier = TARGET_BOARD_IDENTIFIER, .boardIdentifier = TARGET_BOARD_IDENTIFIER,
.hseMhz = SYSTEM_HSE_VALUE, // Only used for F4 and G4 targets .hseMhz = SYSTEM_HSE_VALUE, // Only used for F4 and G4 targets
.configurationState = CONFIGURATION_STATE_DEFAULTS_BARE, .configurationState = CONFIGURATION_STATE_DEFAULTS_BARE,
.schedulerOptimizeRate = SCHEDULER_OPTIMIZE_RATE_AUTO,
.enableStickArming = false, .enableStickArming = false,
); );
@ -157,7 +156,6 @@ void resetConfig(void)
static void activateConfig(void) static void activateConfig(void)
{ {
schedulerOptimizeRate(systemConfig()->schedulerOptimizeRate == SCHEDULER_OPTIMIZE_RATE_ON || (systemConfig()->schedulerOptimizeRate == SCHEDULER_OPTIMIZE_RATE_AUTO && motorConfig()->dev.useDshotTelemetry));
loadPidProfile(); loadPidProfile();
loadControlRateProfile(); loadControlRateProfile();
@ -513,7 +511,7 @@ static void validateAndFixConfig(void)
} }
} }
if ((!configuredMotorProtocolDshot || (motorConfig()->dev.useDshotBitbang == DSHOT_BITBANG_OFF && (motorConfig()->dev.useBurstDshot == DSHOT_DMAR_ON || nChannelTimerUsed)) || systemConfig()->schedulerOptimizeRate == SCHEDULER_OPTIMIZE_RATE_OFF) && motorConfig()->dev.useDshotTelemetry) { if ((!configuredMotorProtocolDshot || (motorConfig()->dev.useDshotBitbang == DSHOT_BITBANG_OFF && (motorConfig()->dev.useBurstDshot == DSHOT_DMAR_ON || nChannelTimerUsed))) && motorConfig()->dev.useDshotTelemetry) {
motorConfigMutable()->dev.useDshotTelemetry = false; motorConfigMutable()->dev.useDshotTelemetry = false;
} }
#endif // USE_DSHOT_TELEMETRY #endif // USE_DSHOT_TELEMETRY

View file

@ -33,12 +33,6 @@ typedef enum {
CONFIGURATION_STATE_CONFIGURED, CONFIGURATION_STATE_CONFIGURED,
} configurationState_e; } configurationState_e;
typedef enum {
SCHEDULER_OPTIMIZE_RATE_OFF = 0,
SCHEDULER_OPTIMIZE_RATE_ON,
SCHEDULER_OPTIMIZE_RATE_AUTO,
} schedulerOptimizeRate_e;
typedef struct pilotConfig_s { typedef struct pilotConfig_s {
char name[MAX_NAME_LENGTH + 1]; char name[MAX_NAME_LENGTH + 1];
char displayName[MAX_NAME_LENGTH + 1]; char displayName[MAX_NAME_LENGTH + 1];
@ -57,7 +51,6 @@ typedef struct systemConfig_s {
char boardIdentifier[sizeof(TARGET_BOARD_IDENTIFIER) + 1]; char boardIdentifier[sizeof(TARGET_BOARD_IDENTIFIER) + 1];
uint8_t hseMhz; // Only used for F4 and G4 targets uint8_t hseMhz; // Only used for F4 and G4 targets
uint8_t configurationState; // The state of the configuration (defaults / configured) uint8_t configurationState; // The state of the configuration (defaults / configured)
uint8_t schedulerOptimizeRate;
uint8_t enableStickArming; // boolean that determines whether stick arming can be used uint8_t enableStickArming; // boolean that determines whether stick arming can be used
} systemConfig_t; } systemConfig_t;

View file

@ -181,7 +181,7 @@ void ws2811UpdateStrip(ledStripFormatRGB_e ledFormat)
{ {
// don't wait - risk of infinite block, just get an update next time round // don't wait - risk of infinite block, just get an update next time round
if (!ws2811Initialised || ws2811LedDataTransferInProgress) { if (!ws2811Initialised || ws2811LedDataTransferInProgress) {
ignoreTaskStateTime(); schedulerIgnoreTaskStateTime();
return; return;
} }

View file

@ -30,6 +30,8 @@
#include "fc/dispatch.h" #include "fc/dispatch.h"
#include "scheduler/scheduler.h"
static dispatchEntry_t *head = NULL; static dispatchEntry_t *head = NULL;
static bool dispatchEnabled = false; static bool dispatchEnabled = false;
@ -43,10 +45,10 @@ void dispatchEnable(void)
dispatchEnabled = true; dispatchEnabled = true;
} }
void dispatchProcess(uint32_t currentTime) void dispatchProcess(uint32_t currentTimeUs)
{ {
for (dispatchEntry_t **p = &head; *p; ) { for (dispatchEntry_t **p = &head; *p; ) {
if (cmp32(currentTime, (*p)->delayedUntil) < 0) if (cmp32(currentTimeUs, (*p)->delayedUntil) < 0)
break; break;
// unlink entry first, so handler can replan self // unlink entry first, so handler can replan self
dispatchEntry_t *current = *p; dispatchEntry_t *current = *p;

View file

@ -113,8 +113,11 @@
#include "tasks.h" #include "tasks.h"
// taskUpdateRxMain() has occasional peaks in execution time so normal moving average duration estimation doesn't work
// Decay the estimated max task duration by 1/(1 << RX_TASK_DECAY_SHIFT) on every invocation
#define RX_TASK_DECAY_SHIFT 5
// Add a margin to the task duration estimation // Add a margin to the task duration estimation
#define RX_TASK_MARGIN 5 #define RX_TASK_MARGIN 1
static void taskMain(timeUs_t currentTimeUs) static void taskMain(timeUs_t currentTimeUs)
{ {
@ -179,14 +182,13 @@ bool taskUpdateRxMainInProgress()
static void taskUpdateRxMain(timeUs_t currentTimeUs) static void taskUpdateRxMain(timeUs_t currentTimeUs)
{ {
static timeUs_t rxStateDurationUs[RX_STATE_COUNT]; static timeUs_t rxStateDurationFracUs[RX_STATE_COUNT];
timeUs_t executeTimeUs; timeUs_t executeTimeUs;
timeUs_t existingDurationUs;
rxState_e oldRxState = rxState; rxState_e oldRxState = rxState;
// Where we are using a state machine call schedulerIgnoreTaskExecRate() for all states bar one // Where we are using a state machine call schedulerIgnoreTaskExecRate() for all states bar one
if (rxState != RX_STATE_UPDATE) { if (rxState != RX_STATE_UPDATE) {
ignoreTaskExecRate(); schedulerIgnoreTaskExecRate();
} }
switch (rxState) { switch (rxState) {
@ -222,22 +224,20 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
break; break;
} }
if (getIgnoreTaskExecTime()) { if (schedulerGetIgnoreTaskExecTime()) {
return; return;
} }
executeTimeUs = micros() - currentTimeUs; executeTimeUs = micros() - currentTimeUs + RX_TASK_MARGIN;
existingDurationUs = rxStateDurationUs[oldRxState] / TASK_STATS_MOVING_SUM_COUNT; if (executeTimeUs > (rxStateDurationFracUs[oldRxState] >> RX_TASK_DECAY_SHIFT)) {
rxStateDurationFracUs[oldRxState] = executeTimeUs << RX_TASK_DECAY_SHIFT;
// If the execution time is higher than expected, double the weight in the moving average } else {
if (executeTimeUs > existingDurationUs) { // Slowly decay the max time
rxStateDurationUs[oldRxState] += executeTimeUs - existingDurationUs; rxStateDurationFracUs[oldRxState]--;
} }
rxStateDurationUs[oldRxState] += executeTimeUs - existingDurationUs; schedulerSetNextStateTime(rxStateDurationFracUs[rxState] >> RX_TASK_DECAY_SHIFT);
schedulerSetNextStateTime((rxStateDurationUs[rxState] / TASK_STATS_MOVING_SUM_COUNT) + RX_TASK_MARGIN);
} }

View file

@ -593,7 +593,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
acc.accADC[X] = 0; acc.accADC[X] = 0;
acc.accADC[Y] = 0; acc.accADC[Y] = 0;
acc.accADC[Z] = 0; acc.accADC[Z] = 0;
ignoreTaskStateTime(); schedulerIgnoreTaskStateTime();
} }
} }
#endif // USE_ACC #endif // USE_ACC

View file

@ -96,7 +96,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
const uint32_t dTime = currentTimeUs - previousTimeUs; const uint32_t dTime = currentTimeUs - previousTimeUs;
if (dTime < BARO_UPDATE_FREQUENCY_40HZ) { if (dTime < BARO_UPDATE_FREQUENCY_40HZ) {
ignoreTaskExecTime(); schedulerIgnoreTaskExecTime();
return; return;
} }
previousTimeUs = currentTimeUs; previousTimeUs = currentTimeUs;

View file

@ -49,6 +49,8 @@
#include "pg/beeper.h" #include "pg/beeper.h"
#include "scheduler/scheduler.h"
#include "sensors/battery.h" #include "sensors/battery.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
@ -390,6 +392,7 @@ void beeperUpdate(timeUs_t currentTimeUs)
} }
if (beeperNextToggleTime > currentTimeUs) { if (beeperNextToggleTime > currentTimeUs) {
schedulerIgnoreTaskExecTime();
return; return;
} }

View file

@ -1066,8 +1066,8 @@ static void applyStatusProfile(timeUs_t now) {
} }
if (!timActive) { if (!timActive) {
// Call ignoreTaskExecTime() unless data is being processed // Call schedulerIgnoreTaskExecTime() unless data is being processed
ignoreTaskExecTime(); schedulerIgnoreTaskExecTime();
return; // no change this update, keep old state return; // no change this update, keep old state
} }
@ -1248,13 +1248,11 @@ static void applySimpleProfile(timeUs_t currentTimeUs)
void ledStripUpdate(timeUs_t currentTimeUs) void ledStripUpdate(timeUs_t currentTimeUs)
{ {
#ifndef USE_LED_STRIP_STATUS_MODE
UNUSED(currentTimeUs); UNUSED(currentTimeUs);
#endif
if (!isWS2811LedStripReady()) { if (!isWS2811LedStripReady()) {
// Call ignoreTaskExecTime() unless data is being processed // Call schedulerIgnoreTaskExecTime() unless data is being processed
ignoreTaskExecTime(); schedulerIgnoreTaskExecTime();
return; return;
} }
@ -1282,8 +1280,8 @@ void ledStripUpdate(timeUs_t currentTimeUs)
break; break;
} }
} else { } else {
// Call ignoreTaskExecTime() unless data is being processed // Call schedulerIgnoreTaskExecTime() unless data is being processed
ignoreTaskExecTime(); schedulerIgnoreTaskExecTime();
} }
} }

View file

@ -2889,7 +2889,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
// This is going to take some time and won't be done where real-time performance is needed so // This is going to take some time and won't be done where real-time performance is needed so
// ignore how long it takes to avoid confusing the scheduler // ignore how long it takes to avoid confusing the scheduler
ignoreTaskStateTime(); schedulerIgnoreTaskStateTime();
writeEEPROM(); writeEEPROM();
readEEPROM(); readEEPROM();

View file

@ -195,6 +195,10 @@ const osd_stats_e osdStatsDisplayOrder[OSD_STAT_COUNT] = {
// Allow a margin by which a group render can exceed that of the sum of the elements before declaring insane // Allow a margin by which a group render can exceed that of the sum of the elements before declaring insane
// This will most likely be violated by a USB interrupt whilst using the CLI // This will most likely be violated by a USB interrupt whilst using the CLI
#define OSD_ELEMENT_RENDER_GROUP_MARGIN 5 #define OSD_ELEMENT_RENDER_GROUP_MARGIN 5
// Safe margin when rendering elements
#define OSD_ELEMENT_RENDER_MARGIN 5
// Safe margin in other states
#define OSD_MARGIN 2
// Format a float to the specified number of decimal places with optional rounding. // Format a float to the specified number of decimal places with optional rounding.
// OSD symbols can optionally be placed before and after the formatted number (use SYM_NONE for no symbol). // OSD symbols can optionally be placed before and after the formatted number (use SYM_NONE for no symbol).
@ -1073,7 +1077,7 @@ void osdUpdate(timeUs_t currentTimeUs)
osdState_e osdCurState = osdState; osdState_e osdCurState = osdState;
if (osdState != OSD_STATE_UPDATE_CANVAS) { if (osdState != OSD_STATE_UPDATE_CANVAS) {
ignoreTaskExecRate(); schedulerIgnoreTaskExecRate();
} }
switch (osdState) { switch (osdState) {
@ -1083,7 +1087,7 @@ void osdUpdate(timeUs_t currentTimeUs)
if (osdDisplayPortDeviceType == OSD_DISPLAYPORT_DEVICE_FRSKYOSD) { if (osdDisplayPortDeviceType == OSD_DISPLAYPORT_DEVICE_FRSKYOSD) {
displayRedraw(osdDisplayPort); displayRedraw(osdDisplayPort);
} else { } else {
ignoreTaskExecTime(); schedulerIgnoreTaskExecTime();
} }
return; return;
} }
@ -1309,14 +1313,14 @@ void osdUpdate(timeUs_t currentTimeUs)
} }
if (osdState == OSD_STATE_UPDATE_ELEMENTS) { if (osdState == OSD_STATE_UPDATE_ELEMENTS) {
schedulerSetNextStateTime(osdElementGroupDurationUs[osdElementGroup]); schedulerSetNextStateTime(osdElementGroupDurationUs[osdElementGroup] + OSD_ELEMENT_RENDER_MARGIN);
} else { } else {
if (osdState == OSD_STATE_IDLE) { if (osdState == OSD_STATE_IDLE) {
schedulerSetNextStateTime(osdStateDurationUs[OSD_STATE_CHECK]); schedulerSetNextStateTime(osdStateDurationUs[OSD_STATE_CHECK] + OSD_MARGIN);
} else { } else {
schedulerSetNextStateTime(osdStateDurationUs[osdState]); schedulerSetNextStateTime(osdStateDurationUs[osdState] + OSD_MARGIN);
} }
ignoreTaskExecTime(); schedulerIgnoreTaskExecTime();
} }
} }

View file

@ -179,6 +179,8 @@ void taskSystemLoad(timeUs_t currentTimeUs)
averageSystemLoadPercent = 100 * taskTotalExecutionTime / deltaTime; averageSystemLoadPercent = 100 * taskTotalExecutionTime / deltaTime;
taskTotalExecutionTime = 0; taskTotalExecutionTime = 0;
lastExecutedAtUs = currentTimeUs; lastExecutedAtUs = currentTimeUs;
} else {
schedulerIgnoreTaskExecTime();
} }
#if defined(SIMULATOR_BUILD) #if defined(SIMULATOR_BUILD)
@ -208,7 +210,7 @@ void getTaskInfo(taskId_e taskId, taskInfo_t * taskInfo)
taskInfo->subTaskName = getTask(taskId)->subTaskName; taskInfo->subTaskName = getTask(taskId)->subTaskName;
taskInfo->maxExecutionTimeUs = getTask(taskId)->maxExecutionTimeUs; taskInfo->maxExecutionTimeUs = getTask(taskId)->maxExecutionTimeUs;
taskInfo->totalExecutionTimeUs = getTask(taskId)->totalExecutionTimeUs; taskInfo->totalExecutionTimeUs = getTask(taskId)->totalExecutionTimeUs;
taskInfo->averageExecutionTimeUs = getTask(taskId)->anticipatedExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT; taskInfo->averageExecutionTimeUs = getTask(taskId)->anticipatedExecutionTime >> TASK_EXEC_TIME_SHIFT;
taskInfo->averageDeltaTimeUs = getTask(taskId)->movingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT; taskInfo->averageDeltaTimeUs = getTask(taskId)->movingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
taskInfo->latestDeltaTimeUs = getTask(taskId)->taskLatestDeltaTimeUs; taskInfo->latestDeltaTimeUs = getTask(taskId)->taskLatestDeltaTimeUs;
taskInfo->movingAverageCycleTimeUs = getTask(taskId)->movingAverageCycleTimeUs; taskInfo->movingAverageCycleTimeUs = getTask(taskId)->movingAverageCycleTimeUs;
@ -262,25 +264,25 @@ timeDelta_t getTaskDeltaTimeUs(taskId_e taskId)
} }
// Called by tasks executing what are known to be short states // Called by tasks executing what are known to be short states
void ignoreTaskStateTime() void schedulerIgnoreTaskStateTime()
{ {
ignoreCurrentTaskExecRate = true; ignoreCurrentTaskExecRate = true;
ignoreCurrentTaskExecTime = true; ignoreCurrentTaskExecTime = true;
} }
// Called by tasks with state machines to only count one state as determining rate // Called by tasks with state machines to only count one state as determining rate
void ignoreTaskExecRate() void schedulerIgnoreTaskExecRate()
{ {
ignoreCurrentTaskExecRate = true; ignoreCurrentTaskExecRate = true;
} }
// Called by tasks without state machines executing in what is known to be a shorter time than peak // Called by tasks without state machines executing in what is known to be a shorter time than peak
void ignoreTaskExecTime() void schedulerIgnoreTaskExecTime()
{ {
ignoreCurrentTaskExecTime = true; ignoreCurrentTaskExecTime = true;
} }
bool getIgnoreTaskExecTime() bool schedulerGetIgnoreTaskExecTime()
{ {
return ignoreCurrentTaskExecTime; return ignoreCurrentTaskExecTime;
} }
@ -288,12 +290,12 @@ bool getIgnoreTaskExecTime()
void schedulerResetTaskStatistics(taskId_e taskId) void schedulerResetTaskStatistics(taskId_e taskId)
{ {
if (taskId == TASK_SELF) { if (taskId == TASK_SELF) {
currentTask->anticipatedExecutionTimeUs = 0; currentTask->anticipatedExecutionTime = 0;
currentTask->movingSumDeltaTimeUs = 0; currentTask->movingSumDeltaTimeUs = 0;
currentTask->totalExecutionTimeUs = 0; currentTask->totalExecutionTimeUs = 0;
currentTask->maxExecutionTimeUs = 0; currentTask->maxExecutionTimeUs = 0;
} else if (taskId < TASK_COUNT) { } else if (taskId < TASK_COUNT) {
getTask(taskId)->anticipatedExecutionTimeUs = 0; getTask(taskId)->anticipatedExecutionTime = 0;
getTask(taskId)->movingSumDeltaTimeUs = 0; getTask(taskId)->movingSumDeltaTimeUs = 0;
getTask(taskId)->totalExecutionTimeUs = 0; getTask(taskId)->totalExecutionTimeUs = 0;
getTask(taskId)->maxExecutionTimeUs = 0; getTask(taskId)->maxExecutionTimeUs = 0;
@ -343,11 +345,10 @@ void schedulerInit(void)
#if defined(USE_LATE_TASK_STATISTICS) #if defined(USE_LATE_TASK_STATISTICS)
nextTimingCycles = lastTargetCycles; nextTimingCycles = lastTargetCycles;
#endif #endif
}
void schedulerOptimizeRate(bool optimizeRate) for (taskId_e taskId = 0; taskId < TASK_COUNT; taskId++) {
{ schedulerResetTaskStatistics(taskId);
optimizeSchedRate = optimizeRate; }
} }
inline static timeUs_t getPeriodCalculationBasis(const task_t* task) inline static timeUs_t getPeriodCalculationBasis(const task_t* task)
@ -393,9 +394,14 @@ FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTi
// Update estimate of expected task duration // Update estimate of expected task duration
if (taskNextStateTime != -1) { if (taskNextStateTime != -1) {
selectedTask->anticipatedExecutionTimeUs = taskNextStateTime * TASK_STATS_MOVING_SUM_COUNT; selectedTask->anticipatedExecutionTime = taskNextStateTime << TASK_EXEC_TIME_SHIFT;
} else if (!ignoreCurrentTaskExecTime) { } else if (!ignoreCurrentTaskExecTime) {
selectedTask->anticipatedExecutionTimeUs += taskExecutionTimeUs - selectedTask->anticipatedExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT; if (taskExecutionTimeUs > (selectedTask->anticipatedExecutionTime >> TASK_EXEC_TIME_SHIFT)) {
selectedTask->anticipatedExecutionTime = taskExecutionTimeUs << TASK_EXEC_TIME_SHIFT;
} else if (selectedTask->anticipatedExecutionTime > 1) {
// Slowly decay the max time
selectedTask->anticipatedExecutionTime--;
}
} }
if (!ignoreCurrentTaskExecTime) { if (!ignoreCurrentTaskExecTime) {
@ -602,7 +608,7 @@ FAST_CODE void scheduler(void)
} }
if (selectedTask) { if (selectedTask) {
timeDelta_t taskRequiredTimeUs = selectedTask->anticipatedExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT; timeDelta_t taskRequiredTimeUs = selectedTask->anticipatedExecutionTime >> TASK_EXEC_TIME_SHIFT;
#if defined(USE_LATE_TASK_STATISTICS) #if defined(USE_LATE_TASK_STATISTICS)
selectedTask->execTime = taskRequiredTimeUs; selectedTask->execTime = taskRequiredTimeUs;
#endif #endif
@ -636,7 +642,7 @@ FAST_CODE void scheduler(void)
if (taskGuardCycles < taskGuardMaxCycles) { if (taskGuardCycles < taskGuardMaxCycles) {
taskGuardCycles += taskGuardDeltaUpCycles; taskGuardCycles += taskGuardDeltaUpCycles;
} }
} else if (taskGuardCycles > taskGuardMinCycles) { } else if (taskGuardCycles > taskGuardMinCycles) {
taskGuardCycles -= taskGuardDeltaDownCycles; taskGuardCycles -= taskGuardDeltaDownCycles;
} }
#if defined(USE_LATE_TASK_STATISTICS) #if defined(USE_LATE_TASK_STATISTICS)
@ -645,7 +651,7 @@ FAST_CODE void scheduler(void)
} else if (selectedTask->taskAgeCycles > TASK_AGE_EXPEDITE_COUNT) { } else if (selectedTask->taskAgeCycles > TASK_AGE_EXPEDITE_COUNT) {
// If a task has been unable to run, then reduce it's recorded estimated run time to ensure // If a task has been unable to run, then reduce it's recorded estimated run time to ensure
// it's ultimate scheduling // it's ultimate scheduling
selectedTask->anticipatedExecutionTimeUs *= TASK_AGE_EXPEDITE_SCALE; selectedTask->anticipatedExecutionTime *= TASK_AGE_EXPEDITE_SCALE;
} }
} }
} }

View file

@ -27,22 +27,26 @@
#define TASK_PERIOD_MS(ms) ((ms) * 1000) #define TASK_PERIOD_MS(ms) ((ms) * 1000)
#define TASK_PERIOD_US(us) (us) #define TASK_PERIOD_US(us) (us)
#define TASK_STATS_MOVING_SUM_COUNT 32 #define TASK_STATS_MOVING_SUM_COUNT 64
#define LOAD_PERCENTAGE_ONE 100 #define LOAD_PERCENTAGE_ONE 100
#define SCHED_START_LOOP_MIN_US 4 // Wait at start of scheduler loop if gyroTask is nearly due #define SCHED_START_LOOP_MIN_US 1 // Wait at start of scheduler loop if gyroTask is nearly due
#define SCHED_START_LOOP_MAX_US 12 #define SCHED_START_LOOP_MAX_US 12
#define SCHED_START_LOOP_DOWN_STEP 50 // Fraction of a us to reduce start loop wait #define SCHED_START_LOOP_DOWN_STEP 50 // Fraction of a us to reduce start loop wait
#define SCHED_START_LOOP_UP_STEP 1 // Fraction of a us to increase start loop wait #define SCHED_START_LOOP_UP_STEP 1 // Fraction of a us to increase start loop wait
#define TASK_GUARD_MARGIN_MIN_US 3 // Add an amount to the estimate of a task duration #define TASK_GUARD_MARGIN_MIN_US 1 // Add an amount to the estimate of a task duration
#define TASK_GUARD_MARGIN_MAX_US 5 #define TASK_GUARD_MARGIN_MAX_US 6
#define TASK_GUARD_MARGIN_DOWN_STEP 50 // Fraction of a us to reduce task guard margin #define TASK_GUARD_MARGIN_DOWN_STEP 50 // Fraction of a us to reduce task guard margin
#define TASK_GUARD_MARGIN_UP_STEP 1 // Fraction of a us to increase task guard margin #define TASK_GUARD_MARGIN_UP_STEP 1 // Fraction of a us to increase task guard margin
#define CHECK_GUARD_MARGIN_US 2 // Add a margin to the amount of time allowed for a check function to run #define CHECK_GUARD_MARGIN_US 2 // Add a margin to the amount of time allowed for a check function to run
// Some tasks have occasional peaks in execution time so normal moving average duration estimation doesn't work
// 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_COUNT 1 // Make aged tasks more schedulable #define TASK_AGE_EXPEDITE_COUNT 1 // Make aged tasks more schedulable
#define TASK_AGE_EXPEDITE_SCALE 0.9 // By scaling their expected execution time #define TASK_AGE_EXPEDITE_SCALE 0.9 // By scaling their expected execution time
@ -196,7 +200,7 @@ typedef struct {
// Statistics // Statistics
float movingAverageCycleTimeUs; float movingAverageCycleTimeUs;
timeUs_t anticipatedExecutionTimeUs; // moving sum over 32 samples timeUs_t anticipatedExecutionTime; // Fixed point expectation of next execution time
timeUs_t movingSumDeltaTimeUs; // moving sum over 32 samples timeUs_t movingSumDeltaTimeUs; // moving sum over 32 samples
timeUs_t maxExecutionTimeUs; timeUs_t maxExecutionTimeUs;
timeUs_t totalExecutionTimeUs; // total time consumed by task since boot timeUs_t totalExecutionTimeUs; // total time consumed by task since boot
@ -213,21 +217,18 @@ void getTaskInfo(taskId_e taskId, taskInfo_t *taskInfo);
void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs); void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs);
void setTaskEnabled(taskId_e taskId, bool newEnabledState); void setTaskEnabled(taskId_e taskId, bool newEnabledState);
timeDelta_t getTaskDeltaTimeUs(taskId_e taskId); timeDelta_t getTaskDeltaTimeUs(taskId_e taskId);
void ignoreTaskStateTime(); void schedulerIgnoreTaskStateTime();
void ignoreTaskExecRate(); void schedulerIgnoreTaskExecRate();
void ignoreTaskExecTime(); void schedulerIgnoreTaskExecTime();
bool getIgnoreTaskExecTime(); bool schedulerGetIgnoreTaskExecTime();
void schedulerResetTaskStatistics(taskId_e taskId); void schedulerResetTaskStatistics(taskId_e taskId);
void schedulerResetTaskMaxExecutionTime(taskId_e taskId); void schedulerResetTaskMaxExecutionTime(taskId_e taskId);
void schedulerResetCheckFunctionMaxExecutionTime(void); void schedulerResetCheckFunctionMaxExecutionTime(void);
void schedulerSetNextStateTime(timeDelta_t nextStateTime); void schedulerSetNextStateTime(timeDelta_t nextStateTime);
void schedulerInit(void); void schedulerInit(void);
void scheduler(void); void scheduler(void);
timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTimeUs); timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTimeUs);
void taskSystemLoad(timeUs_t currentTimeUs); void taskSystemLoad(timeUs_t currentTimeUs);
void schedulerOptimizeRate(bool optimizeRate);
void schedulerEnableGyro(void); void schedulerEnableGyro(void);
uint16_t getAverageSystemLoadPercent(void); uint16_t getAverageSystemLoadPercent(void);
float schedulerGetCycleTimeMultiplier(void); float schedulerGetCycleTimeMultiplier(void);

View file

@ -388,14 +388,14 @@ uint32_t baroUpdate(timeUs_t currentTimeUs)
DEBUG_SET(DEBUG_BARO, 0, state); DEBUG_SET(DEBUG_BARO, 0, state);
// Where we are using a state machine call ignoreTaskExecRate() for all states bar one // Where we are using a state machine call schedulerIgnoreTaskExecRate() for all states bar one
if (state != BARO_STATE_TEMPERATURE_START) { if (state != BARO_STATE_TEMPERATURE_START) {
ignoreTaskExecRate(); schedulerIgnoreTaskExecRate();
} }
if (busBusy(&baro.dev.dev, NULL)) { if (busBusy(&baro.dev.dev, NULL)) {
// If the bus is busy, simply return to have another go later // If the bus is busy, simply return to have another go later
ignoreTaskStateTime(); schedulerIgnoreTaskStateTime();
return sleepTime; return sleepTime;
} }
@ -429,7 +429,7 @@ uint32_t baroUpdate(timeUs_t currentTimeUs)
if (baro.dev.read_up(&baro.dev)) { if (baro.dev.read_up(&baro.dev)) {
state = BARO_STATE_PRESSURE_SAMPLE; state = BARO_STATE_PRESSURE_SAMPLE;
} else { } else {
ignoreTaskStateTime(); schedulerIgnoreTaskStateTime();
} }
break; break;

View file

@ -45,6 +45,8 @@
#include "pg/pg.h" #include "pg/pg.h"
#include "pg/pg_ids.h" #include "pg/pg_ids.h"
#include "scheduler/scheduler.h"
#include "sensors/battery.h" #include "sensors/battery.h"
/** /**
@ -409,12 +411,10 @@ void batteryInit(void)
default: default:
break; break;
} }
} }
void batteryUpdateCurrentMeter(timeUs_t currentTimeUs) void batteryUpdateCurrentMeter(timeUs_t currentTimeUs)
{ {
UNUSED(currentTimeUs);
if (batteryCellCount == 0) { if (batteryCellCount == 0) {
currentMeterReset(&currentMeter); currentMeterReset(&currentMeter);
return; return;

View file

@ -1108,4 +1108,5 @@ extern "C" {
uint16_t getAverageSystemLoadPercent(void) { return 0; } uint16_t getAverageSystemLoadPercent(void) { return 0; }
bool isMotorProtocolEnabled(void) { return true; } bool isMotorProtocolEnabled(void) { return true; }
void pinioBoxTaskControl(void) {} void pinioBoxTaskControl(void) {}
void schedulerSetNextStateTime(timeDelta_t) {}
} }

View file

@ -250,6 +250,8 @@ void mixerSetThrottleAngleCorrection(int) {};
bool gpsRescueIsRunning(void) { return false; } bool gpsRescueIsRunning(void) { return false; }
bool isFixedWing(void) { return false; } bool isFixedWing(void) { return false; }
void pinioBoxTaskControl(void) {} void pinioBoxTaskControl(void) {}
void ignoreTaskExecTime(void) {} void schedulerIgnoreTaskExecTime(void) {}
void ignoreTaskStateTime(void) {} void schedulerIgnoreTaskStateTime(void) {}
void schedulerSetNextStateTime(timeDelta_t) {}
bool schedulerGetIgnoreTaskExecTime() { return false; }
} }

View file

@ -397,5 +397,7 @@ void ws2811LedStripEnable(void) { }
void setUsedLedCount(unsigned) { } void setUsedLedCount(unsigned) { }
void pinioBoxTaskControl(void) {} void pinioBoxTaskControl(void) {}
void ignoreTaskExecTime(void) {} void schedulerIgnoreTaskExecTime(void) {}
bool schedulerGetIgnoreTaskExecTime() { return false; }
void schedulerSetNextStateTime(timeDelta_t) {}
} }

View file

@ -510,9 +510,9 @@ extern "C" {
void failsafeOnValidDataFailed(void) { } void failsafeOnValidDataFailed(void) { }
void pinioBoxTaskControl(void) { } void pinioBoxTaskControl(void) { }
bool taskUpdateRxMainInProgress() { return true; } bool taskUpdateRxMainInProgress() { return true; }
void ignoreTaskStateTime(void) { } void schedulerIgnoreTaskStateTime(void) { }
void ignoreTaskExecRate(void) { } void schedulerIgnoreTaskExecRate(void) { }
void ignoreTaskExecTime(void) { } void schedulerIgnoreTaskExecTime(void) { }
void schedulerSetNextStateTime(timeDelta_t) {} void schedulerSetNextStateTime(timeDelta_t) {}
void rxPwmInit(rxRuntimeState_t *rxRuntimeState, rcReadRawDataFnPtr *callback) void rxPwmInit(rxRuntimeState_t *rxRuntimeState, rcReadRawDataFnPtr *callback)

View file

@ -1376,8 +1376,8 @@ extern "C" {
bool isUpright(void) { return true; } bool isUpright(void) { return true; }
float getMotorOutputLow(void) { return 1000.0; } float getMotorOutputLow(void) { return 1000.0; }
float getMotorOutputHigh(void) { return 2047.0; } float getMotorOutputHigh(void) { return 2047.0; }
void ignoreTaskStateTime(void) { } void schedulerIgnoreTaskStateTime(void) { }
void ignoreTaskExecRate(void) { } void schedulerIgnoreTaskExecRate(void) { }
void ignoreTaskExecTime(void) { } void schedulerIgnoreTaskExecTime(void) { }
void schedulerSetNextStateTime(timeDelta_t) {} void schedulerSetNextStateTime(timeDelta_t) {}
} }

View file

@ -197,5 +197,5 @@ extern "C" {
void sbufWriteU8(sbuf_t *, uint8_t) {} void sbufWriteU8(sbuf_t *, uint8_t) {}
void sbufWriteU16(sbuf_t *, uint16_t) {} void sbufWriteU16(sbuf_t *, uint16_t) {}
void sbufWriteU32(sbuf_t *, uint32_t) {} void sbufWriteU32(sbuf_t *, uint32_t) {}
void schedulerSetNextStateTime(timeDelta_t) {}
} }

View file

@ -32,8 +32,8 @@ extern "C" {
extern "C" { extern "C" {
void updateLEDDMABuffer(ledStripFormatRGB_e ledFormat, rgbColor24bpp_t *color, unsigned ledIndex); void updateLEDDMABuffer(ledStripFormatRGB_e ledFormat, rgbColor24bpp_t *color, unsigned ledIndex);
void ignoreTaskExecTime(void) {} void schedulerIgnoreTaskExecTime(void) {}
void ignoreTaskStateTime(void) {} void schedulerIgnoreTaskStateTime(void) {}
} }
TEST(WS2812, updateDMABuffer) { TEST(WS2812, updateDMABuffer) {