Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 03:50:02 +03:00
Code Issues Wiki Activity

Cleaned up the scheduler.

Browse source
This commit is contained in:
mikeller 2020-03-15 13:28:30 +13:00
parent b729c3cc99
commit db4bd1f186
20 changed files with 326 additions and 392 deletions
Download patch file Download diff file Expand all files Collapse all files

22
src/main/cli/cli.c
View file

@ -4670,14 +4670,14 @@ static void cliStatus(char *cmdline)
// Run status // Run status
const int gyroRate = getTaskDeltaTime(TASK_GYRO) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_GYRO))); const int gyroRate = getTaskDeltaTimeUs(TASK_GYRO) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTimeUs(TASK_GYRO)));
int rxRate = getCurrentRxRefreshRate(); int rxRate = getCurrentRxRefreshRate();
if (rxRate != 0) { if (rxRate != 0) {
rxRate = (int)(1000000.0f / ((float)rxRate)); rxRate = (int)(1000000.0f / ((float)rxRate));
} }
const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM))); const int systemRate = getTaskDeltaTimeUs(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTimeUs(TASK_SYSTEM)));
cliPrintLinef("CPU:%d%%, cycle time: %d, GYRO rate: %d, RX rate: %d, System rate: %d", cliPrintLinef("CPU:%d%%, cycle time: %d, GYRO rate: %d, RX rate: %d, System rate: %d",
constrain(averageSystemLoadPercent, 0, 100), getTaskDeltaTime(TASK_GYRO), gyroRate, rxRate, systemRate); constrain(getAverageSystemLoadPercent(), 0, LOAD_PERCENTAGE_ONE), getTaskDeltaTimeUs(TASK_GYRO), gyroRate, rxRate, systemRate);
// Battery meter // Battery meter
@ -4720,22 +4720,22 @@ static void cliTasks(char *cmdline)
cliPrintLine("Task list"); cliPrintLine("Task list");
} }
#endif #endif
for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; taskId++) { for (taskId_e taskId = 0; taskId < TASK_COUNT; taskId++) {
cfTaskInfo_t taskInfo; taskInfo_t taskInfo;
getTaskInfo(taskId, &taskInfo); getTaskInfo(taskId, &taskInfo);
if (taskInfo.isEnabled) { if (taskInfo.isEnabled) {
int taskFrequency = taskInfo.averageDeltaTime == 0 ? 0 : lrintf(1e6f / taskInfo.averageDeltaTime); int taskFrequency = taskInfo.averageDeltaTimeUs == 0 ? 0 : lrintf(1e6f / taskInfo.averageDeltaTimeUs);
cliPrintf("%02d - (%15s) ", taskId, taskInfo.taskName); cliPrintf("%02d - (%15s) ", taskId, taskInfo.taskName);
const int maxLoad = taskInfo.maxExecutionTime == 0 ? 0 :(taskInfo.maxExecutionTime * taskFrequency + 5000) / 1000; const int maxLoad = taskInfo.maxExecutionTimeUs == 0 ? 0 :(taskInfo.maxExecutionTimeUs * taskFrequency + 5000) / 1000;
const int averageLoad = taskInfo.averageExecutionTime == 0 ? 0 : (taskInfo.averageExecutionTime * taskFrequency + 5000) / 1000; const int averageLoad = taskInfo.averageExecutionTimeUs == 0 ? 0 : (taskInfo.averageExecutionTimeUs * taskFrequency + 5000) / 1000;
if (taskId != TASK_SERIAL) { if (taskId != TASK_SERIAL) {
maxLoadSum += maxLoad; maxLoadSum += maxLoad;
averageLoadSum += averageLoad; averageLoadSum += averageLoad;
} }
if (systemConfig()->task_statistics) { if (systemConfig()->task_statistics) {
cliPrintLinef("%6d %7d %7d %4d.%1d%% %4d.%1d%% %9d", cliPrintLinef("%6d %7d %7d %4d.%1d%% %4d.%1d%% %9d",
taskFrequency, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskFrequency, taskInfo.maxExecutionTimeUs, taskInfo.averageExecutionTimeUs,
maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10, taskInfo.totalExecutionTime / 1000); maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10, taskInfo.totalExecutionTimeUs / 1000);
} else { } else {
cliPrintLinef("%6d", taskFrequency); cliPrintLinef("%6d", taskFrequency);
} }
@ -4746,7 +4746,7 @@ static void cliTasks(char *cmdline)
if (systemConfig()->task_statistics) { if (systemConfig()->task_statistics) {
cfCheckFuncInfo_t checkFuncInfo; cfCheckFuncInfo_t checkFuncInfo;
getCheckFuncInfo(&checkFuncInfo); getCheckFuncInfo(&checkFuncInfo);
cliPrintLinef("RX Check Function %19d %7d %25d", checkFuncInfo.maxExecutionTime, checkFuncInfo.averageExecutionTime, checkFuncInfo.totalExecutionTime / 1000); cliPrintLinef("RX Check Function %19d %7d %25d", checkFuncInfo.maxExecutionTimeUs, checkFuncInfo.averageExecutionTimeUs, checkFuncInfo.totalExecutionTimeUs / 1000);
cliPrintLinef("Total (excluding SERIAL) %25d.%1d%% %4d.%1d%%", maxLoadSum/10, maxLoadSum%10, averageLoadSum/10, averageLoadSum%10); cliPrintLinef("Total (excluding SERIAL) %25d.%1d%% %4d.%1d%%", maxLoadSum/10, maxLoadSum%10, averageLoadSum/10, averageLoadSum%10);
schedulerResetCheckFunctionMaxExecutionTime(); schedulerResetCheckFunctionMaxExecutionTime();
} }

98
src/main/config/config_unittest.h
View file

@ -1,98 +0,0 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifdef SRC_MAIN_SCHEDULER_C_
#ifdef UNIT_TEST
cfTask_t *unittest_scheduler_selectedTask;
uint8_t unittest_scheduler_selectedTaskDynamicPriority;
uint16_t unittest_scheduler_waitingTasks;
#define GET_SCHEDULER_LOCALS() \
{ \
unittest_scheduler_selectedTask = selectedTask; \
unittest_scheduler_selectedTaskDynamicPriority = selectedTaskDynamicPriority; \
unittest_scheduler_waitingTasks = waitingTasks; \
}
#else
#define GET_SCHEDULER_LOCALS() {}
#endif
#endif
#ifdef SRC_MAIN_FLIGHT_PID_C_
#ifdef UNIT_TEST
float unittest_pidLuxFloat_lastErrorForDelta[3];
float unittest_pidLuxFloat_delta1[3];
float unittest_pidLuxFloat_delta2[3];
float unittest_pidLuxFloat_pterm[3];
float unittest_pidLuxFloat_iterm[3];
float unittest_pidLuxFloat_dterm[3];
#define SET_PID_LUX_FLOAT_LOCALS(axis) \
{ \
lastErrorForDelta[axis] = unittest_pidLuxFloat_lastErrorForDelta[axis]; \
delta1[axis] = unittest_pidLuxFloat_delta1[axis]; \
delta2[axis] = unittest_pidLuxFloat_delta2[axis]; \
}
#define GET_PID_LUX_FLOAT_LOCALS(axis) \
{ \
unittest_pidLuxFloat_lastErrorForDelta[axis] = lastErrorForDelta[axis]; \
unittest_pidLuxFloat_delta1[axis] = delta1[axis]; \
unittest_pidLuxFloat_delta2[axis] = delta2[axis]; \
unittest_pidLuxFloat_pterm[axis] = pterm; \
unittest_pidLuxFloat_iterm[axis] = iterm; \
unittest_pidLuxFloat_dterm[axis] = dterm; \
}
int32_t unittest_pidMultiWiiRewrite_lastErrorForDelta[3];
int32_t unittest_pidMultiWiiRewrite_pterm[3];
int32_t unittest_pidMultiWiiRewrite_iterm[3];
int32_t unittest_pidMultiWiiRewrite_dterm[3];
#define SET_PID_MULTI_WII_REWRITE_LOCALS(axis) \
{ \
lastErrorForDelta[axis] = unittest_pidMultiWiiRewrite_lastErrorForDelta[axis]; \
}
#define GET_PID_MULTI_WII_REWRITE_LOCALS(axis) \
{ \
unittest_pidMultiWiiRewrite_lastErrorForDelta[axis] = lastErrorForDelta[axis]; \
unittest_pidMultiWiiRewrite_pterm[axis] = pterm; \
unittest_pidMultiWiiRewrite_iterm[axis] = iterm; \
unittest_pidMultiWiiRewrite_dterm[axis] = dterm; \
}
#else
#define SET_PID_LUX_FLOAT_LOCALS(axis) {}
#define GET_PID_LUX_FLOAT_LOCALS(axis) {}
#define SET_PID_MULTI_WII_REWRITE_LOCALS(axis) {}
#define GET_PID_MULTI_WII_REWRITE_LOCALS(axis) {}
#endif // UNIT_TEST
#endif // SRC_MAIN_FLIGHT_PID_C_

18
src/main/fc/core.c
View file

@ -25,11 +25,11 @@
#include "platform.h" #include "platform.h"
#include "build/debug.h"
#include "blackbox/blackbox.h" #include "blackbox/blackbox.h"
#include "blackbox/blackbox_fielddefs.h" #include "blackbox/blackbox_fielddefs.h"
#include "build/debug.h"
#include "cli/cli.h" #include "cli/cli.h"
#include "cms/cms.h" #include "cms/cms.h"
@ -39,6 +39,7 @@
#include "common/maths.h" #include "common/maths.h"
#include "common/utils.h" #include "common/utils.h"
#include "config/config.h"
#include "config/feature.h" #include "config/feature.h"
#include "drivers/dshot.h" #include "drivers/dshot.h"
@ -50,21 +51,20 @@
#include "drivers/time.h" #include "drivers/time.h"
#include "drivers/transponder_ir.h" #include "drivers/transponder_ir.h"
#include "config/config.h"
#include "fc/controlrate_profile.h" #include "fc/controlrate_profile.h"
#include "fc/core.h"
#include "fc/rc.h" #include "fc/rc.h"
#include "fc/rc_adjustments.h" #include "fc/rc_adjustments.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/stats.h" #include "fc/stats.h"
#include "fc/tasks.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/gps_rescue.h" #include "flight/gps_rescue.h"
#if defined(USE_GYRO_DATA_ANALYSE) #if defined(USE_GYRO_DATA_ANALYSE)
#include "flight/gyroanalyse.h" #include "flight/gyroanalyse.h"
#endif #endif
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/pid.h" #include "flight/pid.h"
@ -105,6 +105,8 @@
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
#include "core.h"
enum { enum {
ALIGN_GYRO = 0, ALIGN_GYRO = 0,
@ -321,7 +323,7 @@ void updateArmingStatus(void)
unsetArmingDisabled(ARMING_DISABLED_ANGLE); unsetArmingDisabled(ARMING_DISABLED_ANGLE);
} }
if (averageSystemLoadPercent > 100) { if (getAverageSystemLoadPercent() > LOAD_PERCENTAGE_ONE) {
setArmingDisabled(ARMING_DISABLED_LOAD); setArmingDisabled(ARMING_DISABLED_LOAD);
} else { } else {
unsetArmingDisabled(ARMING_DISABLED_LOAD); unsetArmingDisabled(ARMING_DISABLED_LOAD);
@ -1280,8 +1282,8 @@ FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
subTaskPidSubprocesses(currentTimeUs); subTaskPidSubprocesses(currentTimeUs);
if (debugMode == DEBUG_CYCLETIME) { if (debugMode == DEBUG_CYCLETIME) {
debug[0] = getTaskDeltaTime(TASK_SELF); DEBUG_SET(DEBUG_CYCLETIME, 0, getTaskDeltaTimeUs(TASK_SELF));
debug[1] = averageSystemLoadPercent; DEBUG_SET(DEBUG_CYCLETIME, 1, getAverageSystemLoadPercent());
} }
} }

4
src/main/fc/rc_controls.c
View file

@ -157,8 +157,8 @@ void processRcStickPositions()
} }
} }
if (stTmp == rcSticks) { if (stTmp == rcSticks) {
if (rcDelayMs <= INT16_MAX - (getTaskDeltaTime(TASK_SELF) / 1000)) { if (rcDelayMs <= INT16_MAX - (getTaskDeltaTimeUs(TASK_SELF) / 1000)) {
rcDelayMs += getTaskDeltaTime(TASK_SELF) / 1000; rcDelayMs += getTaskDeltaTimeUs(TASK_SELF) / 1000;
} }
} else { } else {
rcDelayMs = 0; rcDelayMs = 0;

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

@ -360,20 +360,20 @@ void tasksInit(void)
.subTaskName = subTaskNameParam, \ .subTaskName = subTaskNameParam, \
.checkFunc = checkFuncParam, \ .checkFunc = checkFuncParam, \
.taskFunc = taskFuncParam, \ .taskFunc = taskFuncParam, \
.desiredPeriod = desiredPeriodParam, \ .desiredPeriodUs = desiredPeriodParam, \
.staticPriority = staticPriorityParam \ .staticPriority = staticPriorityParam \
} }
#else #else
#define DEFINE_TASK(taskNameParam, subTaskNameParam, checkFuncParam, taskFuncParam, desiredPeriodParam, staticPriorityParam) { \ #define DEFINE_TASK(taskNameParam, subTaskNameParam, checkFuncParam, taskFuncParam, desiredPeriodParam, staticPriorityParam) { \
.checkFunc = checkFuncParam, \ .checkFunc = checkFuncParam, \
.taskFunc = taskFuncParam, \ .taskFunc = taskFuncParam, \
.desiredPeriod = desiredPeriodParam, \ .desiredPeriodUs = desiredPeriodParam, \
.staticPriority = staticPriorityParam \ .staticPriority = staticPriorityParam \
} }
#endif #endif
cfTask_t cfTasks[TASK_COUNT] = { task_t tasks[TASK_COUNT] = {
[TASK_SYSTEM] = DEFINE_TASK("SYSTEM", "LOAD", NULL, taskSystemLoad, TASK_PERIOD_HZ(10), TASK_PRIORITY_MEDIUM_HIGH), [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_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_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
@ -471,3 +471,8 @@ cfTask_t cfTasks[TASK_COUNT] = {
[TASK_RANGEFINDER] = DEFINE_TASK("RANGEFINDER", NULL, NULL, rangefinderUpdate, TASK_PERIOD_HZ(10), TASK_PRIORITY_IDLE), [TASK_RANGEFINDER] = DEFINE_TASK("RANGEFINDER", NULL, NULL, rangefinderUpdate, TASK_PERIOD_HZ(10), TASK_PRIORITY_IDLE),
#endif #endif
}; };
task_t *getTask(unsigned taskId)
{
return &tasks[taskId];
}

3
src/main/fc/tasks.h
View file

@ -20,4 +20,7 @@
#pragma once #pragma once
#include "scheduler/scheduler.h"
void tasksInit(void); void tasksInit(void);
task_t *getTask(unsigned taskId);

12
src/main/io/dashboard.c
View file

@ -548,14 +548,14 @@ static void showTasksPage(void)
i2c_OLED_set_line(bus, rowIndex++); i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, "Task max avg mx% av%"); i2c_OLED_send_string(bus, "Task max avg mx% av%");
cfTaskInfo_t taskInfo; taskInfo_t taskInfo;
for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; ++taskId) { for (taskId_e taskId = 0; taskId < TASK_COUNT; ++taskId) {
getTaskInfo(taskId, &taskInfo); getTaskInfo(taskId, &taskInfo);
if (taskInfo.isEnabled && taskId != TASK_SERIAL) {// don't waste a line of the display showing serial taskInfo if (taskInfo.isEnabled && taskId != TASK_SERIAL) {// don't waste a line of the display showing serial taskInfo
const int taskFrequency = (int)(1000000.0f / ((float)taskInfo.latestDeltaTime)); const int taskFrequency = (int)(1000000.0f / ((float)taskInfo.latestDeltaTimeUs));
const int maxLoad = (taskInfo.maxExecutionTime * taskFrequency + 5000) / 10000; const int maxLoad = (taskInfo.maxExecutionTimeUs * taskFrequency + 5000) / 10000;
const int averageLoad = (taskInfo.averageExecutionTime * taskFrequency + 5000) / 10000; const int averageLoad = (taskInfo.averageExecutionTimeUs * taskFrequency + 5000) / 10000;
tfp_sprintf(lineBuffer, format, taskId, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, maxLoad, averageLoad); tfp_sprintf(lineBuffer, format, taskId, taskInfo.maxExecutionTimeUs, taskInfo.averageExecutionTimeUs, maxLoad, averageLoad);
padLineBuffer(); padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++); i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer); i2c_OLED_send_string(bus, lineBuffer);

4
src/main/msp/msp.c
View file

@ -986,7 +986,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
boxBitmask_t flightModeFlags; boxBitmask_t flightModeFlags;
const int flagBits = packFlightModeFlags(&flightModeFlags); const int flagBits = packFlightModeFlags(&flightModeFlags);
sbufWriteU16(dst, getTaskDeltaTime(TASK_PID)); sbufWriteU16(dst, getTaskDeltaTimeUs(TASK_PID));
#ifdef USE_I2C #ifdef USE_I2C
sbufWriteU16(dst, i2cGetErrorCounter()); sbufWriteU16(dst, i2cGetErrorCounter());
#else #else
@ -995,7 +995,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_RANGEFINDER) << 4 | sensors(SENSOR_GYRO) << 5); sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_RANGEFINDER) << 4 | sensors(SENSOR_GYRO) << 5);
sbufWriteData(dst, &flightModeFlags, 4); // unconditional part of flags, first 32 bits sbufWriteData(dst, &flightModeFlags, 4); // unconditional part of flags, first 32 bits
sbufWriteU8(dst, getCurrentPidProfileIndex()); sbufWriteU8(dst, getCurrentPidProfileIndex());
sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); sbufWriteU16(dst, constrain(getAverageSystemLoadPercent(), 0, LOAD_PERCENTAGE_ONE));
if (cmdMSP == MSP_STATUS_EX) { if (cmdMSP == MSP_STATUS_EX) {
sbufWriteU8(dst, PID_PROFILE_COUNT); sbufWriteU8(dst, PID_PROFILE_COUNT);
sbufWriteU8(dst, getCurrentControlRateProfileIndex()); sbufWriteU8(dst, getCurrentControlRateProfileIndex());

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

@ -29,10 +29,6 @@
#include "build/build_config.h" #include "build/build_config.h"
#include "build/debug.h" #include "build/debug.h"
#include "scheduler/scheduler.h"
#include "config/config_unittest.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/time.h" #include "common/time.h"
#include "common/utils.h" #include "common/utils.h"
@ -40,6 +36,9 @@
#include "drivers/time.h" #include "drivers/time.h"
#include "fc/core.h" #include "fc/core.h"
#include "fc/tasks.h"
#include "scheduler.h"
#define TASK_AVERAGE_EXECUTE_FALLBACK_US 30 // Default task average time if USE_TASK_STATISTICS is not defined #define TASK_AVERAGE_EXECUTE_FALLBACK_US 30 // Default task average time if USE_TASK_STATISTICS is not defined
#define TASK_AVERAGE_EXECUTE_PADDING_US 5 // Add a little padding to the average execution time #define TASK_AVERAGE_EXECUTE_PADDING_US 5 // Add a little padding to the average execution time
@ -50,7 +49,7 @@
// 2 - time spent in scheduler // 2 - time spent in scheduler
// 3 - time spent executing check function // 3 - time spent executing check function
static FAST_RAM_ZERO_INIT cfTask_t *currentTask = NULL; static FAST_RAM_ZERO_INIT task_t *currentTask = NULL;
static FAST_RAM_ZERO_INIT uint32_t totalWaitingTasks; static FAST_RAM_ZERO_INIT uint32_t totalWaitingTasks;
static FAST_RAM_ZERO_INIT uint32_t totalWaitingTasksSamples; static FAST_RAM_ZERO_INIT uint32_t totalWaitingTasksSamples;
@ -61,12 +60,12 @@ FAST_RAM_ZERO_INIT uint16_t averageSystemLoadPercent = 0;
static FAST_RAM_ZERO_INIT int taskQueuePos = 0; static FAST_RAM_ZERO_INIT int taskQueuePos = 0;
STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT int taskQueueSize = 0; STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT int taskQueueSize = 0;
static FAST_RAM int periodCalculationBasisOffset = offsetof(cfTask_t, lastExecutedAt); static FAST_RAM int periodCalculationBasisOffset = offsetof(task_t, lastExecutedAtUs);
static FAST_RAM_ZERO_INIT bool gyroEnabled; static FAST_RAM_ZERO_INIT bool gyroEnabled;
// No need for a linked list for the queue, since items are only inserted at startup // 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 STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT task_t* taskQueueArray[TASK_COUNT + 1]; // extra item for NULL pointer at end of queue
void queueClear(void) void queueClear(void)
{ {
@ -75,7 +74,7 @@ void queueClear(void)
taskQueueSize = 0; taskQueueSize = 0;
} }
bool queueContains(cfTask_t *task) bool queueContains(task_t *task)
{ {
for (int ii = 0; ii < taskQueueSize; ++ii) { for (int ii = 0; ii < taskQueueSize; ++ii) {
if (taskQueueArray[ii] == task) { if (taskQueueArray[ii] == task) {
@ -85,7 +84,7 @@ bool queueContains(cfTask_t *task)
return false; return false;
} }
bool queueAdd(cfTask_t *task) bool queueAdd(task_t *task)
{ {
if ((taskQueueSize >= TASK_COUNT) || queueContains(task)) { if ((taskQueueSize >= TASK_COUNT) || queueContains(task)) {
return false; return false;
@ -101,7 +100,7 @@ bool queueAdd(cfTask_t *task)
return false; return false;
} }
bool queueRemove(cfTask_t *task) bool queueRemove(task_t *task)
{ {
for (int ii = 0; ii < taskQueueSize; ++ii) { for (int ii = 0; ii < taskQueueSize; ++ii) {
if (taskQueueArray[ii] == task) { if (taskQueueArray[ii] == task) {
@ -116,7 +115,7 @@ bool queueRemove(cfTask_t *task)
/* /*
* Returns first item queue or NULL if queue empty * Returns first item queue or NULL if queue empty
*/ */
FAST_CODE cfTask_t *queueFirst(void) FAST_CODE task_t *queueFirst(void)
{ {
taskQueuePos = 0; taskQueuePos = 0;
return taskQueueArray[0]; // guaranteed to be NULL if queue is empty return taskQueueArray[0]; // guaranteed to be NULL if queue is empty
@ -125,7 +124,7 @@ FAST_CODE cfTask_t *queueFirst(void)
/* /*
* Returns next item in queue or NULL if at end of queue * Returns next item in queue or NULL if at end of queue
*/ */
FAST_CODE cfTask_t *queueNext(void) FAST_CODE task_t *queueNext(void)
{ {
return taskQueueArray[++taskQueuePos]; // guaranteed to be NULL at end of queue return taskQueueArray[++taskQueuePos]; // guaranteed to be NULL at end of queue
} }
@ -146,52 +145,52 @@ void taskSystemLoad(timeUs_t currentTimeUs)
} }
#if defined(USE_TASK_STATISTICS) #if defined(USE_TASK_STATISTICS)
timeUs_t checkFuncMaxExecutionTime; timeUs_t checkFuncMaxExecutionTimeUs;
timeUs_t checkFuncTotalExecutionTime; timeUs_t checkFuncTotalExecutionTimeUs;
timeUs_t checkFuncMovingSumExecutionTime; timeUs_t checkFuncMovingSumExecutionTimeUs;
timeUs_t checkFuncMovingSumDeltaTime; timeUs_t checkFuncMovingSumDeltaTimeUs;
void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo) void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo)
{ {
checkFuncInfo->maxExecutionTime = checkFuncMaxExecutionTime; checkFuncInfo->maxExecutionTimeUs = checkFuncMaxExecutionTimeUs;
checkFuncInfo->totalExecutionTime = checkFuncTotalExecutionTime; checkFuncInfo->totalExecutionTimeUs = checkFuncTotalExecutionTimeUs;
checkFuncInfo->averageExecutionTime = checkFuncMovingSumExecutionTime / TASK_STATS_MOVING_SUM_COUNT; checkFuncInfo->averageExecutionTimeUs = checkFuncMovingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
checkFuncInfo->averageDeltaTime = checkFuncMovingSumDeltaTime / TASK_STATS_MOVING_SUM_COUNT; checkFuncInfo->averageDeltaTimeUs = checkFuncMovingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
} }
#endif #endif
void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo) void getTaskInfo(taskId_e taskId, taskInfo_t * taskInfo)
{ {
taskInfo->isEnabled = queueContains(&cfTasks[taskId]); taskInfo->isEnabled = queueContains(getTask(taskId));
taskInfo->desiredPeriod = cfTasks[taskId].desiredPeriod; taskInfo->desiredPeriodUs = getTask(taskId)->desiredPeriodUs;
taskInfo->staticPriority = cfTasks[taskId].staticPriority; taskInfo->staticPriority = getTask(taskId)->staticPriority;
#if defined(USE_TASK_STATISTICS) #if defined(USE_TASK_STATISTICS)
taskInfo->taskName = cfTasks[taskId].taskName; taskInfo->taskName = getTask(taskId)->taskName;
taskInfo->subTaskName = cfTasks[taskId].subTaskName; taskInfo->subTaskName = getTask(taskId)->subTaskName;
taskInfo->maxExecutionTime = cfTasks[taskId].maxExecutionTime; taskInfo->maxExecutionTimeUs = getTask(taskId)->maxExecutionTimeUs;
taskInfo->totalExecutionTime = cfTasks[taskId].totalExecutionTime; taskInfo->totalExecutionTimeUs = getTask(taskId)->totalExecutionTimeUs;
taskInfo->averageExecutionTime = cfTasks[taskId].movingSumExecutionTime / TASK_STATS_MOVING_SUM_COUNT; taskInfo->averageExecutionTimeUs = getTask(taskId)->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
taskInfo->averageDeltaTime = cfTasks[taskId].movingSumDeltaTime / TASK_STATS_MOVING_SUM_COUNT; taskInfo->averageDeltaTimeUs = getTask(taskId)->movingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
taskInfo->latestDeltaTime = cfTasks[taskId].taskLatestDeltaTime; taskInfo->latestDeltaTimeUs = getTask(taskId)->taskLatestDeltaTimeUs;
taskInfo->movingAverageCycleTime = cfTasks[taskId].movingAverageCycleTime; taskInfo->movingAverageCycleTimeUs = getTask(taskId)->movingAverageCycleTimeUs;
#endif #endif
} }
void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros) void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs)
{ {
if (taskId == TASK_SELF) { if (taskId == TASK_SELF) {
cfTask_t *task = currentTask; task_t *task = currentTask;
task->desiredPeriod = MAX(SCHEDULER_DELAY_LIMIT, (timeDelta_t)newPeriodMicros); // Limit delay to 100us (10 kHz) to prevent scheduler clogging task->desiredPeriodUs = MAX(SCHEDULER_DELAY_LIMIT, newPeriodUs); // Limit delay to 100us (10 kHz) to prevent scheduler clogging
} else if (taskId < TASK_COUNT) { } else if (taskId < TASK_COUNT) {
cfTask_t *task = &cfTasks[taskId]; task_t *task = getTask(taskId);
task->desiredPeriod = MAX(SCHEDULER_DELAY_LIMIT, (timeDelta_t)newPeriodMicros); // Limit delay to 100us (10 kHz) to prevent scheduler clogging task->desiredPeriodUs = MAX(SCHEDULER_DELAY_LIMIT, newPeriodUs); // Limit delay to 100us (10 kHz) to prevent scheduler clogging
} }
} }
void setTaskEnabled(cfTaskId_e taskId, bool enabled) void setTaskEnabled(taskId_e taskId, bool enabled)
{ {
if (taskId == TASK_SELF || taskId < TASK_COUNT) { if (taskId == TASK_SELF || taskId < TASK_COUNT) {
cfTask_t *task = taskId == TASK_SELF ? currentTask : &cfTasks[taskId]; task_t *task = taskId == TASK_SELF ? currentTask : getTask(taskId);
if (enabled && task->taskFunc) { if (enabled && task->taskFunc) {
queueAdd(task); queueAdd(task);
} else { } else {
@ -200,12 +199,12 @@ void setTaskEnabled(cfTaskId_e taskId, bool enabled)
} }
} }
timeDelta_t getTaskDeltaTime(cfTaskId_e taskId) timeDelta_t getTaskDeltaTimeUs(taskId_e taskId)
{ {
if (taskId == TASK_SELF) { if (taskId == TASK_SELF) {
return currentTask->taskLatestDeltaTime; return currentTask->taskLatestDeltaTimeUs;
} else if (taskId < TASK_COUNT) { } else if (taskId < TASK_COUNT) {
return cfTasks[taskId].taskLatestDeltaTime; return getTask(taskId)->taskLatestDeltaTimeUs;
} else { } else {
return 0; return 0;
} }
@ -216,32 +215,32 @@ void schedulerSetCalulateTaskStatistics(bool calculateTaskStatisticsToUse)
calculateTaskStatistics = calculateTaskStatisticsToUse; calculateTaskStatistics = calculateTaskStatisticsToUse;
} }
void schedulerResetTaskStatistics(cfTaskId_e taskId) void schedulerResetTaskStatistics(taskId_e taskId)
{ {
#if defined(USE_TASK_STATISTICS) #if defined(USE_TASK_STATISTICS)
if (taskId == TASK_SELF) { if (taskId == TASK_SELF) {
currentTask->movingSumExecutionTime = 0; currentTask->movingSumExecutionTimeUs = 0;
currentTask->movingSumDeltaTime = 0; currentTask->movingSumDeltaTimeUs = 0;
currentTask->totalExecutionTime = 0; currentTask->totalExecutionTimeUs = 0;
currentTask->maxExecutionTime = 0; currentTask->maxExecutionTimeUs = 0;
} else if (taskId < TASK_COUNT) { } else if (taskId < TASK_COUNT) {
cfTasks[taskId].movingSumExecutionTime = 0; getTask(taskId)->movingSumExecutionTimeUs = 0;
cfTasks[taskId].movingSumDeltaTime = 0; getTask(taskId)->movingSumDeltaTimeUs = 0;
cfTasks[taskId].totalExecutionTime = 0; getTask(taskId)->totalExecutionTimeUs = 0;
cfTasks[taskId].maxExecutionTime = 0; getTask(taskId)->maxExecutionTimeUs = 0;
} }
#else #else
UNUSED(taskId); UNUSED(taskId);
#endif #endif
} }
void schedulerResetTaskMaxExecutionTime(cfTaskId_e taskId) void schedulerResetTaskMaxExecutionTime(taskId_e taskId)
{ {
#if defined(USE_TASK_STATISTICS) #if defined(USE_TASK_STATISTICS)
if (taskId == TASK_SELF) { if (taskId == TASK_SELF) {
currentTask->maxExecutionTime = 0; currentTask->maxExecutionTimeUs = 0;
} else if (taskId < TASK_COUNT) { } else if (taskId < TASK_COUNT) {
cfTasks[taskId].maxExecutionTime = 0; getTask(taskId)->maxExecutionTimeUs = 0;
} }
#else #else
UNUSED(taskId); UNUSED(taskId);
@ -251,7 +250,7 @@ void schedulerResetTaskMaxExecutionTime(cfTaskId_e taskId)
#if defined(USE_TASK_STATISTICS) #if defined(USE_TASK_STATISTICS)
void schedulerResetCheckFunctionMaxExecutionTime(void) void schedulerResetCheckFunctionMaxExecutionTime(void)
{ {
checkFuncMaxExecutionTime = 0; checkFuncMaxExecutionTimeUs = 0;
} }
#endif #endif
@ -259,48 +258,48 @@ void schedulerInit(void)
{ {
calculateTaskStatistics = true; calculateTaskStatistics = true;
queueClear(); queueClear();
queueAdd(&cfTasks[TASK_SYSTEM]); queueAdd(getTask(TASK_SYSTEM));
} }
void schedulerOptimizeRate(bool optimizeRate) void schedulerOptimizeRate(bool optimizeRate)
{ {
periodCalculationBasisOffset = optimizeRate ? offsetof(cfTask_t, lastDesiredAt) : offsetof(cfTask_t, lastExecutedAt); periodCalculationBasisOffset = optimizeRate ? offsetof(task_t, lastDesiredAt) : offsetof(task_t, lastExecutedAtUs);
} }
inline static timeUs_t getPeriodCalculationBasis(const cfTask_t* task) inline static timeUs_t getPeriodCalculationBasis(const task_t* task)
{ {
if (task->staticPriority == TASK_PRIORITY_REALTIME) { if (task->staticPriority == TASK_PRIORITY_REALTIME) {
return *(timeUs_t*)((uint8_t*)task + periodCalculationBasisOffset); return *(timeUs_t*)((uint8_t*)task + periodCalculationBasisOffset);
} else { } else {
return task->lastExecutedAt; return task->lastExecutedAtUs;
} }
} }
FAST_CODE timeUs_t schedulerExecuteTask(cfTask_t *selectedTask, timeUs_t currentTimeUs) FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTimeUs)
{ {
timeUs_t taskExecutionTime = 0; timeUs_t taskExecutionTimeUs = 0;
if (selectedTask) { if (selectedTask) {
currentTask = selectedTask; currentTask = selectedTask;
selectedTask->taskLatestDeltaTime = currentTimeUs - selectedTask->lastExecutedAt; selectedTask->taskLatestDeltaTimeUs = cmpTimeUs(currentTimeUs, selectedTask->lastExecutedAtUs);
#if defined(USE_TASK_STATISTICS) #if defined(USE_TASK_STATISTICS)
float period = currentTimeUs - selectedTask->lastExecutedAt; float period = currentTimeUs - selectedTask->lastExecutedAtUs;
#endif #endif
selectedTask->lastExecutedAt = currentTimeUs; selectedTask->lastExecutedAtUs = currentTimeUs;
selectedTask->lastDesiredAt += (cmpTimeUs(currentTimeUs, selectedTask->lastDesiredAt) / selectedTask->desiredPeriod) * selectedTask->desiredPeriod; selectedTask->lastDesiredAt += (cmpTimeUs(currentTimeUs, selectedTask->lastDesiredAt) / selectedTask->desiredPeriodUs) * selectedTask->desiredPeriodUs;
selectedTask->dynamicPriority = 0; selectedTask->dynamicPriority = 0;
// Execute task // Execute task
#if defined(USE_TASK_STATISTICS) #if defined(USE_TASK_STATISTICS)
if (calculateTaskStatistics) { if (calculateTaskStatistics) {
const timeUs_t currentTimeBeforeTaskCall = micros(); const timeUs_t currentTimeBeforeTaskCallUs = micros();
selectedTask->taskFunc(currentTimeBeforeTaskCall); selectedTask->taskFunc(currentTimeBeforeTaskCallUs);
taskExecutionTime = micros() - currentTimeBeforeTaskCall; taskExecutionTimeUs = micros() - currentTimeBeforeTaskCallUs;
selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / TASK_STATS_MOVING_SUM_COUNT; selectedTask->movingSumExecutionTimeUs += taskExecutionTimeUs - selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
selectedTask->movingSumDeltaTime += selectedTask->taskLatestDeltaTime - selectedTask->movingSumDeltaTime / TASK_STATS_MOVING_SUM_COUNT; selectedTask->movingSumDeltaTimeUs += selectedTask->taskLatestDeltaTimeUs - selectedTask->movingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task selectedTask->totalExecutionTimeUs += taskExecutionTimeUs; // time consumed by scheduler + task
selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime); selectedTask->maxExecutionTimeUs = MAX(selectedTask->maxExecutionTimeUs, taskExecutionTimeUs);
selectedTask->movingAverageCycleTime += 0.05f * (period - selectedTask->movingAverageCycleTime); selectedTask->movingAverageCycleTimeUs += 0.05f * (period - selectedTask->movingAverageCycleTimeUs);
} else } else
#endif #endif
{ {
@ -308,16 +307,29 @@ FAST_CODE timeUs_t schedulerExecuteTask(cfTask_t *selectedTask, timeUs_t current
} }
} }
return taskExecutionTime; return taskExecutionTimeUs;
} }
#if defined(UNIT_TEST)
task_t *unittest_scheduler_selectedTask;
uint8_t unittest_scheduler_selectedTaskDynamicPriority;
uint16_t unittest_scheduler_waitingTasks;
static void readSchedulerLocals(task_t *selectedTask, uint8_t selectedTaskDynamicPriority, uint16_t waitingTasks)
{
unittest_scheduler_selectedTask = selectedTask;
unittest_scheduler_selectedTaskDynamicPriority = selectedTaskDynamicPriority;
unittest_scheduler_waitingTasks = waitingTasks;
}
#endif
FAST_CODE void scheduler(void) FAST_CODE void scheduler(void)
{ {
// Cache currentTime // Cache currentTime
const timeUs_t schedulerStartTimeUs = micros(); const timeUs_t schedulerStartTimeUs = micros();
timeUs_t currentTimeUs = schedulerStartTimeUs; timeUs_t currentTimeUs = schedulerStartTimeUs;
timeUs_t taskExecutionTime = 0; timeUs_t taskExecutionTimeUs = 0;
cfTask_t *selectedTask = NULL; task_t *selectedTask = NULL;
uint16_t selectedTaskDynamicPriority = 0; uint16_t selectedTaskDynamicPriority = 0;
uint16_t waitingTasks = 0; uint16_t waitingTasks = 0;
bool realtimeTaskRan = false; bool realtimeTaskRan = false;
@ -325,16 +337,16 @@ FAST_CODE void scheduler(void)
if (gyroEnabled) { if (gyroEnabled) {
// Realtime gyro/filtering/PID tasks get complete priority // Realtime gyro/filtering/PID tasks get complete priority
cfTask_t *gyroTask = &cfTasks[TASK_GYRO]; task_t *gyroTask = getTask(TASK_GYRO);
const timeUs_t gyroExecuteTime = getPeriodCalculationBasis(gyroTask) + gyroTask->desiredPeriod; const timeUs_t gyroExecuteTimeUs = getPeriodCalculationBasis(gyroTask) + gyroTask->desiredPeriodUs;
gyroTaskDelayUs = cmpTimeUs(gyroExecuteTime, currentTimeUs); // time until the next expected gyro sample gyroTaskDelayUs = cmpTimeUs(gyroExecuteTimeUs, currentTimeUs); // time until the next expected gyro sample
if (cmpTimeUs(currentTimeUs, gyroExecuteTime) >= 0) { if (cmpTimeUs(currentTimeUs, gyroExecuteTimeUs) >= 0) {
taskExecutionTime = schedulerExecuteTask(gyroTask, currentTimeUs); taskExecutionTimeUs = schedulerExecuteTask(gyroTask, currentTimeUs);
if (gyroFilterReady()) { if (gyroFilterReady()) {
taskExecutionTime += schedulerExecuteTask(&cfTasks[TASK_FILTER], currentTimeUs); taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_FILTER), currentTimeUs);
} }
if (pidLoopReady()) { if (pidLoopReady()) {
taskExecutionTime += schedulerExecuteTask(&cfTasks[TASK_PID], currentTimeUs); taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_PID), currentTimeUs);
} }
currentTimeUs = micros(); currentTimeUs = micros();
realtimeTaskRan = true; realtimeTaskRan = true;
@ -345,34 +357,34 @@ FAST_CODE void scheduler(void)
// The task to be invoked // The task to be invoked
// Update task dynamic priorities // Update task dynamic priorities
for (cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) { for (task_t *task = queueFirst(); task != NULL; task = queueNext()) {
if (task->staticPriority != TASK_PRIORITY_REALTIME) { if (task->staticPriority != TASK_PRIORITY_REALTIME) {
// Task has checkFunc - event driven // Task has checkFunc - event driven
if (task->checkFunc) { if (task->checkFunc) {
#if defined(SCHEDULER_DEBUG) #if defined(SCHEDULER_DEBUG)
const timeUs_t currentTimeBeforeCheckFuncCall = micros(); const timeUs_t currentTimeBeforeCheckFuncCallUs = micros();
#else #else
const timeUs_t currentTimeBeforeCheckFuncCall = currentTimeUs; const timeUs_t currentTimeBeforeCheckFuncCallUs = currentTimeUs;
#endif #endif
// Increase priority for event driven tasks // Increase priority for event driven tasks
if (task->dynamicPriority > 0) { if (task->dynamicPriority > 0) {
task->taskAgeCycles = 1 + ((currentTimeUs - task->lastSignaledAt) / task->desiredPeriod); task->taskAgeCycles = 1 + ((currentTimeUs - task->lastSignaledAtUs) / task->desiredPeriodUs);
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles; task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
waitingTasks++; waitingTasks++;
} else if (task->checkFunc(currentTimeBeforeCheckFuncCall, currentTimeBeforeCheckFuncCall - task->lastExecutedAt)) { } else if (task->checkFunc(currentTimeBeforeCheckFuncCallUs, cmpTimeUs(currentTimeBeforeCheckFuncCallUs, task->lastExecutedAtUs))) {
#if defined(SCHEDULER_DEBUG) #if defined(SCHEDULER_DEBUG)
DEBUG_SET(DEBUG_SCHEDULER, 3, micros() - currentTimeBeforeCheckFuncCall); DEBUG_SET(DEBUG_SCHEDULER, 3, micros() - currentTimeBeforeCheckFuncCallUs);
#endif #endif
#if defined(USE_TASK_STATISTICS) #if defined(USE_TASK_STATISTICS)
if (calculateTaskStatistics) { if (calculateTaskStatistics) {
const uint32_t checkFuncExecutionTime = micros() - currentTimeBeforeCheckFuncCall; const uint32_t checkFuncExecutionTimeUs = micros() - currentTimeBeforeCheckFuncCallUs;
checkFuncMovingSumExecutionTime += checkFuncExecutionTime - checkFuncMovingSumExecutionTime / TASK_STATS_MOVING_SUM_COUNT; checkFuncMovingSumExecutionTimeUs += checkFuncExecutionTimeUs - checkFuncMovingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
checkFuncMovingSumDeltaTime += task->taskLatestDeltaTime - checkFuncMovingSumDeltaTime / TASK_STATS_MOVING_SUM_COUNT; checkFuncMovingSumDeltaTimeUs += task->taskLatestDeltaTimeUs - checkFuncMovingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
checkFuncTotalExecutionTime += checkFuncExecutionTime; // time consumed by scheduler + task checkFuncTotalExecutionTimeUs += checkFuncExecutionTimeUs; // time consumed by scheduler + task
checkFuncMaxExecutionTime = MAX(checkFuncMaxExecutionTime, checkFuncExecutionTime); checkFuncMaxExecutionTimeUs = MAX(checkFuncMaxExecutionTimeUs, checkFuncExecutionTimeUs);
} }
#endif #endif
task->lastSignaledAt = currentTimeBeforeCheckFuncCall; task->lastSignaledAtUs = currentTimeBeforeCheckFuncCallUs;
task->taskAgeCycles = 1; task->taskAgeCycles = 1;
task->dynamicPriority = 1 + task->staticPriority; task->dynamicPriority = 1 + task->staticPriority;
waitingTasks++; waitingTasks++;
@ -382,7 +394,7 @@ FAST_CODE void scheduler(void)
} 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 = ((currentTimeUs - getPeriodCalculationBasis(task)) / task->desiredPeriod); task->taskAgeCycles = ((currentTimeUs - getPeriodCalculationBasis(task)) / task->desiredPeriodUs);
if (task->taskAgeCycles > 0) { if (task->taskAgeCycles > 0) {
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles; task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
waitingTasks++; waitingTasks++;
@ -403,13 +415,13 @@ FAST_CODE void scheduler(void)
timeDelta_t taskRequiredTimeUs = TASK_AVERAGE_EXECUTE_FALLBACK_US; // default average time if task statistics are not available timeDelta_t taskRequiredTimeUs = TASK_AVERAGE_EXECUTE_FALLBACK_US; // default average time if task statistics are not available
#if defined(USE_TASK_STATISTICS) #if defined(USE_TASK_STATISTICS)
if (calculateTaskStatistics) { if (calculateTaskStatistics) {
taskRequiredTimeUs = selectedTask->movingSumExecutionTime / TASK_STATS_MOVING_SUM_COUNT + TASK_AVERAGE_EXECUTE_PADDING_US; taskRequiredTimeUs = selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT + TASK_AVERAGE_EXECUTE_PADDING_US;
} }
#endif #endif
// Add in the time spent so far in check functions and the scheduler logic // Add in the time spent so far in check functions and the scheduler logic
taskRequiredTimeUs += cmpTimeUs(micros(), currentTimeUs); taskRequiredTimeUs += cmpTimeUs(micros(), currentTimeUs);
if (!gyroEnabled || realtimeTaskRan || (taskRequiredTimeUs < gyroTaskDelayUs)) { if (!gyroEnabled || realtimeTaskRan || (taskRequiredTimeUs < gyroTaskDelayUs)) {
taskExecutionTime += schedulerExecuteTask(selectedTask, currentTimeUs); taskExecutionTimeUs += schedulerExecuteTask(selectedTask, currentTimeUs);
} else { } else {
selectedTask = NULL; selectedTask = NULL;
} }
@ -418,12 +430,14 @@ FAST_CODE void scheduler(void)
#if defined(SCHEDULER_DEBUG) #if defined(SCHEDULER_DEBUG)
DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - schedulerStartTimeUs - taskExecutionTime); // time spent in scheduler DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - schedulerStartTimeUs - taskExecutionTimeUs); // time spent in scheduler
#else #else
UNUSED(taskExecutionTime); UNUSED(taskExecutionTimeUs);
#endif #endif
GET_SCHEDULER_LOCALS(); #if defined(UNIT_TEST)
readSchedulerLocals(selectedTask, selectedTaskDynamicPriority, waitingTasks);
#endif
} }
void schedulerEnableGyro(void) void schedulerEnableGyro(void)
@ -431,3 +445,7 @@ void schedulerEnableGyro(void)
gyroEnabled = true; gyroEnabled = true;
} }
uint16_t getAverageSystemLoadPercent(void)
{
return averageSystemLoadPercent;
}

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

@ -33,6 +33,8 @@
#define TASK_STATS_MOVING_SUM_COUNT 32 #define TASK_STATS_MOVING_SUM_COUNT 32
#endif #endif
#define LOAD_PERCENTAGE_ONE 100
typedef enum { typedef enum {
TASK_PRIORITY_REALTIME = -1, // Task will be run outside the scheduler logic TASK_PRIORITY_REALTIME = -1, // Task will be run outside the scheduler logic
TASK_PRIORITY_IDLE = 0, // Disables dynamic scheduling, task is executed only if no other task is active this cycle TASK_PRIORITY_IDLE = 0, // Disables dynamic scheduling, task is executed only if no other task is active this cycle
@ -41,13 +43,13 @@ typedef enum {
TASK_PRIORITY_MEDIUM_HIGH = 4, TASK_PRIORITY_MEDIUM_HIGH = 4,
TASK_PRIORITY_HIGH = 5, TASK_PRIORITY_HIGH = 5,
TASK_PRIORITY_MAX = 255 TASK_PRIORITY_MAX = 255
} cfTaskPriority_e; } taskPriority_e;
typedef struct { typedef struct {
timeUs_t maxExecutionTime; timeUs_t maxExecutionTimeUs;
timeUs_t totalExecutionTime; timeUs_t totalExecutionTimeUs;
timeUs_t averageExecutionTime; timeUs_t averageExecutionTimeUs;
timeUs_t averageDeltaTime; timeUs_t averageDeltaTimeUs;
} cfCheckFuncInfo_t; } cfCheckFuncInfo_t;
typedef struct { typedef struct {
@ -55,14 +57,14 @@ typedef struct {
const char * subTaskName; const char * subTaskName;
bool isEnabled; bool isEnabled;
int8_t staticPriority; int8_t staticPriority;
timeDelta_t desiredPeriod; timeDelta_t desiredPeriodUs;
timeDelta_t latestDeltaTime; timeDelta_t latestDeltaTimeUs;
timeUs_t maxExecutionTime; timeUs_t maxExecutionTimeUs;
timeUs_t totalExecutionTime; timeUs_t totalExecutionTimeUs;
timeUs_t averageExecutionTime; timeUs_t averageExecutionTimeUs;
timeUs_t averageDeltaTime; timeUs_t averageDeltaTimeUs;
float movingAverageCycleTime; float movingAverageCycleTimeUs;
} cfTaskInfo_t; } taskInfo_t;
typedef enum { typedef enum {
/* Actual tasks */ /* Actual tasks */
@ -149,7 +151,7 @@ typedef enum {
/* Service task IDs */ /* Service task IDs */
TASK_NONE = TASK_COUNT, TASK_NONE = TASK_COUNT,
TASK_SELF TASK_SELF
} cfTaskId_e; } taskId_e;
typedef struct { typedef struct {
// Configuration // Configuration
@ -159,47 +161,41 @@ typedef struct {
#endif #endif
bool (*checkFunc)(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs); bool (*checkFunc)(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
void (*taskFunc)(timeUs_t currentTimeUs); void (*taskFunc)(timeUs_t currentTimeUs);
timeDelta_t desiredPeriod; // 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
// 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
uint16_t taskAgeCycles; uint16_t taskAgeCycles;
timeDelta_t taskLatestDeltaTime; timeDelta_t taskLatestDeltaTimeUs;
timeUs_t lastExecutedAt; // last time of invocation timeUs_t lastExecutedAtUs; // last time of invocation
timeUs_t lastSignaledAt; // time of invocation event for event-driven tasks timeUs_t lastSignaledAtUs; // time of invocation event for event-driven tasks
timeUs_t lastDesiredAt; // time of last desired execution timeUs_t lastDesiredAt; // time of last desired execution
#if defined(USE_TASK_STATISTICS) #if defined(USE_TASK_STATISTICS)
// Statistics // Statistics
float movingAverageCycleTime; float movingAverageCycleTimeUs;
timeUs_t movingSumExecutionTime; // moving sum over 32 samples timeUs_t movingSumExecutionTimeUs; // moving sum over 32 samples
timeUs_t movingSumDeltaTime; // moving sum over 32 samples timeUs_t movingSumDeltaTimeUs; // moving sum over 32 samples
timeUs_t maxExecutionTime; timeUs_t maxExecutionTimeUs;
timeUs_t totalExecutionTime; // total time consumed by task since boot timeUs_t totalExecutionTimeUs; // total time consumed by task since boot
#endif #endif
} cfTask_t; } task_t;
extern cfTask_t cfTasks[TASK_COUNT];
extern uint16_t averageSystemLoadPercent;
void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo); void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo);
void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t *taskInfo); void getTaskInfo(taskId_e taskId, taskInfo_t *taskInfo);
void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros); void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs);
void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState); void setTaskEnabled(taskId_e taskId, bool newEnabledState);
timeDelta_t getTaskDeltaTime(cfTaskId_e taskId); timeDelta_t getTaskDeltaTimeUs(taskId_e taskId);
void schedulerSetCalulateTaskStatistics(bool calculateTaskStatistics); void schedulerSetCalulateTaskStatistics(bool calculateTaskStatistics);
void schedulerResetTaskStatistics(cfTaskId_e taskId); void schedulerResetTaskStatistics(taskId_e taskId);
void schedulerResetTaskMaxExecutionTime(cfTaskId_e taskId); void schedulerResetTaskMaxExecutionTime(taskId_e taskId);
void schedulerResetCheckFunctionMaxExecutionTime(void); void schedulerResetCheckFunctionMaxExecutionTime(void);
void schedulerInit(void); void schedulerInit(void);
void scheduler(void); void scheduler(void);
timeUs_t schedulerExecuteTask(cfTask_t *selectedTask, timeUs_t currentTimeUs); timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTimeUs);
void taskSystemLoad(timeUs_t currentTime); void taskSystemLoad(timeUs_t currentTimeUs);
void schedulerOptimizeRate(bool optimizeRate); void schedulerOptimizeRate(bool optimizeRate);
void schedulerEnableGyro(void); void schedulerEnableGyro(void);
uint16_t getAverageSystemLoadPercent(void);
#define LOAD_PERCENTAGE_ONE 100
#define isSystemOverloaded() (averageSystemLoadPercent >= LOAD_PERCENTAGE_ONE)

16
src/main/sensors/rangefinder.c
View file

@ -31,29 +31,29 @@
#include "build/debug.h" #include "build/debug.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/utils.h"
#include "common/time.h" #include "common/time.h"
#include "common/utils.h"
#include "config/config.h"
#include "config/feature.h" #include "config/feature.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "drivers/io.h" #include "drivers/io.h"
#include "drivers/time.h"
#include "drivers/rangefinder/rangefinder.h" #include "drivers/rangefinder/rangefinder.h"
#include "drivers/rangefinder/rangefinder_hcsr04.h" #include "drivers/rangefinder/rangefinder_hcsr04.h"
#include "drivers/rangefinder/rangefinder_lidartf.h" #include "drivers/rangefinder/rangefinder_lidartf.h"
#include "drivers/time.h"
#include "config/config.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/tasks.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "scheduler/scheduler.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/rangefinder.h" #include "sensors/rangefinder.h"
#include "sensors/battery.h" #include "sensors/battery.h"
#include "scheduler/scheduler.h"
//#include "uav_interconnect/uav_interconnect.h" //#include "uav_interconnect/uav_interconnect.h"
// XXX Interface to CF/BF legacy(?) altitude estimation code. // XXX Interface to CF/BF legacy(?) altitude estimation code.

4
src/main/target/COLIBRI_RACE/i2c_bst.c
View file

@ -279,7 +279,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
} }
break; break;
case BST_STATUS: case BST_STATUS:
bstWrite16(getTaskDeltaTime(TASK_PID)); bstWrite16(getTaskDeltaTimeUs(TASK_PID));
#ifdef USE_I2C #ifdef USE_I2C
bstWrite16(i2cGetErrorCounter()); bstWrite16(i2cGetErrorCounter());
#else #else
@ -314,7 +314,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
bstWrite8(getCurrentPidProfileIndex()); bstWrite8(getCurrentPidProfileIndex());
break; break;
case BST_LOOP_TIME: case BST_LOOP_TIME:
bstWrite16(getTaskDeltaTime(TASK_PID)); bstWrite16(getTaskDeltaTimeUs(TASK_PID));
break; break;
case BST_RC_TUNING: case BST_RC_TUNING:
bstWrite8(currentControlRateProfile->rcRates[FD_ROLL]); bstWrite8(currentControlRateProfile->rcRates[FD_ROLL]);

4
src/main/telemetry/hott.c
View file

@ -470,9 +470,9 @@ static void hottPrepareMessages(void) {
static void hottTextmodeStart() static void hottTextmodeStart()
{ {
// Increase menu speed // Increase menu speed
cfTaskInfo_t taskInfo; taskInfo_t taskInfo;
getTaskInfo(TASK_TELEMETRY, &taskInfo); getTaskInfo(TASK_TELEMETRY, &taskInfo);
telemetryTaskPeriod = taskInfo.desiredPeriod; telemetryTaskPeriod = taskInfo.desiredPeriodUs;
rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(HOTT_TEXTMODE_TASK_PERIOD)); rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(HOTT_TEXTMODE_TASK_PERIOD));
rxSchedule = HOTT_TEXTMODE_RX_SCHEDULE; rxSchedule = HOTT_TEXTMODE_RX_SCHEDULE;

5
src/test/unit/arming_prevention_unittest.cc
View file

@ -1051,7 +1051,7 @@ extern "C" {
bool calculateRxChannelsAndUpdateFailsafe(timeUs_t) { return true; } bool calculateRxChannelsAndUpdateFailsafe(timeUs_t) { return true; }
bool isMixerUsingServos(void) { return false; } bool isMixerUsingServos(void) { return false; }
void gyroUpdate(void) {} void gyroUpdate(void) {}
timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 0; } timeDelta_t getTaskDeltaTimeUs(taskId_e) { return 0; }
void updateRSSI(timeUs_t) {} void updateRSSI(timeUs_t) {}
bool failsafeIsMonitoring(void) { return false; } bool failsafeIsMonitoring(void) { return false; }
void failsafeStartMonitoring(void) {} void failsafeStartMonitoring(void) {}
@ -1082,7 +1082,7 @@ extern "C" {
void dashboardEnablePageCycling(void) {} void dashboardEnablePageCycling(void) {}
void dashboardDisablePageCycling(void) {} void dashboardDisablePageCycling(void) {}
bool imuQuaternionHeadfreeOffsetSet(void) { return true; } bool imuQuaternionHeadfreeOffsetSet(void) { return true; }
void rescheduleTask(cfTaskId_e, uint32_t) {} void rescheduleTask(taskId_e, timeDelta_t) {}
bool usbCableIsInserted(void) { return false; } bool usbCableIsInserted(void) { return false; }
bool usbVcpIsConnected(void) { return false; } bool usbVcpIsConnected(void) { return false; }
void pidSetAntiGravityState(bool) {} void pidSetAntiGravityState(bool) {}
@ -1103,4 +1103,5 @@ extern "C" {
void gyroFiltering(timeUs_t) {}; void gyroFiltering(timeUs_t) {};
timeDelta_t rxGetFrameDelta(timeDelta_t *) { return 0; } timeDelta_t rxGetFrameDelta(timeDelta_t *) { return 0; }
void updateRcRefreshRate(timeUs_t) {}; void updateRcRefreshRate(timeUs_t) {};
uint16_t getAverageSystemLoadPercent(void) { return 0; }
} }

7
src/test/unit/cli_unittest.cc
View file

@ -299,7 +299,7 @@ uint8_t __config_start = 0x00;
uint8_t __config_end = 0x10; uint8_t __config_end = 0x10;
uint16_t averageSystemLoadPercent = 0; uint16_t averageSystemLoadPercent = 0;
timeDelta_t getTaskDeltaTime(cfTaskId_e){ return 0; } timeDelta_t getTaskDeltaTimeUs(taskId_e){ return 0; }
uint16_t currentRxRefreshRate = 9000; uint16_t currentRxRefreshRate = 9000;
armingDisableFlags_e getArmingDisableFlags(void) { return ARMING_DISABLED_NO_GYRO; } armingDisableFlags_e getArmingDisableFlags(void) { return ARMING_DISABLED_NO_GYRO; }
@ -307,9 +307,9 @@ const char *armingDisableFlagNames[]= {
"DUMMYDISABLEFLAGNAME" "DUMMYDISABLEFLAGNAME"
}; };
void getTaskInfo(cfTaskId_e, cfTaskInfo_t *) {} void getTaskInfo(taskId_e, taskInfo_t *) {}
void getCheckFuncInfo(cfCheckFuncInfo_t *) {} void getCheckFuncInfo(cfCheckFuncInfo_t *) {}
void schedulerResetTaskMaxExecutionTime(cfTaskId_e) {} void schedulerResetTaskMaxExecutionTime(taskId_e) {}
void schedulerResetCheckFunctionMaxExecutionTime(void) {} void schedulerResetCheckFunctionMaxExecutionTime(void) {}
const char * const targetName = "UNITTEST"; const char * const targetName = "UNITTEST";
@ -360,4 +360,5 @@ void delay(uint32_t) {}
displayPort_t *osdGetDisplayPort(osdDisplayPortDevice_e *) { return NULL; } displayPort_t *osdGetDisplayPort(osdDisplayPortDevice_e *) { return NULL; }
mcuTypeId_e getMcuTypeId(void) { return MCU_TYPE_UNKNOWN; } mcuTypeId_e getMcuTypeId(void) { return MCU_TYPE_UNKNOWN; }
uint16_t getCurrentRxRefreshRate(void) { return 0; } uint16_t getCurrentRxRefreshRate(void) { return 0; }
uint16_t getAverageSystemLoadPercent(void) { return 0; }
} }

2
src/test/unit/rc_controls_unittest.cc
View file

@ -646,7 +646,7 @@ rxRuntimeState_t rxRuntimeState;
PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0); PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 2); PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 2);
void resetArmingDisabled(void) {} void resetArmingDisabled(void) {}
timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 20000; } timeDelta_t getTaskDeltaTimeUs(taskId_e) { return 20000; }
armingDisableFlags_e getArmingDisableFlags(void) { armingDisableFlags_e getArmingDisableFlags(void) {
return (armingDisableFlags_e) 0; return (armingDisableFlags_e) 0;
} }

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

@ -42,7 +42,7 @@ const int TEST_DISPATCH_TIME = 1;
#define TASK_PERIOD_HZ(hz) (1000000 / (hz)) #define TASK_PERIOD_HZ(hz) (1000000 / (hz))
extern "C" { extern "C" {
cfTask_t * unittest_scheduler_selectedTask; task_t * unittest_scheduler_selectedTask;
uint8_t unittest_scheduler_selectedTaskDynPrio; uint8_t unittest_scheduler_selectedTaskDynPrio;
uint16_t unittest_scheduler_waitingTasks; uint16_t unittest_scheduler_waitingTasks;
timeDelta_t unittest_scheduler_taskRequiredTimeUs; timeDelta_t unittest_scheduler_taskRequiredTimeUs;
@ -79,87 +79,92 @@ extern "C" {
} }
extern int taskQueueSize; extern int taskQueueSize;
extern cfTask_t* taskQueueArray[]; extern task_t* taskQueueArray[];
extern void queueClear(void); extern void queueClear(void);
extern bool queueContains(cfTask_t *task); extern bool queueContains(task_t *task);
extern bool queueAdd(cfTask_t *task); extern bool queueAdd(task_t *task);
extern bool queueRemove(cfTask_t *task); extern bool queueRemove(task_t *task);
extern cfTask_t *queueFirst(void); extern task_t *queueFirst(void);
extern cfTask_t *queueNext(void); extern task_t *queueNext(void);
cfTask_t cfTasks[TASK_COUNT] = { task_t tasks[TASK_COUNT] = {
[TASK_SYSTEM] = { [TASK_SYSTEM] = {
.taskName = "SYSTEM", .taskName = "SYSTEM",
.taskFunc = taskSystemLoad, .taskFunc = taskSystemLoad,
.desiredPeriod = TASK_PERIOD_HZ(10), .desiredPeriodUs = TASK_PERIOD_HZ(10),
.staticPriority = TASK_PRIORITY_MEDIUM_HIGH, .staticPriority = TASK_PRIORITY_MEDIUM_HIGH,
}, },
[TASK_GYRO] = { [TASK_GYRO] = {
.taskName = "GYRO", .taskName = "GYRO",
.taskFunc = taskGyroSample, .taskFunc = taskGyroSample,
.desiredPeriod = TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ), .desiredPeriodUs = TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ),
.staticPriority = TASK_PRIORITY_REALTIME, .staticPriority = TASK_PRIORITY_REALTIME,
}, },
[TASK_FILTER] = { [TASK_FILTER] = {
.taskName = "FILTER", .taskName = "FILTER",
.taskFunc = taskFiltering, .taskFunc = taskFiltering,
.desiredPeriod = TASK_PERIOD_HZ(4000), .desiredPeriodUs = TASK_PERIOD_HZ(4000),
.staticPriority = TASK_PRIORITY_REALTIME, .staticPriority = TASK_PRIORITY_REALTIME,
}, },
[TASK_PID] = { [TASK_PID] = {
.taskName = "PID", .taskName = "PID",
.taskFunc = taskMainPidLoop, .taskFunc = taskMainPidLoop,
.desiredPeriod = TASK_PERIOD_HZ(4000), .desiredPeriodUs = TASK_PERIOD_HZ(4000),
.staticPriority = TASK_PRIORITY_REALTIME, .staticPriority = TASK_PRIORITY_REALTIME,
}, },
[TASK_ACCEL] = { [TASK_ACCEL] = {
.taskName = "ACCEL", .taskName = "ACCEL",
.taskFunc = taskUpdateAccelerometer, .taskFunc = taskUpdateAccelerometer,
.desiredPeriod = TASK_PERIOD_HZ(1000), .desiredPeriodUs = TASK_PERIOD_HZ(1000),
.staticPriority = TASK_PRIORITY_MEDIUM, .staticPriority = TASK_PRIORITY_MEDIUM,
}, },
[TASK_ATTITUDE] = { [TASK_ATTITUDE] = {
.taskName = "ATTITUDE", .taskName = "ATTITUDE",
.taskFunc = imuUpdateAttitude, .taskFunc = imuUpdateAttitude,
.desiredPeriod = TASK_PERIOD_HZ(100), .desiredPeriodUs = TASK_PERIOD_HZ(100),
.staticPriority = TASK_PRIORITY_MEDIUM, .staticPriority = TASK_PRIORITY_MEDIUM,
}, },
[TASK_RX] = { [TASK_RX] = {
.taskName = "RX", .taskName = "RX",
.checkFunc = rxUpdateCheck, .checkFunc = rxUpdateCheck,
.taskFunc = taskUpdateRxMain, .taskFunc = taskUpdateRxMain,
.desiredPeriod = TASK_PERIOD_HZ(50), .desiredPeriodUs = TASK_PERIOD_HZ(50),
.staticPriority = TASK_PRIORITY_HIGH, .staticPriority = TASK_PRIORITY_HIGH,
}, },
[TASK_SERIAL] = { [TASK_SERIAL] = {
.taskName = "SERIAL", .taskName = "SERIAL",
.taskFunc = taskHandleSerial, .taskFunc = taskHandleSerial,
.desiredPeriod = TASK_PERIOD_HZ(100), .desiredPeriodUs = TASK_PERIOD_HZ(100),
.staticPriority = TASK_PRIORITY_LOW, .staticPriority = TASK_PRIORITY_LOW,
}, },
[TASK_DISPATCH] = { [TASK_DISPATCH] = {
.taskName = "DISPATCH", .taskName = "DISPATCH",
.taskFunc = dispatchProcess, .taskFunc = dispatchProcess,
.desiredPeriod = TASK_PERIOD_HZ(1000), .desiredPeriodUs = TASK_PERIOD_HZ(1000),
.staticPriority = TASK_PRIORITY_HIGH, .staticPriority = TASK_PRIORITY_HIGH,
}, },
[TASK_BATTERY_VOLTAGE] = { [TASK_BATTERY_VOLTAGE] = {
.taskName = "BATTERY_VOLTAGE", .taskName = "BATTERY_VOLTAGE",
.taskFunc = taskUpdateBatteryVoltage, .taskFunc = taskUpdateBatteryVoltage,
.desiredPeriod = TASK_PERIOD_HZ(50), .desiredPeriodUs = TASK_PERIOD_HZ(50),
.staticPriority = TASK_PRIORITY_MEDIUM, .staticPriority = TASK_PRIORITY_MEDIUM,
} }
}; };
task_t *getTask(unsigned taskId)
{
return &tasks[taskId];
}
} }
TEST(SchedulerUnittest, TestPriorites) TEST(SchedulerUnittest, TestPriorites)
{ {
EXPECT_EQ(TASK_PRIORITY_MEDIUM_HIGH, cfTasks[TASK_SYSTEM].staticPriority); EXPECT_EQ(TASK_PRIORITY_MEDIUM_HIGH, tasks[TASK_SYSTEM].staticPriority);
EXPECT_EQ(TASK_PRIORITY_REALTIME, cfTasks[TASK_GYRO].staticPriority); EXPECT_EQ(TASK_PRIORITY_REALTIME, tasks[TASK_GYRO].staticPriority);
EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_ACCEL].staticPriority); EXPECT_EQ(TASK_PRIORITY_MEDIUM, tasks[TASK_ACCEL].staticPriority);
EXPECT_EQ(TASK_PRIORITY_LOW, cfTasks[TASK_SERIAL].staticPriority); EXPECT_EQ(TASK_PRIORITY_LOW, tasks[TASK_SERIAL].staticPriority);
EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_BATTERY_VOLTAGE].staticPriority); EXPECT_EQ(TASK_PRIORITY_MEDIUM, tasks[TASK_BATTERY_VOLTAGE].staticPriority);
} }
TEST(SchedulerUnittest, TestQueueInit) TEST(SchedulerUnittest, TestQueueInit)
@ -173,47 +178,47 @@ TEST(SchedulerUnittest, TestQueueInit)
} }
} }
cfTask_t *deadBeefPtr = reinterpret_cast<cfTask_t*>(0xDEADBEEF); task_t *deadBeefPtr = reinterpret_cast<task_t*>(0xDEADBEEF);
TEST(SchedulerUnittest, TestQueue) TEST(SchedulerUnittest, TestQueue)
{ {
queueClear(); queueClear();
taskQueueArray[TASK_COUNT + 1] = deadBeefPtr; taskQueueArray[TASK_COUNT + 1] = deadBeefPtr;
queueAdd(&cfTasks[TASK_SYSTEM]); // TASK_PRIORITY_MEDIUM_HIGH queueAdd(&tasks[TASK_SYSTEM]); // TASK_PRIORITY_MEDIUM_HIGH
EXPECT_EQ(1, taskQueueSize); EXPECT_EQ(1, taskQueueSize);
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst()); EXPECT_EQ(&tasks[TASK_SYSTEM], queueFirst());
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
queueAdd(&cfTasks[TASK_SERIAL]); // TASK_PRIORITY_LOW queueAdd(&tasks[TASK_SERIAL]); // TASK_PRIORITY_LOW
EXPECT_EQ(2, taskQueueSize); EXPECT_EQ(2, taskQueueSize);
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst()); EXPECT_EQ(&tasks[TASK_SYSTEM], queueFirst());
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); EXPECT_EQ(&tasks[TASK_SERIAL], queueNext());
EXPECT_EQ(NULL, queueNext()); EXPECT_EQ(NULL, queueNext());
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
queueAdd(&cfTasks[TASK_BATTERY_VOLTAGE]); // TASK_PRIORITY_MEDIUM queueAdd(&tasks[TASK_BATTERY_VOLTAGE]); // TASK_PRIORITY_MEDIUM
EXPECT_EQ(3, taskQueueSize); EXPECT_EQ(3, taskQueueSize);
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst()); EXPECT_EQ(&tasks[TASK_SYSTEM], queueFirst());
EXPECT_EQ(&cfTasks[TASK_BATTERY_VOLTAGE], queueNext()); EXPECT_EQ(&tasks[TASK_BATTERY_VOLTAGE], queueNext());
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); EXPECT_EQ(&tasks[TASK_SERIAL], queueNext());
EXPECT_EQ(NULL, queueNext()); EXPECT_EQ(NULL, queueNext());
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
queueAdd(&cfTasks[TASK_RX]); // TASK_PRIORITY_HIGH queueAdd(&tasks[TASK_RX]); // TASK_PRIORITY_HIGH
EXPECT_EQ(4, taskQueueSize); EXPECT_EQ(4, taskQueueSize);
EXPECT_EQ(&cfTasks[TASK_RX], queueFirst()); EXPECT_EQ(&tasks[TASK_RX], queueFirst());
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); EXPECT_EQ(&tasks[TASK_SYSTEM], queueNext());
EXPECT_EQ(&cfTasks[TASK_BATTERY_VOLTAGE], queueNext()); EXPECT_EQ(&tasks[TASK_BATTERY_VOLTAGE], queueNext());
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); EXPECT_EQ(&tasks[TASK_SERIAL], queueNext());
EXPECT_EQ(NULL, queueNext()); EXPECT_EQ(NULL, queueNext());
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
queueRemove(&cfTasks[TASK_SYSTEM]); // TASK_PRIORITY_HIGH queueRemove(&tasks[TASK_SYSTEM]); // TASK_PRIORITY_HIGH
EXPECT_EQ(3, taskQueueSize); EXPECT_EQ(3, taskQueueSize);
EXPECT_EQ(&cfTasks[TASK_RX], queueFirst()); EXPECT_EQ(&tasks[TASK_RX], queueFirst());
EXPECT_EQ(&cfTasks[TASK_BATTERY_VOLTAGE], queueNext()); EXPECT_EQ(&tasks[TASK_BATTERY_VOLTAGE], queueNext());
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); EXPECT_EQ(&tasks[TASK_SERIAL], queueNext());
EXPECT_EQ(NULL, queueNext()); EXPECT_EQ(NULL, queueNext());
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
} }
@ -225,7 +230,7 @@ TEST(SchedulerUnittest, TestQueueAddAndRemove)
// fill up the queue // fill up the queue
for (int taskId = 0; taskId < TASK_COUNT; ++taskId) { for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
const bool added = queueAdd(&cfTasks[taskId]); const bool added = queueAdd(&tasks[taskId]);
EXPECT_TRUE(added); EXPECT_TRUE(added);
EXPECT_EQ(taskId + 1, taskQueueSize); EXPECT_EQ(taskId + 1, taskQueueSize);
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
@ -233,13 +238,13 @@ TEST(SchedulerUnittest, TestQueueAddAndRemove)
// double check end of queue // double check end of queue
EXPECT_EQ(TASK_COUNT, taskQueueSize); EXPECT_EQ(TASK_COUNT, taskQueueSize);
EXPECT_NE(static_cast<cfTask_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
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); // null pointer at end of queue is preserved EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); // null pointer at end of queue is preserved
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); // there hasn't been an out by one error EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); // there hasn't been an out by one error
// and empty it again // and empty it again
for (int taskId = 0; taskId < TASK_COUNT; ++taskId) { for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
const bool removed = queueRemove(&cfTasks[taskId]); const bool removed = queueRemove(&tasks[taskId]);
EXPECT_TRUE(removed); EXPECT_TRUE(removed);
EXPECT_EQ(TASK_COUNT - taskId - 1, taskQueueSize); EXPECT_EQ(TASK_COUNT - taskId - 1, taskQueueSize);
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - taskId]); EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - taskId]);
@ -262,16 +267,16 @@ 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 (cfTasks[taskId].taskFunc) { if (tasks[taskId].taskFunc) {
setTaskEnabled(static_cast<cfTaskId_e>(taskId), true); setTaskEnabled(static_cast<taskId_e>(taskId), true);
enqueuedTasks++; enqueuedTasks++;
EXPECT_EQ(enqueuedTasks, taskQueueSize); EXPECT_EQ(enqueuedTasks, taskQueueSize);
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
} }
} }
EXPECT_NE(static_cast<cfTask_t*>(0), taskQueueArray[enqueuedTasks - 1]); EXPECT_NE(static_cast<task_t*>(0), taskQueueArray[enqueuedTasks - 1]);
const cfTask_t *lastTaskPrev = taskQueueArray[enqueuedTasks - 1]; const task_t *lastTaskPrev = taskQueueArray[enqueuedTasks - 1];
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]); EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]);
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]); EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]);
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
@ -292,10 +297,10 @@ TEST(SchedulerUnittest, TestQueueArray)
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]); EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]);
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
cfTaskInfo_t taskInfo; taskInfo_t taskInfo;
getTaskInfo(static_cast<cfTaskId_e>(enqueuedTasks + 1), &taskInfo); getTaskInfo(static_cast<taskId_e>(enqueuedTasks + 1), &taskInfo);
EXPECT_FALSE(taskInfo.isEnabled); EXPECT_FALSE(taskInfo.isEnabled);
setTaskEnabled(static_cast<cfTaskId_e>(enqueuedTasks), true); setTaskEnabled(static_cast<taskId_e>(enqueuedTasks), true);
EXPECT_EQ(enqueuedTasks, taskQueueSize); EXPECT_EQ(enqueuedTasks, taskQueueSize);
EXPECT_EQ(lastTaskPrev, taskQueueArray[enqueuedTasks - 1]); EXPECT_EQ(lastTaskPrev, taskQueueArray[enqueuedTasks - 1]);
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]); // check no buffer overrun EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]); // check no buffer overrun
@ -327,7 +332,7 @@ TEST(SchedulerUnittest, TestSchedulerInit)
{ {
schedulerInit(); schedulerInit();
EXPECT_EQ(1, taskQueueSize); EXPECT_EQ(1, taskQueueSize);
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst()); EXPECT_EQ(&tasks[TASK_SYSTEM], queueFirst());
} }
TEST(SchedulerUnittest, TestScheduleEmptyQueue) TEST(SchedulerUnittest, TestScheduleEmptyQueue)
@ -344,27 +349,27 @@ TEST(SchedulerUnittest, TestSingleTask)
schedulerInit(); schedulerInit();
// disable all tasks except TASK_ACCEL // disable all tasks except TASK_ACCEL
for (int taskId = 0; taskId < TASK_COUNT; ++taskId) { for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false); setTaskEnabled(static_cast<taskId_e>(taskId), false);
} }
setTaskEnabled(TASK_ACCEL, true); setTaskEnabled(TASK_ACCEL, true);
cfTasks[TASK_ACCEL].lastExecutedAt = 1000; tasks[TASK_ACCEL].lastExecutedAtUs = 1000;
simulatedTime = 2050; simulatedTime = 2050;
// run the scheduler and check the task has executed // run the scheduler and check the task has executed
scheduler(); scheduler();
EXPECT_NE(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask); EXPECT_NE(unittest_scheduler_selectedTask, static_cast<task_t*>(0));
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask); EXPECT_EQ(unittest_scheduler_selectedTask, &tasks[TASK_ACCEL]);
EXPECT_EQ(1050, cfTasks[TASK_ACCEL].taskLatestDeltaTime); EXPECT_EQ(1050, tasks[TASK_ACCEL].taskLatestDeltaTimeUs);
EXPECT_EQ(2050, cfTasks[TASK_ACCEL].lastExecutedAt); EXPECT_EQ(2050, tasks[TASK_ACCEL].lastExecutedAtUs);
EXPECT_EQ(TEST_UPDATE_ACCEL_TIME, cfTasks[TASK_ACCEL].totalExecutionTime); EXPECT_EQ(TEST_UPDATE_ACCEL_TIME, tasks[TASK_ACCEL].totalExecutionTimeUs);
// task has run, so its dynamic priority should have been set to zero // task has run, so its dynamic priority should have been set to zero
EXPECT_EQ(0, cfTasks[TASK_GYRO].dynamicPriority); EXPECT_EQ(0, tasks[TASK_GYRO].dynamicPriority);
} }
TEST(SchedulerUnittest, TestTwoTasks) TEST(SchedulerUnittest, TestTwoTasks)
{ {
// disable all tasks except TASK_ACCEL and TASK_ATTITUDE // disable all tasks except TASK_ACCEL and TASK_ATTITUDE
for (int taskId = 0; taskId < TASK_COUNT; ++taskId) { for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false); setTaskEnabled(static_cast<taskId_e>(taskId), false);
} }
setTaskEnabled(TASK_ACCEL, true); setTaskEnabled(TASK_ACCEL, true);
setTaskEnabled(TASK_ATTITUDE, true); setTaskEnabled(TASK_ATTITUDE, true);
@ -372,49 +377,49 @@ TEST(SchedulerUnittest, TestTwoTasks)
// set it up so that TASK_ACCEL ran just before TASK_ATTITUDE // set it up so that TASK_ACCEL ran just before TASK_ATTITUDE
static const uint32_t startTime = 4000; static const uint32_t startTime = 4000;
simulatedTime = startTime; simulatedTime = startTime;
cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime; tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime;
cfTasks[TASK_ATTITUDE].lastExecutedAt = cfTasks[TASK_ACCEL].lastExecutedAt - TEST_UPDATE_ATTITUDE_TIME; tasks[TASK_ATTITUDE].lastExecutedAtUs = tasks[TASK_ACCEL].lastExecutedAtUs - TEST_UPDATE_ATTITUDE_TIME;
EXPECT_EQ(0, cfTasks[TASK_ATTITUDE].taskAgeCycles); EXPECT_EQ(0, tasks[TASK_ATTITUDE].taskAgeCycles);
// run the scheduler // run the scheduler
scheduler(); scheduler();
// no tasks should have run, since neither task's desired time has elapsed // no tasks should have run, since neither task's desired time has elapsed
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask); EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
// NOTE: // NOTE:
// TASK_ACCEL desiredPeriod is 1000 microseconds // TASK_ACCEL desiredPeriodUs is 1000 microseconds
// TASK_ATTITUDE desiredPeriod is 10000 microseconds // TASK_ATTITUDE desiredPeriodUs is 10000 microseconds
// 500 microseconds later // 500 microseconds later
simulatedTime += 500; simulatedTime += 500;
// no tasks should run, since neither task's desired time has elapsed // no tasks should run, since neither task's desired time has elapsed
scheduler(); scheduler();
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask); EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
EXPECT_EQ(0, unittest_scheduler_waitingTasks); EXPECT_EQ(0, unittest_scheduler_waitingTasks);
// 500 microseconds later, TASK_ACCEL desiredPeriod has elapsed // 500 microseconds later, TASK_ACCEL desiredPeriodUs has elapsed
simulatedTime += 500; simulatedTime += 500;
// TASK_ACCEL should now run // TASK_ACCEL should now run
scheduler(); scheduler();
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask); EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
EXPECT_EQ(1, unittest_scheduler_waitingTasks); EXPECT_EQ(1, unittest_scheduler_waitingTasks);
EXPECT_EQ(5000 + TEST_UPDATE_ACCEL_TIME, simulatedTime); EXPECT_EQ(5000 + TEST_UPDATE_ACCEL_TIME, simulatedTime);
simulatedTime += 1000 - TEST_UPDATE_ACCEL_TIME; simulatedTime += 1000 - TEST_UPDATE_ACCEL_TIME;
scheduler(); scheduler();
// TASK_ACCEL should run again // TASK_ACCEL should run again
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask); EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
scheduler(); scheduler();
// No task should have run // No task should have run
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask); EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
EXPECT_EQ(0, unittest_scheduler_waitingTasks); EXPECT_EQ(0, unittest_scheduler_waitingTasks);
simulatedTime = startTime + 10500; // TASK_ACCEL and TASK_ATTITUDE desiredPeriods have elapsed simulatedTime = startTime + 10500; // TASK_ACCEL and TASK_ATTITUDE desiredPeriodUss have elapsed
// of the two TASK_ACCEL should run first // of the two TASK_ACCEL should run first
scheduler(); scheduler();
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask); EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
// and finally TASK_ATTITUDE should now run // and finally TASK_ATTITUDE should now run
scheduler(); scheduler();
EXPECT_EQ(&cfTasks[TASK_ATTITUDE], unittest_scheduler_selectedTask); EXPECT_EQ(&tasks[TASK_ATTITUDE], unittest_scheduler_selectedTask);
} }
TEST(SchedulerUnittest, TestGyroTask) TEST(SchedulerUnittest, TestGyroTask)
@ -426,7 +431,7 @@ TEST(SchedulerUnittest, TestGyroTask)
// disable all tasks except TASK_GYRO, TASK_FILTER and TASK_PID // disable all tasks except TASK_GYRO, TASK_FILTER and TASK_PID
for (int taskId = 0; taskId < TASK_COUNT; ++taskId) { for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false); setTaskEnabled(static_cast<taskId_e>(taskId), false);
} }
setTaskEnabled(TASK_GYRO, true); setTaskEnabled(TASK_GYRO, true);
setTaskEnabled(TASK_FILTER, true); setTaskEnabled(TASK_FILTER, true);
@ -434,14 +439,14 @@ TEST(SchedulerUnittest, TestGyroTask)
// First set it up so TASK_GYRO just ran // First set it up so TASK_GYRO just ran
simulatedTime = startTime; simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime; tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime;
// reset the flags // reset the flags
resetGyroTaskTestFlags(); resetGyroTaskTestFlags();
// run the scheduler // run the scheduler
scheduler(); scheduler();
// no tasks should have run // no tasks should have run
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask); EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
// also the gyro, filter and PID task indicators should be false // also the gyro, filter and PID task indicators should be false
EXPECT_FALSE(taskGyroRan); EXPECT_FALSE(taskGyroRan);
EXPECT_FALSE(taskFilterRan); EXPECT_FALSE(taskFilterRan);
@ -450,7 +455,7 @@ TEST(SchedulerUnittest, TestGyroTask)
/* Test the gyro task running but not triggering the filtering or PID */ /* Test the gyro task running but not triggering the filtering or PID */
// set the TASK_GYRO last executed time to be one period earlier // set the TASK_GYRO last executed time to be one period earlier
simulatedTime = startTime; simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ); tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
// reset the flags // reset the flags
resetGyroTaskTestFlags(); resetGyroTaskTestFlags();
@ -462,12 +467,12 @@ TEST(SchedulerUnittest, TestGyroTask)
EXPECT_FALSE(taskFilterRan); EXPECT_FALSE(taskFilterRan);
EXPECT_FALSE(taskPidRan); EXPECT_FALSE(taskPidRan);
// expect that no other tasks other than TASK_GYRO should have run // expect that no other tasks other than TASK_GYRO should have run
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask); EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
/* Test the gyro task running and triggering the filtering task */ /* Test the gyro task running and triggering the filtering task */
// set the TASK_GYRO last executed time to be one period earlier // set the TASK_GYRO last executed time to be one period earlier
simulatedTime = startTime; simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ); tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
// reset the flags // reset the flags
resetGyroTaskTestFlags(); resetGyroTaskTestFlags();
@ -480,12 +485,12 @@ TEST(SchedulerUnittest, TestGyroTask)
EXPECT_TRUE(taskFilterRan); EXPECT_TRUE(taskFilterRan);
EXPECT_FALSE(taskPidRan); EXPECT_FALSE(taskPidRan);
// expect that no other tasks other tasks should have run // expect that no other tasks other tasks should have run
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask); EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
/* Test the gyro task running and triggering the PID task */ /* Test the gyro task running and triggering the PID task */
// set the TASK_GYRO last executed time to be one period earlier // set the TASK_GYRO last executed time to be one period earlier
simulatedTime = startTime; simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ); tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
// reset the flags // reset the flags
resetGyroTaskTestFlags(); resetGyroTaskTestFlags();
@ -498,7 +503,7 @@ TEST(SchedulerUnittest, TestGyroTask)
EXPECT_FALSE(taskFilterRan); EXPECT_FALSE(taskFilterRan);
EXPECT_TRUE(taskPidRan); EXPECT_TRUE(taskPidRan);
// expect that no other tasks other tasks should have run // expect that no other tasks other tasks should have run
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask); EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
} }
// Test the scheduling logic that prevents other tasks from running if they // Test the scheduling logic that prevents other tasks from running if they
@ -518,21 +523,21 @@ TEST(SchedulerUnittest, TestGyroLookahead)
// disable all tasks except TASK_GYRO, TASK_ACCEL // disable all tasks except TASK_GYRO, TASK_ACCEL
for (int taskId = 0; taskId < TASK_COUNT; ++taskId) { for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false); setTaskEnabled(static_cast<taskId_e>(taskId), false);
} }
setTaskEnabled(TASK_GYRO, true); setTaskEnabled(TASK_GYRO, true);
setTaskEnabled(TASK_ACCEL, true); setTaskEnabled(TASK_ACCEL, true);
#if defined(USE_TASK_STATISTICS) #if defined(USE_TASK_STATISTICS)
// set the average run time for TASK_ACCEL // set the average run time for TASK_ACCEL
cfTasks[TASK_ACCEL].movingSumExecutionTime = TEST_UPDATE_ACCEL_TIME * TASK_STATS_MOVING_SUM_COUNT; tasks[TASK_ACCEL].movingSumExecutionTimeUs = TEST_UPDATE_ACCEL_TIME * TASK_STATS_MOVING_SUM_COUNT;
#endif #endif
/* Test that another task will run if there's plenty of time till the next gyro sample time */ /* Test that another task will run if there's plenty of time till the next gyro sample time */
// set it up so TASK_GYRO just ran and TASK_ACCEL is ready to run // set it up so TASK_GYRO just ran and TASK_ACCEL is ready to run
simulatedTime = startTime; simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime; tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime;
cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(1000); tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(1000);
// reset the flags // reset the flags
resetGyroTaskTestFlags(); resetGyroTaskTestFlags();
@ -543,13 +548,13 @@ TEST(SchedulerUnittest, TestGyroLookahead)
EXPECT_FALSE(taskFilterRan); EXPECT_FALSE(taskFilterRan);
EXPECT_FALSE(taskPidRan); EXPECT_FALSE(taskPidRan);
// TASK_ACCEL should have run // TASK_ACCEL should have run
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask); EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
/* Test that another task won't run if the time till the gyro task is less than the guard interval */ /* Test that another task won't run if the time till the gyro task is less than the guard interval */
// set it up so TASK_GYRO will run soon and TASK_ACCEL is ready to run // set it up so TASK_GYRO will run soon and TASK_ACCEL is ready to run
simulatedTime = startTime; simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + GYRO_TASK_GUARD_INTERVAL_US / 2; tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + GYRO_TASK_GUARD_INTERVAL_US / 2;
cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(1000); tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(1000);
// reset the flags // reset the flags
resetGyroTaskTestFlags(); resetGyroTaskTestFlags();
@ -560,13 +565,13 @@ TEST(SchedulerUnittest, TestGyroLookahead)
EXPECT_FALSE(taskFilterRan); EXPECT_FALSE(taskFilterRan);
EXPECT_FALSE(taskPidRan); EXPECT_FALSE(taskPidRan);
// TASK_ACCEL should not have run // TASK_ACCEL should not have run
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask); EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
/* Test that another task won't run if the time till the gyro task is less than the average task interval */ /* Test that another task won't run if the time till the gyro task is less than the average task interval */
// set it up so TASK_GYRO will run soon and TASK_ACCEL is ready to run // set it up so TASK_GYRO will run soon and TASK_ACCEL is ready to run
simulatedTime = startTime; simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + TEST_UPDATE_ACCEL_TIME / 2; tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + TEST_UPDATE_ACCEL_TIME / 2;
cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(1000); tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(1000);
// reset the flags // reset the flags
resetGyroTaskTestFlags(); resetGyroTaskTestFlags();
@ -577,13 +582,13 @@ TEST(SchedulerUnittest, TestGyroLookahead)
EXPECT_FALSE(taskFilterRan); EXPECT_FALSE(taskFilterRan);
EXPECT_FALSE(taskPidRan); EXPECT_FALSE(taskPidRan);
// TASK_ACCEL should not have run // TASK_ACCEL should not have run
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask); EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
/* Test that another task will run if the gyro task gets executed */ /* Test that another task will run if the gyro task gets executed */
// set it up so TASK_GYRO will run now and TASK_ACCEL is ready to run // set it up so TASK_GYRO will run now and TASK_ACCEL is ready to run
simulatedTime = startTime; simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ); tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(1000); tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(1000);
// reset the flags // reset the flags
resetGyroTaskTestFlags(); resetGyroTaskTestFlags();
@ -598,5 +603,5 @@ TEST(SchedulerUnittest, TestGyroLookahead)
EXPECT_TRUE(taskFilterRan); EXPECT_TRUE(taskFilterRan);
EXPECT_TRUE(taskPidRan); EXPECT_TRUE(taskPidRan);
// TASK_ACCEL should have run // TASK_ACCEL should have run
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask); EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
} }

