Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 08:15:30 +03:00
Code Issues Wiki Activity

Split initialised/non-initialised task data

Browse source
This commit is contained in:
Steve Evans 2021-12-27 23:53:47 +00:00
parent 2d770f747e
commit 9bdf9c11e9
4 changed files with 167 additions and 155 deletions
Download patch file Download diff file Expand all files Collapse all files

242
src/main/fc/tasks.c
View file

@ -299,8 +299,133 @@ static void taskCameraControl(uint32_t currentTime)
} }
#endif #endif
#define DEFINE_TASK(taskNameParam, subTaskNameParam, checkFuncParam, taskFuncParam, desiredPeriodParam, staticPriorityParam) { \
.taskName = taskNameParam, \
.subTaskName = subTaskNameParam, \
.checkFunc = checkFuncParam, \
.taskFunc = taskFuncParam, \
.desiredPeriodUs = desiredPeriodParam, \
.staticPriority = staticPriorityParam \
}
// Task info in .bss (unitialised data)
task_t tasks[TASK_COUNT];
// Task ID data in .data (initialised data)
task_id_t task_ids[TASK_COUNT] = {
[TASK_SYSTEM] = DEFINE_TASK("SYSTEM", "LOAD", NULL, taskSystemLoad, TASK_PERIOD_HZ(10), TASK_PRIORITY_MEDIUM_HIGH),
[TASK_MAIN] = DEFINE_TASK("SYSTEM", "UPDATE", NULL, taskMain, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM_HIGH),
[TASK_SERIAL] = DEFINE_TASK("SERIAL", NULL, NULL, taskHandleSerial, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW), // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud
[TASK_BATTERY_ALERTS] = DEFINE_TASK("BATTERY_ALERTS", NULL, NULL, taskBatteryAlerts, TASK_PERIOD_HZ(5), TASK_PRIORITY_MEDIUM),
[TASK_BATTERY_VOLTAGE] = DEFINE_TASK("BATTERY_VOLTAGE", NULL, NULL, batteryUpdateVoltage, TASK_PERIOD_HZ(SLOW_VOLTAGE_TASK_FREQ_HZ), TASK_PRIORITY_MEDIUM), // Freq may be updated in tasksInit
[TASK_BATTERY_CURRENT] = DEFINE_TASK("BATTERY_CURRENT", NULL, NULL, batteryUpdateCurrentMeter, TASK_PERIOD_HZ(50), TASK_PRIORITY_MEDIUM),
#ifdef USE_TRANSPONDER
[TASK_TRANSPONDER] = DEFINE_TASK("TRANSPONDER", NULL, NULL, transponderUpdate, TASK_PERIOD_HZ(250), TASK_PRIORITY_LOW),
#endif
#ifdef USE_STACK_CHECK
[TASK_STACK_CHECK] = DEFINE_TASK("STACKCHECK", NULL, NULL, taskStackCheck, TASK_PERIOD_HZ(10), TASK_PRIORITY_IDLE),
#endif
[TASK_GYRO] = DEFINE_TASK("GYRO", NULL, NULL, taskGyroSample, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
[TASK_FILTER] = DEFINE_TASK("FILTER", NULL, NULL, taskFiltering, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
[TASK_PID] = DEFINE_TASK("PID", NULL, NULL, taskMainPidLoop, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
#ifdef USE_ACC
[TASK_ACCEL] = DEFINE_TASK("ACC", NULL, NULL, taskUpdateAccelerometer, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM),
[TASK_ATTITUDE] = DEFINE_TASK("ATTITUDE", NULL, NULL, imuUpdateAttitude, TASK_PERIOD_HZ(100), TASK_PRIORITY_MEDIUM),
#endif
[TASK_RX] = DEFINE_TASK("RX", NULL, rxUpdateCheck, taskUpdateRxMain, TASK_PERIOD_HZ(33), TASK_PRIORITY_HIGH), // If event-based scheduling doesn't work, fallback to periodic scheduling
[TASK_DISPATCH] = DEFINE_TASK("DISPATCH", NULL, NULL, dispatchProcess, TASK_PERIOD_HZ(1000), TASK_PRIORITY_HIGH),
#ifdef USE_BEEPER
[TASK_BEEPER] = DEFINE_TASK("BEEPER", NULL, NULL, beeperUpdate, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW),
#endif
#ifdef USE_GPS
[TASK_GPS] = DEFINE_TASK("GPS", NULL, NULL, gpsUpdate, TASK_PERIOD_HZ(TASK_GPS_RATE), TASK_PRIORITY_MEDIUM), // Required to prevent buffer overruns if running at 115200 baud (115 bytes / period < 256 bytes buffer)
#endif
#ifdef USE_MAG
[TASK_COMPASS] = DEFINE_TASK("COMPASS", NULL, NULL, compassUpdate,TASK_PERIOD_HZ(10), TASK_PRIORITY_LOW),
#endif
#ifdef USE_BARO
[TASK_BARO] = DEFINE_TASK("BARO", NULL, NULL, taskUpdateBaro, TASK_PERIOD_HZ(20), TASK_PRIORITY_LOW),
#endif
#if defined(USE_BARO) || defined(USE_GPS)
[TASK_ALTITUDE] = DEFINE_TASK("ALTITUDE", NULL, NULL, taskCalculateAltitude, TASK_PERIOD_HZ(40), TASK_PRIORITY_LOW),
#endif
#ifdef USE_DASHBOARD
[TASK_DASHBOARD] = DEFINE_TASK("DASHBOARD", NULL, NULL, dashboardUpdate, TASK_PERIOD_HZ(10), TASK_PRIORITY_LOW),
#endif
#ifdef USE_OSD
[TASK_OSD] = DEFINE_TASK("OSD", NULL, osdUpdateCheck, osdUpdate, TASK_PERIOD_HZ(OSD_FRAMERATE_DEFAULT_HZ), TASK_PRIORITY_LOW),
#endif
#ifdef USE_TELEMETRY
[TASK_TELEMETRY] = DEFINE_TASK("TELEMETRY", NULL, NULL, taskTelemetry, TASK_PERIOD_HZ(250), TASK_PRIORITY_LOW),
#endif
#ifdef USE_LED_STRIP
[TASK_LEDSTRIP] = DEFINE_TASK("LEDSTRIP", NULL, NULL, ledStripUpdate, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW),
#endif
#ifdef USE_BST
[TASK_BST_MASTER_PROCESS] = DEFINE_TASK("BST_MASTER_PROCESS", NULL, NULL, taskBstMasterProcess, TASK_PERIOD_HZ(50), TASK_PRIORITY_IDLE),
#endif
#ifdef USE_ESC_SENSOR
[TASK_ESC_SENSOR] = DEFINE_TASK("ESC_SENSOR", NULL, NULL, escSensorProcess, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW),
#endif
#ifdef USE_CMS
[TASK_CMS] = DEFINE_TASK("CMS", NULL, NULL, cmsHandler, TASK_PERIOD_HZ(20), TASK_PRIORITY_LOW),
#endif
#ifdef USE_VTX_CONTROL
[TASK_VTXCTRL] = DEFINE_TASK("VTXCTRL", NULL, NULL, vtxUpdate, TASK_PERIOD_HZ(5), TASK_PRIORITY_IDLE),
#endif
#ifdef USE_RCDEVICE
[TASK_RCDEVICE] = DEFINE_TASK("RCDEVICE", NULL, NULL, rcdeviceUpdate, TASK_PERIOD_HZ(20), TASK_PRIORITY_MEDIUM),
#endif
#ifdef USE_CAMERA_CONTROL
[TASK_CAMCTRL] = DEFINE_TASK("CAMCTRL", NULL, NULL, taskCameraControl, TASK_PERIOD_HZ(5), TASK_PRIORITY_IDLE),
#endif
#ifdef USE_ADC_INTERNAL
[TASK_ADC_INTERNAL] = DEFINE_TASK("ADCINTERNAL", NULL, NULL, adcInternalProcess, TASK_PERIOD_HZ(1), TASK_PRIORITY_IDLE),
#endif
#ifdef USE_PINIOBOX
[TASK_PINIOBOX] = DEFINE_TASK("PINIOBOX", NULL, NULL, pinioBoxUpdate, TASK_PERIOD_HZ(20), TASK_PRIORITY_IDLE),
#endif
#ifdef USE_RANGEFINDER
[TASK_RANGEFINDER] = DEFINE_TASK("RANGEFINDER", NULL, NULL, taskUpdateRangefinder, TASK_PERIOD_HZ(10), TASK_PRIORITY_IDLE),
#endif
#ifdef USE_CRSF_V3
[TASK_SPEED_NEGOTIATION] = DEFINE_TASK("SPEED_NEGOTIATION", NULL, NULL, speedNegotiationProcess, TASK_PERIOD_HZ(100), TASK_PRIORITY_IDLE),
#endif
};
task_t *getTask(unsigned taskId)
{
return &tasks[taskId];
}
void tasksInit(void) void tasksInit(void)
{ {
for (int i = 0; i < TASK_COUNT; i++) {
tasks[i].id = &task_ids[i];
}
schedulerInit(); schedulerInit();
setTaskEnabled(TASK_MAIN, true); setTaskEnabled(TASK_MAIN, true);
@ -448,120 +573,3 @@ void tasksInit(void)
setTaskEnabled(TASK_SPEED_NEGOTIATION, useCRSF); setTaskEnabled(TASK_SPEED_NEGOTIATION, useCRSF);
#endif #endif
} }
#define DEFINE_TASK(taskNameParam, subTaskNameParam, checkFuncParam, taskFuncParam, desiredPeriodParam, staticPriorityParam) { \
.taskName = taskNameParam, \
.subTaskName = subTaskNameParam, \
.checkFunc = checkFuncParam, \
.taskFunc = taskFuncParam, \
.desiredPeriodUs = desiredPeriodParam, \
.staticPriority = staticPriorityParam \
}
task_t tasks[TASK_COUNT] = {
[TASK_SYSTEM] = DEFINE_TASK("SYSTEM", "LOAD", NULL, taskSystemLoad, TASK_PERIOD_HZ(10), TASK_PRIORITY_MEDIUM_HIGH),
[TASK_MAIN] = DEFINE_TASK("SYSTEM", "UPDATE", NULL, taskMain, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM_HIGH),
[TASK_SERIAL] = DEFINE_TASK("SERIAL", NULL, NULL, taskHandleSerial, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW), // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud
[TASK_BATTERY_ALERTS] = DEFINE_TASK("BATTERY_ALERTS", NULL, NULL, taskBatteryAlerts, TASK_PERIOD_HZ(5), TASK_PRIORITY_MEDIUM),
[TASK_BATTERY_VOLTAGE] = DEFINE_TASK("BATTERY_VOLTAGE", NULL, NULL, batteryUpdateVoltage, TASK_PERIOD_HZ(SLOW_VOLTAGE_TASK_FREQ_HZ), TASK_PRIORITY_MEDIUM), // Freq may be updated in tasksInit
[TASK_BATTERY_CURRENT] = DEFINE_TASK("BATTERY_CURRENT", NULL, NULL, batteryUpdateCurrentMeter, TASK_PERIOD_HZ(50), TASK_PRIORITY_MEDIUM),
#ifdef USE_TRANSPONDER
[TASK_TRANSPONDER] = DEFINE_TASK("TRANSPONDER", NULL, NULL, transponderUpdate, TASK_PERIOD_HZ(250), TASK_PRIORITY_LOW),
#endif
#ifdef USE_STACK_CHECK
[TASK_STACK_CHECK] = DEFINE_TASK("STACKCHECK", NULL, NULL, taskStackCheck, TASK_PERIOD_HZ(10), TASK_PRIORITY_IDLE),
#endif
[TASK_GYRO] = DEFINE_TASK("GYRO", NULL, NULL, taskGyroSample, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
[TASK_FILTER] = DEFINE_TASK("FILTER", NULL, NULL, taskFiltering, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
[TASK_PID] = DEFINE_TASK("PID", NULL, NULL, taskMainPidLoop, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
#ifdef USE_ACC
[TASK_ACCEL] = DEFINE_TASK("ACC", NULL, NULL, taskUpdateAccelerometer, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM),
[TASK_ATTITUDE] = DEFINE_TASK("ATTITUDE", NULL, NULL, imuUpdateAttitude, TASK_PERIOD_HZ(100), TASK_PRIORITY_MEDIUM),
#endif
[TASK_RX] = DEFINE_TASK("RX", NULL, rxUpdateCheck, taskUpdateRxMain, TASK_PERIOD_HZ(33), TASK_PRIORITY_HIGH), // If event-based scheduling doesn't work, fallback to periodic scheduling
[TASK_DISPATCH] = DEFINE_TASK("DISPATCH", NULL, NULL, dispatchProcess, TASK_PERIOD_HZ(1000), TASK_PRIORITY_HIGH),
#ifdef USE_BEEPER
[TASK_BEEPER] = DEFINE_TASK("BEEPER", NULL, NULL, beeperUpdate, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW),
#endif
#ifdef USE_GPS
[TASK_GPS] = DEFINE_TASK("GPS", NULL, NULL, gpsUpdate, TASK_PERIOD_HZ(TASK_GPS_RATE), TASK_PRIORITY_MEDIUM), // Required to prevent buffer overruns if running at 115200 baud (115 bytes / period < 256 bytes buffer)
#endif
#ifdef USE_MAG
[TASK_COMPASS] = DEFINE_TASK("COMPASS", NULL, NULL, compassUpdate,TASK_PERIOD_HZ(10), TASK_PRIORITY_LOW),
#endif
#ifdef USE_BARO
[TASK_BARO] = DEFINE_TASK("BARO", NULL, NULL, taskUpdateBaro, TASK_PERIOD_HZ(20), TASK_PRIORITY_LOW),
#endif
#if defined(USE_BARO) || defined(USE_GPS)
[TASK_ALTITUDE] = DEFINE_TASK("ALTITUDE", NULL, NULL, taskCalculateAltitude, TASK_PERIOD_HZ(40), TASK_PRIORITY_LOW),
#endif
#ifdef USE_DASHBOARD
[TASK_DASHBOARD] = DEFINE_TASK("DASHBOARD", NULL, NULL, dashboardUpdate, TASK_PERIOD_HZ(10), TASK_PRIORITY_LOW),
#endif
#ifdef USE_OSD
[TASK_OSD] = DEFINE_TASK("OSD", NULL, osdUpdateCheck, osdUpdate, TASK_PERIOD_HZ(OSD_FRAMERATE_DEFAULT_HZ), TASK_PRIORITY_LOW),
#endif
#ifdef USE_TELEMETRY
[TASK_TELEMETRY] = DEFINE_TASK("TELEMETRY", NULL, NULL, taskTelemetry, TASK_PERIOD_HZ(250), TASK_PRIORITY_LOW),
#endif
#ifdef USE_LED_STRIP
[TASK_LEDSTRIP] = DEFINE_TASK("LEDSTRIP", NULL, NULL, ledStripUpdate, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW),
#endif
#ifdef USE_BST
[TASK_BST_MASTER_PROCESS] = DEFINE_TASK("BST_MASTER_PROCESS", NULL, NULL, taskBstMasterProcess, TASK_PERIOD_HZ(50), TASK_PRIORITY_IDLE),
#endif
#ifdef USE_ESC_SENSOR
[TASK_ESC_SENSOR] = DEFINE_TASK("ESC_SENSOR", NULL, NULL, escSensorProcess, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW),
#endif
#ifdef USE_CMS
[TASK_CMS] = DEFINE_TASK("CMS", NULL, NULL, cmsHandler, TASK_PERIOD_HZ(20), TASK_PRIORITY_LOW),
#endif
#ifdef USE_VTX_CONTROL
[TASK_VTXCTRL] = DEFINE_TASK("VTXCTRL", NULL, NULL, vtxUpdate, TASK_PERIOD_HZ(5), TASK_PRIORITY_IDLE),
#endif
#ifdef USE_RCDEVICE
[TASK_RCDEVICE] = DEFINE_TASK("RCDEVICE", NULL, NULL, rcdeviceUpdate, TASK_PERIOD_HZ(20), TASK_PRIORITY_MEDIUM),
#endif
#ifdef USE_CAMERA_CONTROL
[TASK_CAMCTRL] = DEFINE_TASK("CAMCTRL", NULL, NULL, taskCameraControl, TASK_PERIOD_HZ(5), TASK_PRIORITY_IDLE),
#endif
#ifdef USE_ADC_INTERNAL
[TASK_ADC_INTERNAL] = DEFINE_TASK("ADCINTERNAL", NULL, NULL, adcInternalProcess, TASK_PERIOD_HZ(1), TASK_PRIORITY_IDLE),
#endif
#ifdef USE_PINIOBOX
[TASK_PINIOBOX] = DEFINE_TASK("PINIOBOX", NULL, NULL, pinioBoxUpdate, TASK_PERIOD_HZ(20), TASK_PRIORITY_IDLE),
#endif
#ifdef USE_RANGEFINDER
[TASK_RANGEFINDER] = DEFINE_TASK("RANGEFINDER", NULL, NULL, taskUpdateRangefinder, TASK_PERIOD_HZ(10), TASK_PRIORITY_IDLE),
#endif
#ifdef USE_CRSF_V3
[TASK_SPEED_NEGOTIATION] = DEFINE_TASK("SPEED_NEGOTIATION", NULL, NULL, speedNegotiationProcess, TASK_PERIOD_HZ(100), TASK_PRIORITY_IDLE),
#endif
};
task_t *getTask(unsigned taskId)
{
return &tasks[taskId];
}

50
src/main/scheduler/scheduler.c
View file

@ -87,8 +87,6 @@ FAST_DATA_ZERO_INIT uint16_t averageSystemLoadPercent = 0;
static FAST_DATA_ZERO_INIT int taskQueuePos = 0; static FAST_DATA_ZERO_INIT int taskQueuePos = 0;
STATIC_UNIT_TESTED FAST_DATA_ZERO_INIT int taskQueueSize = 0; STATIC_UNIT_TESTED FAST_DATA_ZERO_INIT int taskQueueSize = 0;
static bool optimizeSchedRate;
static FAST_DATA_ZERO_INIT bool gyroEnabled; static FAST_DATA_ZERO_INIT bool gyroEnabled;
static int32_t desiredPeriodCycles; static int32_t desiredPeriodCycles;
@ -128,7 +126,7 @@ bool queueAdd(task_t *task)
return false; return false;
} }
for (int ii = 0; ii <= taskQueueSize; ++ii) { for (int ii = 0; ii <= taskQueueSize; ++ii) {
if (taskQueueArray[ii] == NULL || taskQueueArray[ii]->staticPriority < task->staticPriority) { if (taskQueueArray[ii] == NULL || taskQueueArray[ii]->id->staticPriority < task->id->staticPriority) {
memmove(&taskQueueArray[ii+1], &taskQueueArray[ii], sizeof(task) * (taskQueueSize - ii)); memmove(&taskQueueArray[ii+1], &taskQueueArray[ii], sizeof(task) * (taskQueueSize - ii));
taskQueueArray[ii] = task; taskQueueArray[ii] = task;
++taskQueueSize; ++taskQueueSize;
@ -204,10 +202,10 @@ void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo)
void getTaskInfo(taskId_e taskId, taskInfo_t * taskInfo) void getTaskInfo(taskId_e taskId, taskInfo_t * taskInfo)
{ {
taskInfo->isEnabled = queueContains(getTask(taskId)); taskInfo->isEnabled = queueContains(getTask(taskId));
taskInfo->desiredPeriodUs = getTask(taskId)->desiredPeriodUs; taskInfo->desiredPeriodUs = getTask(taskId)->id->desiredPeriodUs;
taskInfo->staticPriority = getTask(taskId)->staticPriority; taskInfo->staticPriority = getTask(taskId)->id->staticPriority;
taskInfo->taskName = getTask(taskId)->taskName; taskInfo->taskName = getTask(taskId)->id->taskName;
taskInfo->subTaskName = getTask(taskId)->subTaskName; taskInfo->subTaskName = getTask(taskId)->id->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)->anticipatedExecutionTime >> TASK_EXEC_TIME_SHIFT; taskInfo->averageExecutionTimeUs = getTask(taskId)->anticipatedExecutionTime >> TASK_EXEC_TIME_SHIFT;
@ -232,11 +230,11 @@ void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs)
} else { } else {
return; return;
} }
task->desiredPeriodUs = MAX(SCHEDULER_DELAY_LIMIT, newPeriodUs); // Limit delay to 100us (10 kHz) to prevent scheduler clogging task->id->desiredPeriodUs = MAX(SCHEDULER_DELAY_LIMIT, newPeriodUs); // Limit delay to 100us (10 kHz) to prevent scheduler clogging
// Catch the case where the gyro loop is adjusted // Catch the case where the gyro loop is adjusted
if (taskId == TASK_GYRO) { if (taskId == TASK_GYRO) {
desiredPeriodCycles = (int32_t)clockMicrosToCycles((uint32_t)getTask(TASK_GYRO)->desiredPeriodUs); desiredPeriodCycles = (int32_t)clockMicrosToCycles((uint32_t)getTask(TASK_GYRO)->id->desiredPeriodUs);
} }
} }
@ -244,7 +242,7 @@ void setTaskEnabled(taskId_e taskId, bool enabled)
{ {
if (taskId == TASK_SELF || taskId < TASK_COUNT) { if (taskId == TASK_SELF || taskId < TASK_COUNT) {
task_t *task = taskId == TASK_SELF ? currentTask : getTask(taskId); task_t *task = taskId == TASK_SELF ? currentTask : getTask(taskId);
if (enabled && task->taskFunc) { if (enabled && task->id->taskFunc) {
queueAdd(task); queueAdd(task);
} else { } else {
queueRemove(task); queueRemove(task);
@ -338,7 +336,7 @@ void schedulerInit(void)
taskGuardDeltaDownCycles = clockMicrosToCycles(1) / TASK_GUARD_MARGIN_DOWN_STEP; taskGuardDeltaDownCycles = clockMicrosToCycles(1) / TASK_GUARD_MARGIN_DOWN_STEP;
taskGuardDeltaUpCycles = clockMicrosToCycles(1) / TASK_GUARD_MARGIN_UP_STEP; taskGuardDeltaUpCycles = clockMicrosToCycles(1) / TASK_GUARD_MARGIN_UP_STEP;
desiredPeriodCycles = (int32_t)clockMicrosToCycles((uint32_t)getTask(TASK_GYRO)->desiredPeriodUs); desiredPeriodCycles = (int32_t)clockMicrosToCycles((uint32_t)getTask(TASK_GYRO)->id->desiredPeriodUs);
lastTargetCycles = getCycleCounter(); lastTargetCycles = getCycleCounter();
@ -351,14 +349,6 @@ void schedulerInit(void)
} }
} }
inline static timeUs_t getPeriodCalculationBasis(const task_t* task)
{
if ((task->staticPriority == TASK_PRIORITY_REALTIME) && (optimizeSchedRate)) {
return task->lastDesiredAt;
}
return task->lastExecutedAtUs;
}
static timeDelta_t taskNextStateTime; static timeDelta_t taskNextStateTime;
FAST_CODE void schedulerSetNextStateTime(timeDelta_t nextStateTime) FAST_CODE void schedulerSetNextStateTime(timeDelta_t nextStateTime)
@ -377,12 +367,12 @@ FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTi
taskNextStateTime = -1; taskNextStateTime = -1;
float period = currentTimeUs - selectedTask->lastExecutedAtUs; float period = currentTimeUs - selectedTask->lastExecutedAtUs;
selectedTask->lastExecutedAtUs = currentTimeUs; selectedTask->lastExecutedAtUs = currentTimeUs;
selectedTask->lastDesiredAt += selectedTask->desiredPeriodUs; selectedTask->lastDesiredAt += selectedTask->id->desiredPeriodUs;
selectedTask->dynamicPriority = 0; selectedTask->dynamicPriority = 0;
// Execute task // Execute task
const timeUs_t currentTimeBeforeTaskCallUs = micros(); const timeUs_t currentTimeBeforeTaskCallUs = micros();
selectedTask->taskFunc(currentTimeBeforeTaskCallUs); selectedTask->id->taskFunc(currentTimeBeforeTaskCallUs);
taskExecutionTimeUs = micros() - currentTimeBeforeTaskCallUs; taskExecutionTimeUs = micros() - currentTimeBeforeTaskCallUs;
taskTotalExecutionTime += taskExecutionTimeUs; taskTotalExecutionTime += taskExecutionTimeUs;
if (!ignoreCurrentTaskExecRate) { if (!ignoreCurrentTaskExecRate) {
@ -569,14 +559,14 @@ FAST_CODE void scheduler(void)
// Update task dynamic priorities // Update task dynamic priorities
for (task_t *task = queueFirst(); task != NULL; task = queueNext()) { for (task_t *task = queueFirst(); task != NULL; task = queueNext()) {
if (task->staticPriority != TASK_PRIORITY_REALTIME) { if (task->id->staticPriority != TASK_PRIORITY_REALTIME) {
// Task has checkFunc - event driven // Task has checkFunc - event driven
if (task->checkFunc) { if (task->id->checkFunc) {
// Increase priority for event driven tasks // Increase priority for event driven tasks
if (task->dynamicPriority > 0) { if (task->dynamicPriority > 0) {
task->taskAgeCycles = 1 + (cmpTimeUs(currentTimeUs, task->lastSignaledAtUs) / task->desiredPeriodUs); task->taskAgeCycles = 1 + (cmpTimeUs(currentTimeUs, task->lastSignaledAtUs) / task->id->desiredPeriodUs);
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles; task->dynamicPriority = 1 + task->id->staticPriority * task->taskAgeCycles;
} else if (task->checkFunc(currentTimeUs, cmpTimeUs(currentTimeUs, task->lastExecutedAtUs))) { } else if (task->id->checkFunc(currentTimeUs, cmpTimeUs(currentTimeUs, task->lastExecutedAtUs))) {
const uint32_t checkFuncExecutionTimeUs = cmpTimeUs(micros(), currentTimeUs); const uint32_t checkFuncExecutionTimeUs = cmpTimeUs(micros(), currentTimeUs);
#if !defined(UNIT_TEST) #if !defined(UNIT_TEST)
DEBUG_SET(DEBUG_SCHEDULER, 3, checkFuncExecutionTimeUs); DEBUG_SET(DEBUG_SCHEDULER, 3, checkFuncExecutionTimeUs);
@ -587,16 +577,16 @@ FAST_CODE void scheduler(void)
checkFuncMaxExecutionTimeUs = MAX(checkFuncMaxExecutionTimeUs, checkFuncExecutionTimeUs); checkFuncMaxExecutionTimeUs = MAX(checkFuncMaxExecutionTimeUs, checkFuncExecutionTimeUs);
task->lastSignaledAtUs = currentTimeUs; task->lastSignaledAtUs = currentTimeUs;
task->taskAgeCycles = 1; task->taskAgeCycles = 1;
task->dynamicPriority = 1 + task->staticPriority; task->dynamicPriority = 1 + task->id->staticPriority;
} else { } else {
task->taskAgeCycles = 0; task->taskAgeCycles = 0;
} }
} else { } else {
// Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods) // Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods)
// Task age is calculated from last execution // Task age is calculated from last execution
task->taskAgeCycles = (cmpTimeUs(currentTimeUs, getPeriodCalculationBasis(task)) / task->desiredPeriodUs); task->taskAgeCycles = (cmpTimeUs(currentTimeUs, task->lastExecutedAtUs) / task->id->desiredPeriodUs);
if (task->taskAgeCycles > 0) { if (task->taskAgeCycles > 0) {
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles; task->dynamicPriority = 1 + task->id->staticPriority * task->taskAgeCycles;
} }
} }
@ -677,5 +667,5 @@ uint16_t getAverageSystemLoadPercent(void)
float schedulerGetCycleTimeMultiplier(void) float schedulerGetCycleTimeMultiplier(void)
{ {
return (float)clockMicrosToCycles(getTask(TASK_GYRO)->desiredPeriodUs) / desiredPeriodCycles; return (float)clockMicrosToCycles(getTask(TASK_GYRO)->id->desiredPeriodUs) / desiredPeriodCycles;
} }

5
src/main/scheduler/scheduler.h
View file

@ -189,6 +189,11 @@ typedef struct {
void (*taskFunc)(timeUs_t currentTimeUs); void (*taskFunc)(timeUs_t currentTimeUs);
timeDelta_t desiredPeriodUs; // target period of execution timeDelta_t desiredPeriodUs; // target period of execution
const int8_t staticPriority; // dynamicPriority grows in steps of this size const int8_t staticPriority; // dynamicPriority grows in steps of this size
} task_id_t;
typedef struct {
// Task static data
task_id_t *id;
// Scheduling // Scheduling
uint16_t dynamicPriority; // measurement of how old task was last executed, used to avoid task starvation uint16_t dynamicPriority; // measurement of how old task was last executed, used to avoid task starvation

25
src/test/unit/scheduler_unittest.cc
View file

@ -95,7 +95,7 @@ extern "C" {
extern task_t *queueFirst(void); extern task_t *queueFirst(void);
extern task_t *queueNext(void); extern task_t *queueNext(void);
task_t tasks[TASK_COUNT] = { task_id_t task_ids[TASK_COUNT] = {
[TASK_SYSTEM] = { [TASK_SYSTEM] = {
.taskName = "SYSTEM", .taskName = "SYSTEM",
.taskFunc = taskSystemLoad, .taskFunc = taskSystemLoad,
@ -159,19 +159,29 @@ extern "C" {
} }
}; };
task_t tasks[TASK_COUNT];
task_t *getTask(unsigned taskId) task_t *getTask(unsigned taskId)
{ {
return &tasks[taskId]; return &tasks[taskId];
} }
} }
TEST(SchedulerUnittest, SetupTasks)
{
for (int i = 0; i < TASK_COUNT; ++i) {
tasks[i].id = &task_ids[i];
}
}
TEST(SchedulerUnittest, TestPriorites) TEST(SchedulerUnittest, TestPriorites)
{ {
EXPECT_EQ(TASK_PRIORITY_MEDIUM_HIGH, tasks[TASK_SYSTEM].staticPriority); EXPECT_EQ(TASK_PRIORITY_MEDIUM_HIGH, tasks[TASK_SYSTEM].id->staticPriority);
EXPECT_EQ(TASK_PRIORITY_REALTIME, tasks[TASK_GYRO].staticPriority); EXPECT_EQ(TASK_PRIORITY_REALTIME, tasks[TASK_GYRO].id->staticPriority);
EXPECT_EQ(TASK_PRIORITY_MEDIUM, tasks[TASK_ACCEL].staticPriority); EXPECT_EQ(TASK_PRIORITY_MEDIUM, tasks[TASK_ACCEL].id->staticPriority);
EXPECT_EQ(TASK_PRIORITY_LOW, tasks[TASK_SERIAL].staticPriority); EXPECT_EQ(TASK_PRIORITY_LOW, tasks[TASK_SERIAL].id->staticPriority);
EXPECT_EQ(TASK_PRIORITY_MEDIUM, tasks[TASK_BATTERY_VOLTAGE].staticPriority); EXPECT_EQ(TASK_PRIORITY_MEDIUM, tasks[TASK_BATTERY_VOLTAGE].id->staticPriority);
} }
TEST(SchedulerUnittest, TestQueueInit) TEST(SchedulerUnittest, TestQueueInit)
@ -242,7 +252,6 @@ TEST(SchedulerUnittest, TestQueueAddAndRemove)
EXPECT_EQ(taskId + 1, taskQueueSize); EXPECT_EQ(taskId + 1, taskQueueSize);
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
} }
// double check end of queue // double check end of queue
EXPECT_EQ(TASK_COUNT, taskQueueSize); EXPECT_EQ(TASK_COUNT, taskQueueSize);
EXPECT_NE(static_cast<task_t*>(0), taskQueueArray[TASK_COUNT - 1]); // last item was indeed added to queue EXPECT_NE(static_cast<task_t*>(0), taskQueueArray[TASK_COUNT - 1]); // last item was indeed added to queue
@ -274,7 +283,7 @@ TEST(SchedulerUnittest, TestQueueArray)
EXPECT_EQ(enqueuedTasks, taskQueueSize); EXPECT_EQ(enqueuedTasks, taskQueueSize);
for (int taskId = 0; taskId < TASK_COUNT_UNITTEST - 1; ++taskId) { for (int taskId = 0; taskId < TASK_COUNT_UNITTEST - 1; ++taskId) {
if (tasks[taskId].taskFunc) { if (tasks[taskId].id->taskFunc) {
setTaskEnabled(static_cast<taskId_e>(taskId), true); setTaskEnabled(static_cast<taskId_e>(taskId), true);
enqueuedTasks++; enqueuedTasks++;
EXPECT_EQ(enqueuedTasks, taskQueueSize); EXPECT_EQ(enqueuedTasks, taskQueueSize);