rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #9590 from mikeller/clean_up_scheduler 

Cleaned up the scheduler.
This commit is contained in:
Michael Keller 2020-03-16 18:02:36 +13:00 committed by GitHub
parent a6d84a82a7 db4bd1f186
commit 01f0095296
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
20 changed files with 326 additions and 392 deletions
Show Stats 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
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();
if (rxRate != 0) {
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",
constrain(averageSystemLoadPercent, 0, 100), getTaskDeltaTime(TASK_GYRO), gyroRate, rxRate, systemRate);
constrain(getAverageSystemLoadPercent(), 0, LOAD_PERCENTAGE_ONE), getTaskDeltaTimeUs(TASK_GYRO), gyroRate, rxRate, systemRate);
// Battery meter
@ -4720,22 +4720,22 @@ static void cliTasks(char *cmdline)
cliPrintLine("Task list");
}
#endif
for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; taskId++) {
cfTaskInfo_t taskInfo;
for (taskId_e taskId = 0; taskId < TASK_COUNT; taskId++) {
taskInfo_t taskInfo;
getTaskInfo(taskId, &taskInfo);
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);
const int maxLoad = taskInfo.maxExecutionTime == 0 ? 0 :(taskInfo.maxExecutionTime * taskFrequency + 5000) / 1000;
const int averageLoad = taskInfo.averageExecutionTime == 0 ? 0 : (taskInfo.averageExecutionTime * taskFrequency + 5000) / 1000;
const int maxLoad = taskInfo.maxExecutionTimeUs == 0 ? 0 :(taskInfo.maxExecutionTimeUs * taskFrequency + 5000) / 1000;
const int averageLoad = taskInfo.averageExecutionTimeUs == 0 ? 0 : (taskInfo.averageExecutionTimeUs * taskFrequency + 5000) / 1000;
if (taskId != TASK_SERIAL) {
maxLoadSum += maxLoad;
averageLoadSum += averageLoad;
}
if (systemConfig()->task_statistics) {
cliPrintLinef("%6d %7d %7d %4d.%1d%% %4d.%1d%% %9d",
taskFrequency, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime,
maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10, taskInfo.totalExecutionTime / 1000);
taskFrequency, taskInfo.maxExecutionTimeUs, taskInfo.averageExecutionTimeUs,
maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10, taskInfo.totalExecutionTimeUs / 1000);
} else {
cliPrintLinef("%6d", taskFrequency);
}
@ -4746,7 +4746,7 @@ static void cliTasks(char *cmdline)
if (systemConfig()->task_statistics) {
cfCheckFuncInfo_t 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);
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 "build/debug.h"
#include "blackbox/blackbox.h"
#include "blackbox/blackbox_fielddefs.h"
#include "build/debug.h"
#include "cli/cli.h"
#include "cms/cms.h"
@ -39,6 +39,7 @@
#include "common/maths.h"
#include "common/utils.h"
#include "config/config.h"
#include "config/feature.h"
#include "drivers/dshot.h"
@ -50,21 +51,20 @@
#include "drivers/time.h"
#include "drivers/transponder_ir.h"
#include "config/config.h"
#include "fc/controlrate_profile.h"
#include "fc/core.h"
#include "fc/rc.h"
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "fc/stats.h"
#include "fc/tasks.h"
#include "flight/failsafe.h"
#include "flight/gps_rescue.h"
#if defined(USE_GYRO_DATA_ANALYSE)
#include "flight/gyroanalyse.h"
#endif
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
@ -105,6 +105,8 @@
#include "telemetry/telemetry.h"
#include "core.h"
enum {
ALIGN_GYRO = 0,
@ -321,7 +323,7 @@ void updateArmingStatus(void)
unsetArmingDisabled(ARMING_DISABLED_ANGLE);
}
if (averageSystemLoadPercent > 100) {
if (getAverageSystemLoadPercent() > LOAD_PERCENTAGE_ONE) {
setArmingDisabled(ARMING_DISABLED_LOAD);
} else {
unsetArmingDisabled(ARMING_DISABLED_LOAD);
@ -1284,8 +1286,8 @@ FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
subTaskPidSubprocesses(currentTimeUs);
if (debugMode == DEBUG_CYCLETIME) {
debug[0] = getTaskDeltaTime(TASK_SELF);
debug[1] = averageSystemLoadPercent;
DEBUG_SET(DEBUG_CYCLETIME, 0, getTaskDeltaTimeUs(TASK_SELF));
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 (rcDelayMs <= INT16_MAX - (getTaskDeltaTime(TASK_SELF) / 1000)) {
rcDelayMs += getTaskDeltaTime(TASK_SELF) / 1000;
if (rcDelayMs <= INT16_MAX - (getTaskDeltaTimeUs(TASK_SELF) / 1000)) {
rcDelayMs += getTaskDeltaTimeUs(TASK_SELF) / 1000;
}
} else {
rcDelayMs = 0;

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

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

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

@ -20,4 +20,7 @@
#pragma once
#include "scheduler/scheduler.h"
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_send_string(bus, "Task max avg mx% av%");
cfTaskInfo_t taskInfo;
for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; ++taskId) {
taskInfo_t taskInfo;
for (taskId_e taskId = 0; taskId < TASK_COUNT; ++taskId) {
getTaskInfo(taskId, &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 maxLoad = (taskInfo.maxExecutionTime * taskFrequency + 5000) / 10000;
const int averageLoad = (taskInfo.averageExecutionTime * taskFrequency + 5000) / 10000;
tfp_sprintf(lineBuffer, format, taskId, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, maxLoad, averageLoad);
const int taskFrequency = (int)(1000000.0f / ((float)taskInfo.latestDeltaTimeUs));
const int maxLoad = (taskInfo.maxExecutionTimeUs * taskFrequency + 5000) / 10000;
const int averageLoad = (taskInfo.averageExecutionTimeUs * taskFrequency + 5000) / 10000;
tfp_sprintf(lineBuffer, format, taskId, taskInfo.maxExecutionTimeUs, taskInfo.averageExecutionTimeUs, maxLoad, averageLoad);
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
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;
const int flagBits = packFlightModeFlags(&flightModeFlags);
sbufWriteU16(dst, getTaskDeltaTime(TASK_PID));
sbufWriteU16(dst, getTaskDeltaTimeUs(TASK_PID));
#ifdef USE_I2C
sbufWriteU16(dst, i2cGetErrorCounter());
#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);
sbufWriteData(dst, &flightModeFlags, 4); // unconditional part of flags, first 32 bits
sbufWriteU8(dst, getCurrentPidProfileIndex());
sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
sbufWriteU16(dst, constrain(getAverageSystemLoadPercent(), 0, LOAD_PERCENTAGE_ONE));
if (cmdMSP == MSP_STATUS_EX) {
sbufWriteU8(dst, PID_PROFILE_COUNT);
sbufWriteU8(dst, getCurrentControlRateProfileIndex());

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

@ -29,10 +29,6 @@
#include "build/build_config.h"
#include "build/debug.h"
#include "scheduler/scheduler.h"
#include "config/config_unittest.h"
#include "common/maths.h"
#include "common/time.h"
#include "common/utils.h"
@ -40,6 +36,9 @@
#include "drivers/time.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_PADDING_US 5 // Add a little padding to the average execution time
@ -50,7 +49,7 @@
// 2 - time spent in scheduler
// 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 totalWaitingTasksSamples;
@ -61,12 +60,12 @@ FAST_RAM_ZERO_INIT uint16_t averageSystemLoadPercent = 0;
static FAST_RAM_ZERO_INIT int taskQueuePos = 0;
STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT int taskQueueSize = 0;
static FAST_RAM int periodCalculationBasisOffset = offsetof(cfTask_t, lastExecutedAt);
static FAST_RAM int periodCalculationBasisOffset = offsetof(task_t, lastExecutedAtUs);
static FAST_RAM_ZERO_INIT bool gyroEnabled;
// 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)
{
@ -75,7 +74,7 @@ void queueClear(void)
taskQueueSize = 0;
}
bool queueContains(cfTask_t *task)
bool queueContains(task_t *task)
{
for (int ii = 0; ii < taskQueueSize; ++ii) {
if (taskQueueArray[ii] == task) {
@ -85,7 +84,7 @@ bool queueContains(cfTask_t *task)
return false;
}
bool queueAdd(cfTask_t *task)
bool queueAdd(task_t *task)
{
if ((taskQueueSize >= TASK_COUNT) || queueContains(task)) {
return false;
@ -101,7 +100,7 @@ bool queueAdd(cfTask_t *task)
return false;
}
bool queueRemove(cfTask_t *task)
bool queueRemove(task_t *task)
{
for (int ii = 0; ii < taskQueueSize; ++ii) {
if (taskQueueArray[ii] == task) {
@ -116,7 +115,7 @@ bool queueRemove(cfTask_t *task)
/*
* Returns first item queue or NULL if queue empty
*/
FAST_CODE cfTask_t *queueFirst(void)
FAST_CODE task_t *queueFirst(void)
{
taskQueuePos = 0;
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
*/
FAST_CODE cfTask_t *queueNext(void)
FAST_CODE task_t *queueNext(void)
{
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)
timeUs_t checkFuncMaxExecutionTime;
timeUs_t checkFuncTotalExecutionTime;
timeUs_t checkFuncMovingSumExecutionTime;
timeUs_t checkFuncMovingSumDeltaTime;
timeUs_t checkFuncMaxExecutionTimeUs;
timeUs_t checkFuncTotalExecutionTimeUs;
timeUs_t checkFuncMovingSumExecutionTimeUs;
timeUs_t checkFuncMovingSumDeltaTimeUs;
void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo)
{
checkFuncInfo->maxExecutionTime = checkFuncMaxExecutionTime;
checkFuncInfo->totalExecutionTime = checkFuncTotalExecutionTime;
checkFuncInfo->averageExecutionTime = checkFuncMovingSumExecutionTime / TASK_STATS_MOVING_SUM_COUNT;
checkFuncInfo->averageDeltaTime = checkFuncMovingSumDeltaTime / TASK_STATS_MOVING_SUM_COUNT;
checkFuncInfo->maxExecutionTimeUs = checkFuncMaxExecutionTimeUs;
checkFuncInfo->totalExecutionTimeUs = checkFuncTotalExecutionTimeUs;
checkFuncInfo->averageExecutionTimeUs = checkFuncMovingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
checkFuncInfo->averageDeltaTimeUs = checkFuncMovingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
}
#endif
void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
void getTaskInfo(taskId_e taskId, taskInfo_t * taskInfo)
{
taskInfo->isEnabled = queueContains(&cfTasks[taskId]);
taskInfo->desiredPeriod = cfTasks[taskId].desiredPeriod;
taskInfo->staticPriority = cfTasks[taskId].staticPriority;
taskInfo->isEnabled = queueContains(getTask(taskId));
taskInfo->desiredPeriodUs = getTask(taskId)->desiredPeriodUs;
taskInfo->staticPriority = getTask(taskId)->staticPriority;
#if defined(USE_TASK_STATISTICS)
taskInfo->taskName = cfTasks[taskId].taskName;
taskInfo->subTaskName = cfTasks[taskId].subTaskName;
taskInfo->maxExecutionTime = cfTasks[taskId].maxExecutionTime;
taskInfo->totalExecutionTime = cfTasks[taskId].totalExecutionTime;
taskInfo->averageExecutionTime = cfTasks[taskId].movingSumExecutionTime / TASK_STATS_MOVING_SUM_COUNT;
taskInfo->averageDeltaTime = cfTasks[taskId].movingSumDeltaTime / TASK_STATS_MOVING_SUM_COUNT;
taskInfo->latestDeltaTime = cfTasks[taskId].taskLatestDeltaTime;
taskInfo->movingAverageCycleTime = cfTasks[taskId].movingAverageCycleTime;
taskInfo->taskName = getTask(taskId)->taskName;
taskInfo->subTaskName = getTask(taskId)->subTaskName;
taskInfo->maxExecutionTimeUs = getTask(taskId)->maxExecutionTimeUs;
taskInfo->totalExecutionTimeUs = getTask(taskId)->totalExecutionTimeUs;
taskInfo->averageExecutionTimeUs = getTask(taskId)->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
taskInfo->averageDeltaTimeUs = getTask(taskId)->movingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
taskInfo->latestDeltaTimeUs = getTask(taskId)->taskLatestDeltaTimeUs;
taskInfo->movingAverageCycleTimeUs = getTask(taskId)->movingAverageCycleTimeUs;
#endif
}
void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros)
void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs)
{
if (taskId == TASK_SELF) {
cfTask_t *task = currentTask;
task->desiredPeriod = MAX(SCHEDULER_DELAY_LIMIT, (timeDelta_t)newPeriodMicros); // Limit delay to 100us (10 kHz) to prevent scheduler clogging
task_t *task = currentTask;
task->desiredPeriodUs = MAX(SCHEDULER_DELAY_LIMIT, newPeriodUs); // Limit delay to 100us (10 kHz) to prevent scheduler clogging
} else if (taskId < TASK_COUNT) {
cfTask_t *task = &cfTasks[taskId];
task->desiredPeriod = MAX(SCHEDULER_DELAY_LIMIT, (timeDelta_t)newPeriodMicros); // Limit delay to 100us (10 kHz) to prevent scheduler clogging
task_t *task = getTask(taskId);
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) {
cfTask_t *task = taskId == TASK_SELF ? currentTask : &cfTasks[taskId];
task_t *task = taskId == TASK_SELF ? currentTask : getTask(taskId);
if (enabled && task->taskFunc) {
queueAdd(task);
} 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) {
return currentTask->taskLatestDeltaTime;
return currentTask->taskLatestDeltaTimeUs;
} else if (taskId < TASK_COUNT) {
return cfTasks[taskId].taskLatestDeltaTime;
return getTask(taskId)->taskLatestDeltaTimeUs;
} else {
return 0;
}
@ -216,32 +215,32 @@ void schedulerSetCalulateTaskStatistics(bool calculateTaskStatisticsToUse)
calculateTaskStatistics = calculateTaskStatisticsToUse;
}
void schedulerResetTaskStatistics(cfTaskId_e taskId)
void schedulerResetTaskStatistics(taskId_e taskId)
{
#if defined(USE_TASK_STATISTICS)
if (taskId == TASK_SELF) {
currentTask->movingSumExecutionTime = 0;
currentTask->movingSumDeltaTime = 0;
currentTask->totalExecutionTime = 0;
currentTask->maxExecutionTime = 0;
currentTask->movingSumExecutionTimeUs = 0;
currentTask->movingSumDeltaTimeUs = 0;
currentTask->totalExecutionTimeUs = 0;
currentTask->maxExecutionTimeUs = 0;
} else if (taskId < TASK_COUNT) {
cfTasks[taskId].movingSumExecutionTime = 0;
cfTasks[taskId].movingSumDeltaTime = 0;
cfTasks[taskId].totalExecutionTime = 0;
cfTasks[taskId].maxExecutionTime = 0;
getTask(taskId)->movingSumExecutionTimeUs = 0;
getTask(taskId)->movingSumDeltaTimeUs = 0;
getTask(taskId)->totalExecutionTimeUs = 0;
getTask(taskId)->maxExecutionTimeUs = 0;
}
#else
UNUSED(taskId);
#endif
}
void schedulerResetTaskMaxExecutionTime(cfTaskId_e taskId)
void schedulerResetTaskMaxExecutionTime(taskId_e taskId)
{
#if defined(USE_TASK_STATISTICS)
if (taskId == TASK_SELF) {
currentTask->maxExecutionTime = 0;
currentTask->maxExecutionTimeUs = 0;
} else if (taskId < TASK_COUNT) {
cfTasks[taskId].maxExecutionTime = 0;
getTask(taskId)->maxExecutionTimeUs = 0;
}
#else
UNUSED(taskId);
@ -251,7 +250,7 @@ void schedulerResetTaskMaxExecutionTime(cfTaskId_e taskId)
#if defined(USE_TASK_STATISTICS)
void schedulerResetCheckFunctionMaxExecutionTime(void)
{
checkFuncMaxExecutionTime = 0;
checkFuncMaxExecutionTimeUs = 0;
}
#endif
@ -259,48 +258,48 @@ void schedulerInit(void)
{
calculateTaskStatistics = true;
queueClear();
queueAdd(&cfTasks[TASK_SYSTEM]);
queueAdd(getTask(TASK_SYSTEM));
}
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) {
return *(timeUs_t*)((uint8_t*)task + periodCalculationBasisOffset);
} 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) {
currentTask = selectedTask;
selectedTask->taskLatestDeltaTime = currentTimeUs - selectedTask->lastExecutedAt;
selectedTask->taskLatestDeltaTimeUs = cmpTimeUs(currentTimeUs, selectedTask->lastExecutedAtUs);
#if defined(USE_TASK_STATISTICS)
float period = currentTimeUs - selectedTask->lastExecutedAt;
float period = currentTimeUs - selectedTask->lastExecutedAtUs;
#endif
selectedTask->lastExecutedAt = currentTimeUs;
selectedTask->lastDesiredAt += (cmpTimeUs(currentTimeUs, selectedTask->lastDesiredAt) / selectedTask->desiredPeriod) * selectedTask->desiredPeriod;
selectedTask->lastExecutedAtUs = currentTimeUs;
selectedTask->lastDesiredAt += (cmpTimeUs(currentTimeUs, selectedTask->lastDesiredAt) / selectedTask->desiredPeriodUs) * selectedTask->desiredPeriodUs;
selectedTask->dynamicPriority = 0;
// Execute task
#if defined(USE_TASK_STATISTICS)
if (calculateTaskStatistics) {
const timeUs_t currentTimeBeforeTaskCall = micros();
selectedTask->taskFunc(currentTimeBeforeTaskCall);
taskExecutionTime = micros() - currentTimeBeforeTaskCall;
selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / TASK_STATS_MOVING_SUM_COUNT;
selectedTask->movingSumDeltaTime += selectedTask->taskLatestDeltaTime - selectedTask->movingSumDeltaTime / TASK_STATS_MOVING_SUM_COUNT;
selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task
selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
selectedTask->movingAverageCycleTime += 0.05f * (period - selectedTask->movingAverageCycleTime);
const timeUs_t currentTimeBeforeTaskCallUs = micros();
selectedTask->taskFunc(currentTimeBeforeTaskCallUs);
taskExecutionTimeUs = micros() - currentTimeBeforeTaskCallUs;
selectedTask->movingSumExecutionTimeUs += taskExecutionTimeUs - selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
selectedTask->movingSumDeltaTimeUs += selectedTask->taskLatestDeltaTimeUs - selectedTask->movingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
selectedTask->totalExecutionTimeUs += taskExecutionTimeUs; // time consumed by scheduler + task
selectedTask->maxExecutionTimeUs = MAX(selectedTask->maxExecutionTimeUs, taskExecutionTimeUs);
selectedTask->movingAverageCycleTimeUs += 0.05f * (period - selectedTask->movingAverageCycleTimeUs);
} else
#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)
{
// Cache currentTime
const timeUs_t schedulerStartTimeUs = micros();
timeUs_t currentTimeUs = schedulerStartTimeUs;
timeUs_t taskExecutionTime = 0;
cfTask_t *selectedTask = NULL;
timeUs_t taskExecutionTimeUs = 0;
task_t *selectedTask = NULL;
uint16_t selectedTaskDynamicPriority = 0;
uint16_t waitingTasks = 0;
bool realtimeTaskRan = false;
@ -325,16 +337,16 @@ FAST_CODE void scheduler(void)
if (gyroEnabled) {
// Realtime gyro/filtering/PID tasks get complete priority
cfTask_t *gyroTask = &cfTasks[TASK_GYRO];
const timeUs_t gyroExecuteTime = getPeriodCalculationBasis(gyroTask) + gyroTask->desiredPeriod;
gyroTaskDelayUs = cmpTimeUs(gyroExecuteTime, currentTimeUs); // time until the next expected gyro sample
if (cmpTimeUs(currentTimeUs, gyroExecuteTime) >= 0) {
taskExecutionTime = schedulerExecuteTask(gyroTask, currentTimeUs);
task_t *gyroTask = getTask(TASK_GYRO);
const timeUs_t gyroExecuteTimeUs = getPeriodCalculationBasis(gyroTask) + gyroTask->desiredPeriodUs;
gyroTaskDelayUs = cmpTimeUs(gyroExecuteTimeUs, currentTimeUs); // time until the next expected gyro sample
if (cmpTimeUs(currentTimeUs, gyroExecuteTimeUs) >= 0) {
taskExecutionTimeUs = schedulerExecuteTask(gyroTask, currentTimeUs);
if (gyroFilterReady()) {
taskExecutionTime += schedulerExecuteTask(&cfTasks[TASK_FILTER], currentTimeUs);
taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_FILTER), currentTimeUs);
}
if (pidLoopReady()) {
taskExecutionTime += schedulerExecuteTask(&cfTasks[TASK_PID], currentTimeUs);
taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_PID), currentTimeUs);
}
currentTimeUs = micros();
realtimeTaskRan = true;
@ -345,34 +357,34 @@ FAST_CODE void scheduler(void)
// The task to be invoked
// 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) {
// Task has checkFunc - event driven
if (task->checkFunc) {
#if defined(SCHEDULER_DEBUG)
const timeUs_t currentTimeBeforeCheckFuncCall = micros();
const timeUs_t currentTimeBeforeCheckFuncCallUs = micros();
#else
const timeUs_t currentTimeBeforeCheckFuncCall = currentTimeUs;
const timeUs_t currentTimeBeforeCheckFuncCallUs = currentTimeUs;
#endif
// Increase priority for event driven tasks
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;
waitingTasks++;
} else if (task->checkFunc(currentTimeBeforeCheckFuncCall, currentTimeBeforeCheckFuncCall - task->lastExecutedAt)) {
} else if (task->checkFunc(currentTimeBeforeCheckFuncCallUs, cmpTimeUs(currentTimeBeforeCheckFuncCallUs, task->lastExecutedAtUs))) {
#if defined(SCHEDULER_DEBUG)
DEBUG_SET(DEBUG_SCHEDULER, 3, micros() - currentTimeBeforeCheckFuncCall);
DEBUG_SET(DEBUG_SCHEDULER, 3, micros() - currentTimeBeforeCheckFuncCallUs);
#endif
#if defined(USE_TASK_STATISTICS)
if (calculateTaskStatistics) {
const uint32_t checkFuncExecutionTime = micros() - currentTimeBeforeCheckFuncCall;
checkFuncMovingSumExecutionTime += checkFuncExecutionTime - checkFuncMovingSumExecutionTime / TASK_STATS_MOVING_SUM_COUNT;
checkFuncMovingSumDeltaTime += task->taskLatestDeltaTime - checkFuncMovingSumDeltaTime / TASK_STATS_MOVING_SUM_COUNT;
checkFuncTotalExecutionTime += checkFuncExecutionTime; // time consumed by scheduler + task
checkFuncMaxExecutionTime = MAX(checkFuncMaxExecutionTime, checkFuncExecutionTime);
const uint32_t checkFuncExecutionTimeUs = micros() - currentTimeBeforeCheckFuncCallUs;
checkFuncMovingSumExecutionTimeUs += checkFuncExecutionTimeUs - checkFuncMovingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
checkFuncMovingSumDeltaTimeUs += task->taskLatestDeltaTimeUs - checkFuncMovingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
checkFuncTotalExecutionTimeUs += checkFuncExecutionTimeUs; // time consumed by scheduler + task
checkFuncMaxExecutionTimeUs = MAX(checkFuncMaxExecutionTimeUs, checkFuncExecutionTimeUs);
}
#endif
task->lastSignaledAt = currentTimeBeforeCheckFuncCall;
task->lastSignaledAtUs = currentTimeBeforeCheckFuncCallUs;
task->taskAgeCycles = 1;
task->dynamicPriority = 1 + task->staticPriority;
waitingTasks++;
@ -382,7 +394,7 @@ FAST_CODE void scheduler(void)
} else {
// Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods)
// Task age is calculated from last execution
task->taskAgeCycles = ((currentTimeUs - getPeriodCalculationBasis(task)) / task->desiredPeriod);
task->taskAgeCycles = ((currentTimeUs - getPeriodCalculationBasis(task)) / task->desiredPeriodUs);
if (task->taskAgeCycles > 0) {
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
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
#if defined(USE_TASK_STATISTICS)
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
// Add in the time spent so far in check functions and the scheduler logic
taskRequiredTimeUs += cmpTimeUs(micros(), currentTimeUs);
if (!gyroEnabled || realtimeTaskRan || (taskRequiredTimeUs < gyroTaskDelayUs)) {
taskExecutionTime += schedulerExecuteTask(selectedTask, currentTimeUs);
taskExecutionTimeUs += schedulerExecuteTask(selectedTask, currentTimeUs);
} else {
selectedTask = NULL;
}
@ -418,12 +430,14 @@ FAST_CODE void scheduler(void)
#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
UNUSED(taskExecutionTime);
UNUSED(taskExecutionTimeUs);
#endif
GET_SCHEDULER_LOCALS();
#if defined(UNIT_TEST)
readSchedulerLocals(selectedTask, selectedTaskDynamicPriority, waitingTasks);
#endif
}
void schedulerEnableGyro(void)
@ -431,3 +445,7 @@ void schedulerEnableGyro(void)
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
#endif
#define LOAD_PERCENTAGE_ONE 100
typedef enum {
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
@ -41,13 +43,13 @@ typedef enum {
TASK_PRIORITY_MEDIUM_HIGH = 4,
TASK_PRIORITY_HIGH = 5,
TASK_PRIORITY_MAX = 255
} cfTaskPriority_e;
} taskPriority_e;
typedef struct {
timeUs_t maxExecutionTime;
timeUs_t totalExecutionTime;
timeUs_t averageExecutionTime;
timeUs_t averageDeltaTime;
timeUs_t maxExecutionTimeUs;
timeUs_t totalExecutionTimeUs;
timeUs_t averageExecutionTimeUs;
timeUs_t averageDeltaTimeUs;
} cfCheckFuncInfo_t;
typedef struct {
@ -55,14 +57,14 @@ typedef struct {
const char * subTaskName;
bool isEnabled;
int8_t staticPriority;
timeDelta_t desiredPeriod;
timeDelta_t latestDeltaTime;
timeUs_t maxExecutionTime;
timeUs_t totalExecutionTime;
timeUs_t averageExecutionTime;
timeUs_t averageDeltaTime;
float movingAverageCycleTime;
} cfTaskInfo_t;
timeDelta_t desiredPeriodUs;
timeDelta_t latestDeltaTimeUs;
timeUs_t maxExecutionTimeUs;
timeUs_t totalExecutionTimeUs;
timeUs_t averageExecutionTimeUs;
timeUs_t averageDeltaTimeUs;
float movingAverageCycleTimeUs;
} taskInfo_t;
typedef enum {
/* Actual tasks */
@ -149,7 +151,7 @@ typedef enum {
/* Service task IDs */
TASK_NONE = TASK_COUNT,
TASK_SELF
} cfTaskId_e;
} taskId_e;
typedef struct {
// Configuration
@ -159,47 +161,41 @@ typedef struct {
#endif
bool (*checkFunc)(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
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
// Scheduling
uint16_t dynamicPriority; // measurement of how old task was last executed, used to avoid task starvation
uint16_t taskAgeCycles;
timeDelta_t taskLatestDeltaTime;
timeUs_t lastExecutedAt; // last time of invocation
timeUs_t lastSignaledAt; // time of invocation event for event-driven tasks
timeDelta_t taskLatestDeltaTimeUs;
timeUs_t lastExecutedAtUs; // last time of invocation
timeUs_t lastSignaledAtUs; // time of invocation event for event-driven tasks
timeUs_t lastDesiredAt; // time of last desired execution
#if defined(USE_TASK_STATISTICS)
// Statistics
float movingAverageCycleTime;
timeUs_t movingSumExecutionTime; // moving sum over 32 samples
timeUs_t movingSumDeltaTime; // moving sum over 32 samples
timeUs_t maxExecutionTime;
timeUs_t totalExecutionTime; // total time consumed by task since boot
float movingAverageCycleTimeUs;
timeUs_t movingSumExecutionTimeUs; // moving sum over 32 samples
timeUs_t movingSumDeltaTimeUs; // moving sum over 32 samples
timeUs_t maxExecutionTimeUs;
timeUs_t totalExecutionTimeUs; // total time consumed by task since boot
#endif
} cfTask_t;
extern cfTask_t cfTasks[TASK_COUNT];
extern uint16_t averageSystemLoadPercent;
} task_t;
void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo);
void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t *taskInfo);
void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros);
void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState);
timeDelta_t getTaskDeltaTime(cfTaskId_e taskId);
void getTaskInfo(taskId_e taskId, taskInfo_t *taskInfo);
void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs);
void setTaskEnabled(taskId_e taskId, bool newEnabledState);
timeDelta_t getTaskDeltaTimeUs(taskId_e taskId);
void schedulerSetCalulateTaskStatistics(bool calculateTaskStatistics);
void schedulerResetTaskStatistics(cfTaskId_e taskId);
void schedulerResetTaskMaxExecutionTime(cfTaskId_e taskId);
void schedulerResetTaskStatistics(taskId_e taskId);
void schedulerResetTaskMaxExecutionTime(taskId_e taskId);
void schedulerResetCheckFunctionMaxExecutionTime(void);
void schedulerInit(void);
void scheduler(void);
timeUs_t schedulerExecuteTask(cfTask_t *selectedTask, timeUs_t currentTimeUs);
void taskSystemLoad(timeUs_t currentTime);
timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTimeUs);
void taskSystemLoad(timeUs_t currentTimeUs);
void schedulerOptimizeRate(bool optimizeRate);
void schedulerEnableGyro(void);
#define LOAD_PERCENTAGE_ONE 100
#define isSystemOverloaded() (averageSystemLoadPercent >= LOAD_PERCENTAGE_ONE)
uint16_t getAverageSystemLoadPercent(void);

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

@ -31,29 +31,29 @@
#include "build/debug.h"
#include "common/maths.h"
#include "common/utils.h"
#include "common/time.h"
#include "common/utils.h"
#include "config/config.h"
#include "config/feature.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "drivers/io.h"
#include "drivers/time.h"
#include "drivers/rangefinder/rangefinder.h"
#include "drivers/rangefinder/rangefinder_hcsr04.h"
#include "drivers/rangefinder/rangefinder_lidartf.h"
#include "drivers/time.h"
#include "config/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/rangefinder.h"
#include "sensors/battery.h"
#include "scheduler/scheduler.h"
//#include "uav_interconnect/uav_interconnect.h"
// 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;
case BST_STATUS:
bstWrite16(getTaskDeltaTime(TASK_PID));
bstWrite16(getTaskDeltaTimeUs(TASK_PID));
#ifdef USE_I2C
bstWrite16(i2cGetErrorCounter());
#else
@ -314,7 +314,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
bstWrite8(getCurrentPidProfileIndex());
break;
case BST_LOOP_TIME:
bstWrite16(getTaskDeltaTime(TASK_PID));
bstWrite16(getTaskDeltaTimeUs(TASK_PID));
break;
case BST_RC_TUNING:
bstWrite8(currentControlRateProfile->rcRates[FD_ROLL]);

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

@ -470,9 +470,9 @@ static void hottPrepareMessages(void) {
static void hottTextmodeStart()
{
// Increase menu speed
cfTaskInfo_t taskInfo;
taskInfo_t taskInfo;
getTaskInfo(TASK_TELEMETRY, &taskInfo);
telemetryTaskPeriod = taskInfo.desiredPeriod;
telemetryTaskPeriod = taskInfo.desiredPeriodUs;
rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(HOTT_TEXTMODE_TASK_PERIOD));
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 isMixerUsingServos(void) { return false; }
void gyroUpdate(void) {}
timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 0; }
timeDelta_t getTaskDeltaTimeUs(taskId_e) { return 0; }
void updateRSSI(timeUs_t) {}
bool failsafeIsMonitoring(void) { return false; }
void failsafeStartMonitoring(void) {}
@ -1082,7 +1082,7 @@ extern "C" {
void dashboardEnablePageCycling(void) {}
void dashboardDisablePageCycling(void) {}
bool imuQuaternionHeadfreeOffsetSet(void) { return true; }
void rescheduleTask(cfTaskId_e, uint32_t) {}
void rescheduleTask(taskId_e, timeDelta_t) {}
bool usbCableIsInserted(void) { return false; }
bool usbVcpIsConnected(void) { return false; }
void pidSetAntiGravityState(bool) {}
@ -1103,4 +1103,5 @@ extern "C" {
void gyroFiltering(timeUs_t) {};
timeDelta_t rxGetFrameDelta(timeDelta_t *) { return 0; }
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;
uint16_t averageSystemLoadPercent = 0;
timeDelta_t getTaskDeltaTime(cfTaskId_e){ return 0; }
timeDelta_t getTaskDeltaTimeUs(taskId_e){ return 0; }
uint16_t currentRxRefreshRate = 9000;
armingDisableFlags_e getArmingDisableFlags(void) { return ARMING_DISABLED_NO_GYRO; }
@ -307,9 +307,9 @@ const char *armingDisableFlagNames[]= {
"DUMMYDISABLEFLAGNAME"
};
void getTaskInfo(cfTaskId_e, cfTaskInfo_t *) {}
void getTaskInfo(taskId_e, taskInfo_t *) {}
void getCheckFuncInfo(cfCheckFuncInfo_t *) {}
void schedulerResetTaskMaxExecutionTime(cfTaskId_e) {}
void schedulerResetTaskMaxExecutionTime(taskId_e) {}
void schedulerResetCheckFunctionMaxExecutionTime(void) {}
const char * const targetName = "UNITTEST";
@ -360,4 +360,5 @@ void delay(uint32_t) {}
displayPort_t *osdGetDisplayPort(osdDisplayPortDevice_e *) { return NULL; }
mcuTypeId_e getMcuTypeId(void) { return MCU_TYPE_UNKNOWN; }
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(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 2);
void resetArmingDisabled(void) {}
timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 20000; }
timeDelta_t getTaskDeltaTimeUs(taskId_e) { return 20000; }
armingDisableFlags_e getArmingDisableFlags(void) {
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))
extern "C" {
cfTask_t * unittest_scheduler_selectedTask;
task_t * unittest_scheduler_selectedTask;
uint8_t unittest_scheduler_selectedTaskDynPrio;
uint16_t unittest_scheduler_waitingTasks;
timeDelta_t unittest_scheduler_taskRequiredTimeUs;
@ -79,87 +79,92 @@ extern "C" {
}
extern int taskQueueSize;
extern cfTask_t* taskQueueArray[];
extern task_t* taskQueueArray[];
extern void queueClear(void);
extern bool queueContains(cfTask_t *task);
extern bool queueAdd(cfTask_t *task);
extern bool queueRemove(cfTask_t *task);
extern cfTask_t *queueFirst(void);
extern cfTask_t *queueNext(void);
extern bool queueContains(task_t *task);
extern bool queueAdd(task_t *task);
extern bool queueRemove(task_t *task);
extern task_t *queueFirst(void);
extern task_t *queueNext(void);
cfTask_t cfTasks[TASK_COUNT] = {
task_t tasks[TASK_COUNT] = {
[TASK_SYSTEM] = {
.taskName = "SYSTEM",
.taskFunc = taskSystemLoad,
.desiredPeriod = TASK_PERIOD_HZ(10),
.desiredPeriodUs = TASK_PERIOD_HZ(10),
.staticPriority = TASK_PRIORITY_MEDIUM_HIGH,
},
[TASK_GYRO] = {
.taskName = "GYRO",
.taskFunc = taskGyroSample,
.desiredPeriod = TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ),
.desiredPeriodUs = TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ),
.staticPriority = TASK_PRIORITY_REALTIME,
},
[TASK_FILTER] = {
.taskName = "FILTER",
.taskFunc = taskFiltering,
.desiredPeriod = TASK_PERIOD_HZ(4000),
.desiredPeriodUs = TASK_PERIOD_HZ(4000),
.staticPriority = TASK_PRIORITY_REALTIME,
},
[TASK_PID] = {
.taskName = "PID",
.taskFunc = taskMainPidLoop,
.desiredPeriod = TASK_PERIOD_HZ(4000),
.desiredPeriodUs = TASK_PERIOD_HZ(4000),
.staticPriority = TASK_PRIORITY_REALTIME,
},
[TASK_ACCEL] = {
.taskName = "ACCEL",
.taskFunc = taskUpdateAccelerometer,
.desiredPeriod = TASK_PERIOD_HZ(1000),
.desiredPeriodUs = TASK_PERIOD_HZ(1000),
.staticPriority = TASK_PRIORITY_MEDIUM,
},
[TASK_ATTITUDE] = {
.taskName = "ATTITUDE",
.taskFunc = imuUpdateAttitude,
.desiredPeriod = TASK_PERIOD_HZ(100),
.desiredPeriodUs = TASK_PERIOD_HZ(100),
.staticPriority = TASK_PRIORITY_MEDIUM,
},
[TASK_RX] = {
.taskName = "RX",
.checkFunc = rxUpdateCheck,
.taskFunc = taskUpdateRxMain,
.desiredPeriod = TASK_PERIOD_HZ(50),
.desiredPeriodUs = TASK_PERIOD_HZ(50),
.staticPriority = TASK_PRIORITY_HIGH,
},
[TASK_SERIAL] = {
.taskName = "SERIAL",
.taskFunc = taskHandleSerial,
.desiredPeriod = TASK_PERIOD_HZ(100),
.desiredPeriodUs = TASK_PERIOD_HZ(100),
.staticPriority = TASK_PRIORITY_LOW,
},
[TASK_DISPATCH] = {
.taskName = "DISPATCH",
.taskFunc = dispatchProcess,
.desiredPeriod = TASK_PERIOD_HZ(1000),
.desiredPeriodUs = TASK_PERIOD_HZ(1000),
.staticPriority = TASK_PRIORITY_HIGH,
},
[TASK_BATTERY_VOLTAGE] = {
.taskName = "BATTERY_VOLTAGE",
.taskFunc = taskUpdateBatteryVoltage,
.desiredPeriod = TASK_PERIOD_HZ(50),
.desiredPeriodUs = TASK_PERIOD_HZ(50),
.staticPriority = TASK_PRIORITY_MEDIUM,
}
};
task_t *getTask(unsigned taskId)
{
return &tasks[taskId];
}
}
TEST(SchedulerUnittest, TestPriorites)
{
EXPECT_EQ(TASK_PRIORITY_MEDIUM_HIGH, cfTasks[TASK_SYSTEM].staticPriority);
EXPECT_EQ(TASK_PRIORITY_REALTIME, cfTasks[TASK_GYRO].staticPriority);
EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_ACCEL].staticPriority);
EXPECT_EQ(TASK_PRIORITY_LOW, cfTasks[TASK_SERIAL].staticPriority);
EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_BATTERY_VOLTAGE].staticPriority);
EXPECT_EQ(TASK_PRIORITY_MEDIUM_HIGH, tasks[TASK_SYSTEM].staticPriority);
EXPECT_EQ(TASK_PRIORITY_REALTIME, tasks[TASK_GYRO].staticPriority);
EXPECT_EQ(TASK_PRIORITY_MEDIUM, tasks[TASK_ACCEL].staticPriority);
EXPECT_EQ(TASK_PRIORITY_LOW, tasks[TASK_SERIAL].staticPriority);
EXPECT_EQ(TASK_PRIORITY_MEDIUM, tasks[TASK_BATTERY_VOLTAGE].staticPriority);
}
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)
{
queueClear();
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(&cfTasks[TASK_SYSTEM], queueFirst());
EXPECT_EQ(&tasks[TASK_SYSTEM], queueFirst());
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(&cfTasks[TASK_SYSTEM], queueFirst());
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
EXPECT_EQ(&tasks[TASK_SYSTEM], queueFirst());
EXPECT_EQ(&tasks[TASK_SERIAL], queueNext());
EXPECT_EQ(NULL, queueNext());
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(&cfTasks[TASK_SYSTEM], queueFirst());
EXPECT_EQ(&cfTasks[TASK_BATTERY_VOLTAGE], queueNext());
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
EXPECT_EQ(&tasks[TASK_SYSTEM], queueFirst());
EXPECT_EQ(&tasks[TASK_BATTERY_VOLTAGE], queueNext());
EXPECT_EQ(&tasks[TASK_SERIAL], queueNext());
EXPECT_EQ(NULL, queueNext());
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(&cfTasks[TASK_RX], queueFirst());
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
EXPECT_EQ(&cfTasks[TASK_BATTERY_VOLTAGE], queueNext());
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
EXPECT_EQ(&tasks[TASK_RX], queueFirst());
EXPECT_EQ(&tasks[TASK_SYSTEM], queueNext());
EXPECT_EQ(&tasks[TASK_BATTERY_VOLTAGE], queueNext());
EXPECT_EQ(&tasks[TASK_SERIAL], queueNext());
EXPECT_EQ(NULL, queueNext());
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(&cfTasks[TASK_RX], queueFirst());
EXPECT_EQ(&cfTasks[TASK_BATTERY_VOLTAGE], queueNext());
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
EXPECT_EQ(&tasks[TASK_RX], queueFirst());
EXPECT_EQ(&tasks[TASK_BATTERY_VOLTAGE], queueNext());
EXPECT_EQ(&tasks[TASK_SERIAL], queueNext());
EXPECT_EQ(NULL, queueNext());
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
}
@ -225,7 +230,7 @@ TEST(SchedulerUnittest, TestQueueAddAndRemove)
// fill up the queue
for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
const bool added = queueAdd(&cfTasks[taskId]);
const bool added = queueAdd(&tasks[taskId]);
EXPECT_TRUE(added);
EXPECT_EQ(taskId + 1, taskQueueSize);
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
@ -233,13 +238,13 @@ TEST(SchedulerUnittest, TestQueueAddAndRemove)
// double check end of queue
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(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); // there hasn't been an out by one error
// and empty it again
for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
const bool removed = queueRemove(&cfTasks[taskId]);
const bool removed = queueRemove(&tasks[taskId]);
EXPECT_TRUE(removed);
EXPECT_EQ(TASK_COUNT - taskId - 1, taskQueueSize);
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - taskId]);
@ -262,16 +267,16 @@ TEST(SchedulerUnittest, TestQueueArray)
EXPECT_EQ(enqueuedTasks, taskQueueSize);
for (int taskId = 0; taskId < TASK_COUNT_UNITTEST - 1; ++taskId) {
if (cfTasks[taskId].taskFunc) {
setTaskEnabled(static_cast<cfTaskId_e>(taskId), true);
if (tasks[taskId].taskFunc) {
setTaskEnabled(static_cast<taskId_e>(taskId), true);
enqueuedTasks++;
EXPECT_EQ(enqueuedTasks, taskQueueSize);
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
}
}
EXPECT_NE(static_cast<cfTask_t*>(0), taskQueueArray[enqueuedTasks - 1]);
const cfTask_t *lastTaskPrev = taskQueueArray[enqueuedTasks - 1];
EXPECT_NE(static_cast<task_t*>(0), taskQueueArray[enqueuedTasks - 1]);
const task_t *lastTaskPrev = taskQueueArray[enqueuedTasks - 1];
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]);
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]);
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
@ -292,10 +297,10 @@ TEST(SchedulerUnittest, TestQueueArray)
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]);
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
cfTaskInfo_t taskInfo;
getTaskInfo(static_cast<cfTaskId_e>(enqueuedTasks + 1), &taskInfo);
taskInfo_t taskInfo;
getTaskInfo(static_cast<taskId_e>(enqueuedTasks + 1), &taskInfo);
EXPECT_FALSE(taskInfo.isEnabled);
setTaskEnabled(static_cast<cfTaskId_e>(enqueuedTasks), true);
setTaskEnabled(static_cast<taskId_e>(enqueuedTasks), true);
EXPECT_EQ(enqueuedTasks, taskQueueSize);
EXPECT_EQ(lastTaskPrev, taskQueueArray[enqueuedTasks - 1]);
EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]); // check no buffer overrun
@ -327,7 +332,7 @@ TEST(SchedulerUnittest, TestSchedulerInit)
{
schedulerInit();
EXPECT_EQ(1, taskQueueSize);
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst());
EXPECT_EQ(&tasks[TASK_SYSTEM], queueFirst());
}
TEST(SchedulerUnittest, TestScheduleEmptyQueue)
@ -344,27 +349,27 @@ TEST(SchedulerUnittest, TestSingleTask)
schedulerInit();
// disable all tasks except TASK_ACCEL
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);
cfTasks[TASK_ACCEL].lastExecutedAt = 1000;
tasks[TASK_ACCEL].lastExecutedAtUs = 1000;
simulatedTime = 2050;
// run the scheduler and check the task has executed
scheduler();
EXPECT_NE(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
EXPECT_EQ(1050, cfTasks[TASK_ACCEL].taskLatestDeltaTime);
EXPECT_EQ(2050, cfTasks[TASK_ACCEL].lastExecutedAt);
EXPECT_EQ(TEST_UPDATE_ACCEL_TIME, cfTasks[TASK_ACCEL].totalExecutionTime);
EXPECT_NE(unittest_scheduler_selectedTask, static_cast<task_t*>(0));
EXPECT_EQ(unittest_scheduler_selectedTask, &tasks[TASK_ACCEL]);
EXPECT_EQ(1050, tasks[TASK_ACCEL].taskLatestDeltaTimeUs);
EXPECT_EQ(2050, tasks[TASK_ACCEL].lastExecutedAtUs);
EXPECT_EQ(TEST_UPDATE_ACCEL_TIME, tasks[TASK_ACCEL].totalExecutionTimeUs);
// 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)
{
// disable all tasks except TASK_ACCEL and TASK_ATTITUDE
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_ATTITUDE, true);
@ -372,49 +377,49 @@ TEST(SchedulerUnittest, TestTwoTasks)
// set it up so that TASK_ACCEL ran just before TASK_ATTITUDE
static const uint32_t startTime = 4000;
simulatedTime = startTime;
cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime;
cfTasks[TASK_ATTITUDE].lastExecutedAt = cfTasks[TASK_ACCEL].lastExecutedAt - TEST_UPDATE_ATTITUDE_TIME;
EXPECT_EQ(0, cfTasks[TASK_ATTITUDE].taskAgeCycles);
tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime;
tasks[TASK_ATTITUDE].lastExecutedAtUs = tasks[TASK_ACCEL].lastExecutedAtUs - TEST_UPDATE_ATTITUDE_TIME;
EXPECT_EQ(0, tasks[TASK_ATTITUDE].taskAgeCycles);
// run the scheduler
scheduler();
// 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:
// TASK_ACCEL desiredPeriod is 1000 microseconds
// TASK_ATTITUDE desiredPeriod is 10000 microseconds
// TASK_ACCEL desiredPeriodUs is 1000 microseconds
// TASK_ATTITUDE desiredPeriodUs is 10000 microseconds
// 500 microseconds later
simulatedTime += 500;
// no tasks should run, since neither task's desired time has elapsed
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);
// 500 microseconds later, TASK_ACCEL desiredPeriod has elapsed
// 500 microseconds later, TASK_ACCEL desiredPeriodUs has elapsed
simulatedTime += 500;
// TASK_ACCEL should now run
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(5000 + TEST_UPDATE_ACCEL_TIME, simulatedTime);
simulatedTime += 1000 - TEST_UPDATE_ACCEL_TIME;
scheduler();
// TASK_ACCEL should run again
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
scheduler();
// 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);
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
scheduler();
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
// and finally TASK_ATTITUDE should now run
scheduler();
EXPECT_EQ(&cfTasks[TASK_ATTITUDE], unittest_scheduler_selectedTask);
EXPECT_EQ(&tasks[TASK_ATTITUDE], unittest_scheduler_selectedTask);
}
TEST(SchedulerUnittest, TestGyroTask)
@ -426,7 +431,7 @@ TEST(SchedulerUnittest, TestGyroTask)
// disable all tasks except TASK_GYRO, TASK_FILTER and TASK_PID
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_FILTER, true);
@ -434,14 +439,14 @@ TEST(SchedulerUnittest, TestGyroTask)
// First set it up so TASK_GYRO just ran
simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime;
tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime;
// reset the flags
resetGyroTaskTestFlags();
// run the scheduler
scheduler();
// 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
EXPECT_FALSE(taskGyroRan);
EXPECT_FALSE(taskFilterRan);
@ -450,7 +455,7 @@ TEST(SchedulerUnittest, TestGyroTask)
/* Test the gyro task running but not triggering the filtering or PID */
// set the TASK_GYRO last executed time to be one period earlier
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
resetGyroTaskTestFlags();
@ -462,12 +467,12 @@ TEST(SchedulerUnittest, TestGyroTask)
EXPECT_FALSE(taskFilterRan);
EXPECT_FALSE(taskPidRan);
// 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 */
// set the TASK_GYRO last executed time to be one period earlier
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
resetGyroTaskTestFlags();
@ -480,12 +485,12 @@ TEST(SchedulerUnittest, TestGyroTask)
EXPECT_TRUE(taskFilterRan);
EXPECT_FALSE(taskPidRan);
// 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 */
// set the TASK_GYRO last executed time to be one period earlier
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
resetGyroTaskTestFlags();
@ -498,7 +503,7 @@ TEST(SchedulerUnittest, TestGyroTask)
EXPECT_FALSE(taskFilterRan);
EXPECT_TRUE(taskPidRan);
// 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
@ -518,21 +523,21 @@ TEST(SchedulerUnittest, TestGyroLookahead)
// disable all tasks except TASK_GYRO, TASK_ACCEL
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_ACCEL, true);
#if defined(USE_TASK_STATISTICS)
// 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
/* 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
simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime;
cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(1000);
tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime;
tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(1000);
// reset the flags
resetGyroTaskTestFlags();
@ -543,13 +548,13 @@ TEST(SchedulerUnittest, TestGyroLookahead)
EXPECT_FALSE(taskFilterRan);
EXPECT_FALSE(taskPidRan);
// 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 */
// set it up so TASK_GYRO will run soon and TASK_ACCEL is ready to run
simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = 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_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + GYRO_TASK_GUARD_INTERVAL_US / 2;
tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(1000);
// reset the flags
resetGyroTaskTestFlags();
@ -560,13 +565,13 @@ TEST(SchedulerUnittest, TestGyroLookahead)
EXPECT_FALSE(taskFilterRan);
EXPECT_FALSE(taskPidRan);
// 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 */
// set it up so TASK_GYRO will run soon and TASK_ACCEL is ready to run
simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + TEST_UPDATE_ACCEL_TIME / 2;
cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(1000);
tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + TEST_UPDATE_ACCEL_TIME / 2;
tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(1000);
// reset the flags
resetGyroTaskTestFlags();
@ -577,13 +582,13 @@ TEST(SchedulerUnittest, TestGyroLookahead)
EXPECT_FALSE(taskFilterRan);
EXPECT_FALSE(taskPidRan);
// 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 */
// set it up so TASK_GYRO will run now and TASK_ACCEL is ready to run
simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(1000);
tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(1000);
// reset the flags
resetGyroTaskTestFlags();
@ -598,5 +603,5 @@ TEST(SchedulerUnittest, TestGyroLookahead)
EXPECT_TRUE(taskFilterRan);
EXPECT_TRUE(taskPidRan);
// 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 };
timeDelta_t getGyroUpdateRate(void) {return gyro.targetLooptime;}
void sensorsSet(uint32_t) {}
void schedulerResetTaskStatistics(cfTaskId_e) {}
void schedulerResetTaskStatistics(taskId_e) {}
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 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(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 isMixerUsingServos(void) { return false; }
void gyroUpdate() {}
timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 0; }
timeDelta_t getTaskDeltaTimeUs(taskId_e) { return 0; }
void updateRSSI(timeUs_t) {}
bool failsafeIsMonitoring(void) { return false; }
void failsafeStartMonitoring(void) {}
@ -173,7 +173,7 @@ extern "C" {
void dashboardEnablePageCycling(void) {}
void dashboardDisablePageCycling(void) {}
bool imuQuaternionHeadfreeOffsetSet(void) { return true; }
void rescheduleTask(cfTaskId_e, uint32_t) {}
void rescheduleTask(taskId_e, timeDelta_t) {}
bool usbCableIsInserted(void) { return false; }
bool usbVcpIsConnected(void) { return false; }
void pidSetAntiGravityState(bool newState) { UNUSED(newState); }
@ -188,4 +188,5 @@ extern "C" {
void gyroFiltering(timeUs_t) {};
timeDelta_t rxGetFrameDelta(timeDelta_t *) { return 0; }
void updateRcRefreshRate(timeUs_t) {};
uint16_t getAverageSystemLoadPercent(void) { return 0; }
}