2
src/test/unit/sensor_gyro_unittest.cc
View file

@ -160,6 +160,6 @@ void beeper(beeperMode_e) {}
uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE }; uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE };
timeDelta_t getGyroUpdateRate(void) {return gyro.targetLooptime;} timeDelta_t getGyroUpdateRate(void) {return gyro.targetLooptime;}
void sensorsSet(uint32_t) {} void sensorsSet(uint32_t) {}
void schedulerResetTaskStatistics(cfTaskId_e) {} void schedulerResetTaskStatistics(taskId_e) {}
int getArmingDisableFlags(void) {return 0;} int getArmingDisableFlags(void) {return 0;}
} }

4
src/test/unit/telemetry_ibus_unittest.cc
View file

@ -154,10 +154,10 @@ static bool portIsShared = false;
static bool openSerial_called = false; static bool openSerial_called = false;
static bool telemetryDetermineEnabledState_stub_retval; static bool telemetryDetermineEnabledState_stub_retval;
void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros) void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs)
{ {
EXPECT_EQ(TASK_TELEMETRY, taskId); EXPECT_EQ(TASK_TELEMETRY, taskId);
EXPECT_EQ(1000, newPeriodMicros); EXPECT_EQ(1000, newPeriodUs);
} }

5
src/test/unit/vtx_unittest.cc
View file

@ -143,7 +143,7 @@ extern "C" {
bool calculateRxChannelsAndUpdateFailsafe(timeUs_t) { return true; } bool calculateRxChannelsAndUpdateFailsafe(timeUs_t) { return true; }
bool isMixerUsingServos(void) { return false; } bool isMixerUsingServos(void) { return false; }
void gyroUpdate() {} void gyroUpdate() {}
timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 0; } timeDelta_t getTaskDeltaTimeUs(taskId_e) { return 0; }
void updateRSSI(timeUs_t) {} void updateRSSI(timeUs_t) {}
bool failsafeIsMonitoring(void) { return false; } bool failsafeIsMonitoring(void) { return false; }
void failsafeStartMonitoring(void) {} void failsafeStartMonitoring(void) {}
@ -173,7 +173,7 @@ extern "C" {
void dashboardEnablePageCycling(void) {} void dashboardEnablePageCycling(void) {}
void dashboardDisablePageCycling(void) {} void dashboardDisablePageCycling(void) {}
bool imuQuaternionHeadfreeOffsetSet(void) { return true; } bool imuQuaternionHeadfreeOffsetSet(void) { return true; }
void rescheduleTask(cfTaskId_e, uint32_t) {} void rescheduleTask(taskId_e, timeDelta_t) {}
bool usbCableIsInserted(void) { return false; } bool usbCableIsInserted(void) { return false; }
bool usbVcpIsConnected(void) { return false; } bool usbVcpIsConnected(void) { return false; }
void pidSetAntiGravityState(bool newState) { UNUSED(newState); } void pidSetAntiGravityState(bool newState) { UNUSED(newState); }
@ -188,4 +188,5 @@ extern "C" {
void gyroFiltering(timeUs_t) {}; void gyroFiltering(timeUs_t) {};
timeDelta_t rxGetFrameDelta(timeDelta_t *) { return 0; } timeDelta_t rxGetFrameDelta(timeDelta_t *) { return 0; }
void updateRcRefreshRate(timeUs_t) {}; void updateRcRefreshRate(timeUs_t) {};
uint16_t getAverageSystemLoadPercent(void) { return 0; }
} }