mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 16:25:26 +03:00
Merge branch 'scheduler-queue'
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity)
commit
ddc75033ef
13 changed files with 872 additions and 296 deletions
4
Makefile
4
Makefile
|
|
@ -223,8 +223,8 @@ COMMON_SRC = build_config.c \
|
||||||
common/typeconversion.c \
|
common/typeconversion.c \
|
||||||
common/encoding.c \
|
common/encoding.c \
|
||||||
common/filter.c \
|
common/filter.c \
|
||||||
scheduler.c \
|
scheduler/scheduler.c \
|
||||||
scheduler_tasks.c \
|
scheduler/scheduler_tasks.c \
|
||||||
main.c \
|
main.c \
|
||||||
mw.c \
|
mw.c \
|
||||||
flight/failsafe.c \
|
flight/failsafe.c \
|
||||||
|
|
|
||||||
|
|
@ -21,10 +21,15 @@
|
||||||
#define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2*!!(condition)]))
|
#define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2*!!(condition)]))
|
||||||
|
|
||||||
#ifdef UNIT_TEST
|
#ifdef UNIT_TEST
|
||||||
#define STATIC_UNIT_TESTED // make visible to unit test
|
// make these visible to unit test
|
||||||
|
#define STATIC_UNIT_TESTED
|
||||||
|
#define STATIC_INLINE_UNIT_TESTED
|
||||||
|
#define INLINE_UNIT_TESTED
|
||||||
#define UNIT_TESTED
|
#define UNIT_TESTED
|
||||||
#else
|
#else
|
||||||
#define STATIC_UNIT_TESTED static
|
#define STATIC_UNIT_TESTED static
|
||||||
|
#define STATIC_INLINE_UNIT_TESTED static inline
|
||||||
|
#define INLINE_UNIT_TESTED inline
|
||||||
#define UNIT_TESTED
|
#define UNIT_TESTED
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
||||||
100
src/main/config/config_unittest.h
Normal file
100
src/main/config/config_unittest.h
Normal file
|
|
@ -0,0 +1,100 @@
|
||||||
|
/*
|
||||||
|
* This file is part of Cleanflight.
|
||||||
|
*
|
||||||
|
* Cleanflight is free software: you can redistribute it and/or modify
|
||||||
|
* it 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 is distributed in the hope that it 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 Cleanflight. 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;
|
||||||
|
uint32_t unittest_scheduler_timeToNextRealtimeTask;
|
||||||
|
bool unittest_outsideRealtimeGuardInterval;
|
||||||
|
|
||||||
|
#define GET_SCHEDULER_LOCALS() \
|
||||||
|
{ \
|
||||||
|
unittest_scheduler_selectedTask = selectedTask; \
|
||||||
|
unittest_scheduler_selectedTaskDynamicPriority = selectedTaskDynamicPriority; \
|
||||||
|
unittest_scheduler_waitingTasks = waitingTasks; \
|
||||||
|
unittest_scheduler_timeToNextRealtimeTask = timeToNextRealtimeTask; \
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
#define SET_SCHEDULER_LOCALS() {}
|
||||||
|
#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_
|
||||||
|
|
||||||
|
|
@ -24,9 +24,10 @@
|
||||||
#include <ctype.h>
|
#include <ctype.h>
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
#include "scheduler.h"
|
|
||||||
#include "version.h"
|
#include "version.h"
|
||||||
|
|
||||||
|
#include "scheduler/scheduler.h"
|
||||||
|
|
||||||
#include "build_config.h"
|
#include "build_config.h"
|
||||||
|
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
|
|
@ -2442,11 +2443,13 @@ static void cliTasks(char *cmdline)
|
||||||
cfTaskId_e taskId;
|
cfTaskId_e taskId;
|
||||||
cfTaskInfo_t taskInfo;
|
cfTaskInfo_t taskInfo;
|
||||||
|
|
||||||
cliPrintf("Task list:\r\n");
|
cliPrintf("Task list max/us avg/us rate/hz total/ms\r\n");
|
||||||
for (taskId = 0; taskId < TASK_COUNT; taskId++) {
|
for (taskId = 0; taskId < TASK_COUNT; taskId++) {
|
||||||
getTaskInfo(taskId, &taskInfo);
|
getTaskInfo(taskId, &taskInfo);
|
||||||
if (taskInfo.isEnabled) {
|
if (taskInfo.isEnabled) {
|
||||||
cliPrintf("%d - %s, max = %d us, avg = %d us, total = %d ms\r\n", taskId, taskInfo.taskName, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskInfo.totalExecutionTime / 1000);
|
const uint16_t taskFrequency = (uint16_t)(1.0f / ((float)taskInfo.latestDeltaTime * 0.000001f));
|
||||||
|
cliPrintf("%2d - %12s, %6d, %5d, %5d, %8d\r\n",
|
||||||
|
taskId, taskInfo.taskName, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskFrequency, taskInfo.totalExecutionTime / 1000);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -24,7 +24,8 @@
|
||||||
#include "build_config.h"
|
#include "build_config.h"
|
||||||
#include "debug.h"
|
#include "debug.h"
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
#include "scheduler.h"
|
|
||||||
|
#include "scheduler/scheduler.h"
|
||||||
|
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "common/color.h"
|
#include "common/color.h"
|
||||||
|
|
|
||||||
|
|
@ -21,7 +21,8 @@
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
#include "scheduler.h"
|
|
||||||
|
#include "scheduler/scheduler.h"
|
||||||
|
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "common/color.h"
|
#include "common/color.h"
|
||||||
|
|
@ -542,16 +543,15 @@ int main(void) {
|
||||||
init();
|
init();
|
||||||
|
|
||||||
/* Setup scheduler */
|
/* Setup scheduler */
|
||||||
if (masterConfig.gyroSync) {
|
schedulerInit();
|
||||||
rescheduleTask(TASK_GYROPID, targetLooptime - INTERRUPT_WAIT_TIME);
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
rescheduleTask(TASK_GYROPID, targetLooptime);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
rescheduleTask(TASK_GYROPID, targetLooptime);
|
||||||
setTaskEnabled(TASK_GYROPID, true);
|
setTaskEnabled(TASK_GYROPID, true);
|
||||||
|
|
||||||
setTaskEnabled(TASK_SERIAL, true);
|
setTaskEnabled(TASK_SERIAL, true);
|
||||||
|
#ifdef BEEPER
|
||||||
setTaskEnabled(TASK_BEEPER, true);
|
setTaskEnabled(TASK_BEEPER, true);
|
||||||
|
#endif
|
||||||
setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER));
|
setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER));
|
||||||
setTaskEnabled(TASK_RX, true);
|
setTaskEnabled(TASK_RX, true);
|
||||||
#ifdef GPS
|
#ifdef GPS
|
||||||
|
|
|
||||||
|
|
@ -21,9 +21,10 @@
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
#include "scheduler.h"
|
|
||||||
#include "debug.h"
|
#include "debug.h"
|
||||||
|
|
||||||
|
#include "scheduler/scheduler.h"
|
||||||
|
|
||||||
#include "common/maths.h"
|
#include "common/maths.h"
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "common/color.h"
|
#include "common/color.h"
|
||||||
|
|
|
||||||
|
|
@ -1,226 +0,0 @@
|
||||||
/*
|
|
||||||
* This file is part of Cleanflight.
|
|
||||||
*
|
|
||||||
* Cleanflight is free software: you can redistribute it and/or modify
|
|
||||||
* it 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 is distributed in the hope that it 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <stdbool.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <stdint.h>
|
|
||||||
|
|
||||||
#include "platform.h"
|
|
||||||
#include "scheduler.h"
|
|
||||||
#include "debug.h"
|
|
||||||
|
|
||||||
#include "common/maths.h"
|
|
||||||
|
|
||||||
#include "drivers/system.h"
|
|
||||||
|
|
||||||
cfTaskId_e currentTaskId = TASK_NONE;
|
|
||||||
|
|
||||||
static uint32_t totalWaitingTasks;
|
|
||||||
static uint32_t totalWaitingTasksSamples;
|
|
||||||
static uint32_t realtimeGuardInterval;
|
|
||||||
|
|
||||||
uint32_t currentTime = 0;
|
|
||||||
uint16_t averageSystemLoadPercent = 0;
|
|
||||||
|
|
||||||
#define REALTIME_GUARD_INTERVAL_MIN 10
|
|
||||||
#define REALTIME_GUARD_INTERVAL_MAX 300
|
|
||||||
#define REALTIME_GUARD_INTERVAL_MARGIN 25
|
|
||||||
|
|
||||||
void taskSystem(void)
|
|
||||||
{
|
|
||||||
uint8_t taskId;
|
|
||||||
|
|
||||||
/* Calculate system load */
|
|
||||||
if (totalWaitingTasksSamples > 0) {
|
|
||||||
averageSystemLoadPercent = 100 * totalWaitingTasks / totalWaitingTasksSamples;
|
|
||||||
totalWaitingTasksSamples = 0;
|
|
||||||
totalWaitingTasks = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Calculate guard interval */
|
|
||||||
uint32_t maxNonRealtimeTaskTime = 0;
|
|
||||||
for (taskId = 0; taskId < TASK_COUNT; taskId++) {
|
|
||||||
if (cfTasks[taskId].staticPriority != TASK_PRIORITY_REALTIME) {
|
|
||||||
maxNonRealtimeTaskTime = MAX(maxNonRealtimeTaskTime, cfTasks[taskId].averageExecutionTime);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
realtimeGuardInterval = constrain(maxNonRealtimeTaskTime, REALTIME_GUARD_INTERVAL_MIN, REALTIME_GUARD_INTERVAL_MAX) + REALTIME_GUARD_INTERVAL_MARGIN;
|
|
||||||
#if defined SCHEDULER_DEBUG
|
|
||||||
debug[2] = realtimeGuardInterval;
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
#ifndef SKIP_TASK_STATISTICS
|
|
||||||
void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
|
|
||||||
{
|
|
||||||
taskInfo->taskName = cfTasks[taskId].taskName;
|
|
||||||
taskInfo->isEnabled= cfTasks[taskId].isEnabled;
|
|
||||||
taskInfo->desiredPeriod = cfTasks[taskId].desiredPeriod;
|
|
||||||
taskInfo->staticPriority = cfTasks[taskId].staticPriority;
|
|
||||||
taskInfo->maxExecutionTime = cfTasks[taskId].maxExecutionTime;
|
|
||||||
taskInfo->totalExecutionTime = cfTasks[taskId].totalExecutionTime;
|
|
||||||
taskInfo->averageExecutionTime = cfTasks[taskId].averageExecutionTime;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros)
|
|
||||||
{
|
|
||||||
if (taskId == TASK_SELF)
|
|
||||||
taskId = currentTaskId;
|
|
||||||
|
|
||||||
if (taskId < TASK_COUNT) {
|
|
||||||
cfTasks[taskId].desiredPeriod = MAX(100, newPeriodMicros); // Limit delay to 100us (10 kHz) to prevent scheduler clogging
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState)
|
|
||||||
{
|
|
||||||
if (taskId == TASK_SELF)
|
|
||||||
taskId = currentTaskId;
|
|
||||||
|
|
||||||
if (taskId < TASK_COUNT) {
|
|
||||||
cfTasks[taskId].isEnabled = newEnabledState;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
uint32_t getTaskDeltaTime(cfTaskId_e taskId)
|
|
||||||
{
|
|
||||||
if (taskId == TASK_SELF)
|
|
||||||
taskId = currentTaskId;
|
|
||||||
|
|
||||||
if (taskId < TASK_COUNT) {
|
|
||||||
return cfTasks[taskId].taskLatestDeltaTime;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void scheduler(void)
|
|
||||||
{
|
|
||||||
uint8_t taskId;
|
|
||||||
uint8_t selectedTaskId;
|
|
||||||
uint8_t selectedTaskDynPrio;
|
|
||||||
uint16_t waitingTasks = 0;
|
|
||||||
uint32_t timeToNextRealtimeTask = UINT32_MAX;
|
|
||||||
|
|
||||||
/* Cache currentTime */
|
|
||||||
currentTime = micros();
|
|
||||||
|
|
||||||
/* The task to be invoked */
|
|
||||||
selectedTaskId = TASK_NONE;
|
|
||||||
selectedTaskDynPrio = 0;
|
|
||||||
|
|
||||||
/* Check for realtime tasks */
|
|
||||||
for (taskId = 0; taskId < TASK_COUNT; taskId++) {
|
|
||||||
if (cfTasks[taskId].staticPriority == TASK_PRIORITY_REALTIME) {
|
|
||||||
uint32_t nextExecuteAt = cfTasks[taskId].lastExecutedAt + cfTasks[taskId].desiredPeriod;
|
|
||||||
if ((int32_t)(currentTime - nextExecuteAt) >= 0) {
|
|
||||||
timeToNextRealtimeTask = 0;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
uint32_t newTimeInterval = nextExecuteAt - currentTime;
|
|
||||||
timeToNextRealtimeTask = MIN(timeToNextRealtimeTask, newTimeInterval);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > realtimeGuardInterval);
|
|
||||||
|
|
||||||
/* Update task dynamic priorities */
|
|
||||||
for (taskId = 0; taskId < TASK_COUNT; taskId++) {
|
|
||||||
if (cfTasks[taskId].isEnabled) {
|
|
||||||
/* Task has checkFunc - event driven */
|
|
||||||
if (cfTasks[taskId].checkFunc != NULL) {
|
|
||||||
/* Increase priority for event driven tasks */
|
|
||||||
if (cfTasks[taskId].dynamicPriority > 0) {
|
|
||||||
cfTasks[taskId].taskAgeCycles = 1 + ((currentTime - cfTasks[taskId].lastSignaledAt) / cfTasks[taskId].desiredPeriod);
|
|
||||||
cfTasks[taskId].dynamicPriority = 1 + cfTasks[taskId].staticPriority * cfTasks[taskId].taskAgeCycles;
|
|
||||||
waitingTasks++;
|
|
||||||
}
|
|
||||||
else if (cfTasks[taskId].checkFunc(currentTime - cfTasks[taskId].lastExecutedAt)) {
|
|
||||||
cfTasks[taskId].lastSignaledAt = currentTime;
|
|
||||||
cfTasks[taskId].taskAgeCycles = 1;
|
|
||||||
cfTasks[taskId].dynamicPriority = 1 + cfTasks[taskId].staticPriority;
|
|
||||||
waitingTasks++;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
cfTasks[taskId].taskAgeCycles = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
/* Task is time-driven, dynamicPriority is last execution age measured in desiredPeriods) */
|
|
||||||
else {
|
|
||||||
// Task age is calculated from last execution
|
|
||||||
cfTasks[taskId].taskAgeCycles = ((currentTime - cfTasks[taskId].lastExecutedAt) / cfTasks[taskId].desiredPeriod);
|
|
||||||
if (cfTasks[taskId].taskAgeCycles > 0) {
|
|
||||||
cfTasks[taskId].dynamicPriority = 1 + cfTasks[taskId].staticPriority * cfTasks[taskId].taskAgeCycles;
|
|
||||||
waitingTasks++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/* limit new priority to avoid overflow of uint8_t */
|
|
||||||
cfTasks[taskId].dynamicPriority = MIN(cfTasks[taskId].dynamicPriority, TASK_PRIORITY_MAX);;
|
|
||||||
|
|
||||||
bool taskCanBeChosenForScheduling =
|
|
||||||
(outsideRealtimeGuardInterval) ||
|
|
||||||
(cfTasks[taskId].taskAgeCycles > 1) ||
|
|
||||||
(cfTasks[taskId].staticPriority == TASK_PRIORITY_REALTIME);
|
|
||||||
|
|
||||||
if (taskCanBeChosenForScheduling && (cfTasks[taskId].dynamicPriority > selectedTaskDynPrio)) {
|
|
||||||
selectedTaskDynPrio = cfTasks[taskId].dynamicPriority;
|
|
||||||
selectedTaskId = taskId;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
totalWaitingTasksSamples += 1;
|
|
||||||
totalWaitingTasks += waitingTasks;
|
|
||||||
|
|
||||||
/* Found a task that should be run */
|
|
||||||
if (selectedTaskId != TASK_NONE) {
|
|
||||||
cfTasks[selectedTaskId].taskLatestDeltaTime = currentTime - cfTasks[selectedTaskId].lastExecutedAt;
|
|
||||||
cfTasks[selectedTaskId].lastExecutedAt = currentTime;
|
|
||||||
cfTasks[selectedTaskId].dynamicPriority = 0;
|
|
||||||
|
|
||||||
currentTaskId = selectedTaskId;
|
|
||||||
|
|
||||||
uint32_t currentTimeBeforeTaskCall = micros();
|
|
||||||
|
|
||||||
/* Execute task */
|
|
||||||
if (cfTasks[selectedTaskId].taskFunc != NULL) {
|
|
||||||
cfTasks[selectedTaskId].taskFunc();
|
|
||||||
}
|
|
||||||
|
|
||||||
uint32_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
|
|
||||||
|
|
||||||
cfTasks[selectedTaskId].averageExecutionTime = ((uint32_t)cfTasks[selectedTaskId].averageExecutionTime * 31 + taskExecutionTime) / 32;
|
|
||||||
#ifndef SKIP_TASK_STATISTICS
|
|
||||||
cfTasks[selectedTaskId].totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task
|
|
||||||
cfTasks[selectedTaskId].maxExecutionTime = MAX(cfTasks[selectedTaskId].maxExecutionTime, taskExecutionTime);
|
|
||||||
#endif
|
|
||||||
#if defined SCHEDULER_DEBUG
|
|
||||||
debug[3] = (micros() - currentTime) - taskExecutionTime;
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
currentTaskId = TASK_NONE;
|
|
||||||
#if defined SCHEDULER_DEBUG
|
|
||||||
debug[3] = (micros() - currentTime);
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
}
|
|
||||||
286
src/main/scheduler/scheduler.c
Executable file
286
src/main/scheduler/scheduler.c
Executable file
|
|
@ -0,0 +1,286 @@
|
||||||
|
/*
|
||||||
|
* This file is part of Cleanflight.
|
||||||
|
*
|
||||||
|
* Cleanflight is free software: you can redistribute it and/or modify
|
||||||
|
* it 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 is distributed in the hope that it 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
#ifdef UNIT_TEST
|
||||||
|
typedef enum {TEST_IRQ = 0 } IRQn_Type;
|
||||||
|
#endif
|
||||||
|
#include "platform.h"
|
||||||
|
|
||||||
|
#include "scheduler.h"
|
||||||
|
#include "debug.h"
|
||||||
|
#include "build_config.h"
|
||||||
|
|
||||||
|
#include "common/maths.h"
|
||||||
|
|
||||||
|
#include "drivers/system.h"
|
||||||
|
|
||||||
|
static cfTask_t *currentTask = NULL;
|
||||||
|
|
||||||
|
static uint32_t totalWaitingTasks;
|
||||||
|
static uint32_t totalWaitingTasksSamples;
|
||||||
|
static uint32_t realtimeGuardInterval;
|
||||||
|
|
||||||
|
uint32_t currentTime = 0;
|
||||||
|
uint16_t averageSystemLoadPercent = 0;
|
||||||
|
|
||||||
|
#define REALTIME_GUARD_INTERVAL_MIN 10
|
||||||
|
#define REALTIME_GUARD_INTERVAL_MAX 300
|
||||||
|
#define REALTIME_GUARD_INTERVAL_MARGIN 25
|
||||||
|
|
||||||
|
static int taskQueuePos = 0;
|
||||||
|
static int taskQueueSize = 0;
|
||||||
|
// No need for a linked list for the queue, since items are only inserted at startup
|
||||||
|
#ifdef UNIT_TEST
|
||||||
|
STATIC_UNIT_TESTED cfTask_t* taskQueueArray[TASK_COUNT + 2]; // 1 extra space so test code can check for buffer overruns
|
||||||
|
#else
|
||||||
|
static cfTask_t* taskQueueArray[TASK_COUNT + 1]; // extra item for NULL pointer at end of queue
|
||||||
|
#endif
|
||||||
|
STATIC_UNIT_TESTED void queueClear(void)
|
||||||
|
{
|
||||||
|
memset(taskQueueArray, 0, sizeof(taskQueueArray));
|
||||||
|
taskQueuePos = 0;
|
||||||
|
taskQueueSize = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef UNIT_TEST
|
||||||
|
STATIC_UNIT_TESTED int queueSize(void)
|
||||||
|
{
|
||||||
|
return taskQueueSize;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
STATIC_UNIT_TESTED bool queueContains(cfTask_t *task)
|
||||||
|
{
|
||||||
|
for (int ii = 0; ii < taskQueueSize; ++ii) {
|
||||||
|
if (taskQueueArray[ii] == task) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
STATIC_UNIT_TESTED bool queueAdd(cfTask_t *task)
|
||||||
|
{
|
||||||
|
if ((taskQueueSize >= TASK_COUNT) || queueContains(task)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
for (int ii = 0; ii <= taskQueueSize; ++ii) {
|
||||||
|
if (taskQueueArray[ii] == NULL || taskQueueArray[ii]->staticPriority < task->staticPriority) {
|
||||||
|
memmove(&taskQueueArray[ii+1], &taskQueueArray[ii], sizeof(task) * (taskQueueSize - ii));
|
||||||
|
taskQueueArray[ii] = task;
|
||||||
|
++taskQueueSize;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
STATIC_UNIT_TESTED bool queueRemove(cfTask_t *task)
|
||||||
|
{
|
||||||
|
for (int ii = 0; ii < taskQueueSize; ++ii) {
|
||||||
|
if (taskQueueArray[ii] == task) {
|
||||||
|
memmove(&taskQueueArray[ii], &taskQueueArray[ii+1], sizeof(task) * (taskQueueSize - ii));
|
||||||
|
--taskQueueSize;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Returns first item queue or NULL if queue empty
|
||||||
|
*/
|
||||||
|
STATIC_INLINE_UNIT_TESTED cfTask_t *queueFirst(void)
|
||||||
|
{
|
||||||
|
taskQueuePos = 0;
|
||||||
|
return taskQueueArray[0]; // guaranteed to be NULL if queue is empty
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Returns next item in queue or NULL if at end of queue
|
||||||
|
*/
|
||||||
|
STATIC_INLINE_UNIT_TESTED cfTask_t *queueNext(void)
|
||||||
|
{
|
||||||
|
return taskQueueArray[++taskQueuePos]; // guaranteed to be NULL at end of queue
|
||||||
|
}
|
||||||
|
|
||||||
|
void taskSystem(void)
|
||||||
|
{
|
||||||
|
/* Calculate system load */
|
||||||
|
if (totalWaitingTasksSamples > 0) {
|
||||||
|
averageSystemLoadPercent = 100 * totalWaitingTasks / totalWaitingTasksSamples;
|
||||||
|
totalWaitingTasksSamples = 0;
|
||||||
|
totalWaitingTasks = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Calculate guard interval */
|
||||||
|
uint32_t maxNonRealtimeTaskTime = 0;
|
||||||
|
for (const cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) {
|
||||||
|
if (task->staticPriority != TASK_PRIORITY_REALTIME) {
|
||||||
|
maxNonRealtimeTaskTime = MAX(maxNonRealtimeTaskTime, task->averageExecutionTime);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
realtimeGuardInterval = constrain(maxNonRealtimeTaskTime, REALTIME_GUARD_INTERVAL_MIN, REALTIME_GUARD_INTERVAL_MAX) + REALTIME_GUARD_INTERVAL_MARGIN;
|
||||||
|
#if defined SCHEDULER_DEBUG
|
||||||
|
debug[2] = realtimeGuardInterval;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifndef SKIP_TASK_STATISTICS
|
||||||
|
void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
|
||||||
|
{
|
||||||
|
taskInfo->taskName = cfTasks[taskId].taskName;
|
||||||
|
taskInfo->isEnabled = queueContains(&cfTasks[taskId]);
|
||||||
|
taskInfo->desiredPeriod = cfTasks[taskId].desiredPeriod;
|
||||||
|
taskInfo->staticPriority = cfTasks[taskId].staticPriority;
|
||||||
|
taskInfo->maxExecutionTime = cfTasks[taskId].maxExecutionTime;
|
||||||
|
taskInfo->totalExecutionTime = cfTasks[taskId].totalExecutionTime;
|
||||||
|
taskInfo->averageExecutionTime = cfTasks[taskId].averageExecutionTime;
|
||||||
|
taskInfo->latestDeltaTime = cfTasks[taskId].taskLatestDeltaTime;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros)
|
||||||
|
{
|
||||||
|
if (taskId == TASK_SELF || taskId < TASK_COUNT) {
|
||||||
|
cfTask_t *task = taskId == TASK_SELF ? currentTask : &cfTasks[taskId];
|
||||||
|
task->desiredPeriod = MAX((uint32_t)100, newPeriodMicros); // Limit delay to 100us (10 kHz) to prevent scheduler clogging
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void setTaskEnabled(cfTaskId_e taskId, bool enabled)
|
||||||
|
{
|
||||||
|
if (taskId == TASK_SELF || taskId < TASK_COUNT) {
|
||||||
|
cfTask_t *task = taskId == TASK_SELF ? currentTask : &cfTasks[taskId];
|
||||||
|
if (enabled && task->taskFunc) {
|
||||||
|
queueAdd(task);
|
||||||
|
} else {
|
||||||
|
queueRemove(task);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t getTaskDeltaTime(cfTaskId_e taskId)
|
||||||
|
{
|
||||||
|
if (taskId == TASK_SELF || taskId < TASK_COUNT) {
|
||||||
|
cfTask_t *task = taskId == TASK_SELF ? currentTask : &cfTasks[taskId];
|
||||||
|
return task->taskLatestDeltaTime;
|
||||||
|
} else {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void schedulerInit(void)
|
||||||
|
{
|
||||||
|
queueClear();
|
||||||
|
queueAdd(&cfTasks[TASK_SYSTEM]);
|
||||||
|
}
|
||||||
|
|
||||||
|
void scheduler(void)
|
||||||
|
{
|
||||||
|
// Cache currentTime
|
||||||
|
currentTime = micros();
|
||||||
|
|
||||||
|
uint32_t timeToNextRealtimeTask = UINT32_MAX;
|
||||||
|
for (const cfTask_t *task = queueFirst(); task != NULL && task->staticPriority >= TASK_PRIORITY_REALTIME; task = queueNext()) {
|
||||||
|
const uint32_t nextExecuteAt = task->lastExecutedAt + task->desiredPeriod;
|
||||||
|
if ((int32_t)(currentTime - nextExecuteAt) >= 0) {
|
||||||
|
timeToNextRealtimeTask = 0;
|
||||||
|
} else {
|
||||||
|
const uint32_t newTimeInterval = nextExecuteAt - currentTime;
|
||||||
|
timeToNextRealtimeTask = MIN(timeToNextRealtimeTask, newTimeInterval);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
const bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > realtimeGuardInterval);
|
||||||
|
|
||||||
|
// The task to be invoked
|
||||||
|
cfTask_t *selectedTask = NULL;
|
||||||
|
uint16_t selectedTaskDynamicPriority = 0;
|
||||||
|
|
||||||
|
// Update task dynamic priorities
|
||||||
|
uint16_t waitingTasks = 0;
|
||||||
|
for (cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) {
|
||||||
|
// Task has checkFunc - event driven
|
||||||
|
if (task->checkFunc != NULL) {
|
||||||
|
// Increase priority for event driven tasks
|
||||||
|
if (task->dynamicPriority > 0) {
|
||||||
|
task->taskAgeCycles = 1 + ((currentTime - task->lastSignaledAt) / task->desiredPeriod);
|
||||||
|
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
|
||||||
|
waitingTasks++;
|
||||||
|
} else if (task->checkFunc(currentTime - task->lastExecutedAt)) {
|
||||||
|
task->lastSignaledAt = currentTime;
|
||||||
|
task->taskAgeCycles = 1;
|
||||||
|
task->dynamicPriority = 1 + task->staticPriority;
|
||||||
|
waitingTasks++;
|
||||||
|
} else {
|
||||||
|
task->taskAgeCycles = 0;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
// Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods)
|
||||||
|
// Task age is calculated from last execution
|
||||||
|
task->taskAgeCycles = ((currentTime - task->lastExecutedAt) / task->desiredPeriod);
|
||||||
|
if (task->taskAgeCycles > 0) {
|
||||||
|
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
|
||||||
|
waitingTasks++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (task->dynamicPriority > selectedTaskDynamicPriority) {
|
||||||
|
const bool taskCanBeChosenForScheduling =
|
||||||
|
(outsideRealtimeGuardInterval) ||
|
||||||
|
(task->taskAgeCycles > 1) ||
|
||||||
|
(task->staticPriority == TASK_PRIORITY_REALTIME);
|
||||||
|
if (taskCanBeChosenForScheduling) {
|
||||||
|
selectedTaskDynamicPriority = task->dynamicPriority;
|
||||||
|
selectedTask = task;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
totalWaitingTasksSamples++;
|
||||||
|
totalWaitingTasks += waitingTasks;
|
||||||
|
|
||||||
|
currentTask = selectedTask;
|
||||||
|
|
||||||
|
if (selectedTask != NULL) {
|
||||||
|
// Found a task that should be run
|
||||||
|
selectedTask->taskLatestDeltaTime = currentTime - selectedTask->lastExecutedAt;
|
||||||
|
selectedTask->lastExecutedAt = currentTime;
|
||||||
|
selectedTask->dynamicPriority = 0;
|
||||||
|
|
||||||
|
// Execute task
|
||||||
|
const uint32_t currentTimeBeforeTaskCall = micros();
|
||||||
|
selectedTask->taskFunc();
|
||||||
|
const uint32_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
|
||||||
|
|
||||||
|
selectedTask->averageExecutionTime = ((uint32_t)selectedTask->averageExecutionTime * 31 + taskExecutionTime) / 32;
|
||||||
|
#ifndef SKIP_TASK_STATISTICS
|
||||||
|
selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task
|
||||||
|
selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
|
||||||
|
#endif
|
||||||
|
#if defined SCHEDULER_DEBUG
|
||||||
|
debug[3] = (micros() - currentTime) - taskExecutionTime;
|
||||||
|
} else {
|
||||||
|
debug[3] = (micros() - currentTime);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -36,6 +36,7 @@ typedef struct {
|
||||||
uint32_t maxExecutionTime;
|
uint32_t maxExecutionTime;
|
||||||
uint32_t totalExecutionTime;
|
uint32_t totalExecutionTime;
|
||||||
uint32_t averageExecutionTime;
|
uint32_t averageExecutionTime;
|
||||||
|
uint32_t latestDeltaTime;
|
||||||
} cfTaskInfo_t;
|
} cfTaskInfo_t;
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
|
|
@ -81,18 +82,17 @@ typedef struct {
|
||||||
const char * taskName;
|
const char * taskName;
|
||||||
bool (*checkFunc)(uint32_t currentDeltaTime);
|
bool (*checkFunc)(uint32_t currentDeltaTime);
|
||||||
void (*taskFunc)(void);
|
void (*taskFunc)(void);
|
||||||
bool isEnabled;
|
|
||||||
uint32_t desiredPeriod; // target period of execution
|
uint32_t desiredPeriod; // target period of execution
|
||||||
uint8_t staticPriority; // dynamicPriority grows in steps of this size, shouldn't be zero
|
const uint8_t staticPriority; // dynamicPriority grows in steps of this size, shouldn't be zero
|
||||||
|
|
||||||
/* Scheduling */
|
/* Scheduling */
|
||||||
uint8_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;
|
||||||
uint32_t lastExecutedAt; // last time of invocation
|
uint32_t lastExecutedAt; // last time of invocation
|
||||||
uint32_t lastSignaledAt; // time of invocation event for event-driven tasks
|
uint32_t lastSignaledAt; // time of invocation event for event-driven tasks
|
||||||
uint16_t taskAgeCycles;
|
|
||||||
|
|
||||||
/* Statistics */
|
/* Statistics */
|
||||||
uint32_t averageExecutionTime; // Moving averate over 6 samples, used to calculate guard interval
|
uint32_t averageExecutionTime; // Moving average over 6 samples, used to calculate guard interval
|
||||||
uint32_t taskLatestDeltaTime; //
|
uint32_t taskLatestDeltaTime; //
|
||||||
#ifndef SKIP_TASK_STATISTICS
|
#ifndef SKIP_TASK_STATISTICS
|
||||||
uint32_t maxExecutionTime;
|
uint32_t maxExecutionTime;
|
||||||
|
|
@ -109,6 +109,7 @@ void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros);
|
||||||
void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState);
|
void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState);
|
||||||
uint32_t getTaskDeltaTime(cfTaskId_e taskId);
|
uint32_t getTaskDeltaTime(cfTaskId_e taskId);
|
||||||
|
|
||||||
|
void schedulerInit(void);
|
||||||
void scheduler(void);
|
void scheduler(void);
|
||||||
|
|
||||||
#define isSystemOverloaded() (averageSystemLoadPercent >= 100)
|
#define isSystemOverloaded() (averageSystemLoadPercent >= 100)
|
||||||
|
|
@ -39,7 +39,6 @@ void taskSystem(void);
|
||||||
|
|
||||||
cfTask_t cfTasks[TASK_COUNT] = {
|
cfTask_t cfTasks[TASK_COUNT] = {
|
||||||
[TASK_SYSTEM] = {
|
[TASK_SYSTEM] = {
|
||||||
.isEnabled = true,
|
|
||||||
.taskName = "SYSTEM",
|
.taskName = "SYSTEM",
|
||||||
.taskFunc = taskSystem,
|
.taskFunc = taskSystem,
|
||||||
.desiredPeriod = 1000000 / 10, // run every 100 ms
|
.desiredPeriod = 1000000 / 10, // run every 100 ms
|
||||||
|
|
@ -20,7 +20,8 @@
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
#include "scheduler.h"
|
|
||||||
|
#include "scheduler/scheduler.h"
|
||||||
|
|
||||||
#include "common/maths.h"
|
#include "common/maths.h"
|
||||||
|
|
||||||
|
|
|
||||||
405
src/test/unit/scheduler_unittest.cc
Normal file
405
src/test/unit/scheduler_unittest.cc
Normal file
|
|
@ -0,0 +1,405 @@
|
||||||
|
/*
|
||||||
|
* This file is part of Cleanflight.
|
||||||
|
*
|
||||||
|
* Cleanflight is free software: you can redistribute it and/or modify
|
||||||
|
* it 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 is distributed in the hope that it 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
extern "C" {
|
||||||
|
#include "platform.h"
|
||||||
|
#include "scheduler.h"
|
||||||
|
}
|
||||||
|
|
||||||
|
#include "unittest_macros.h"
|
||||||
|
#include "gtest/gtest.h"
|
||||||
|
enum {
|
||||||
|
systemTime = 10,
|
||||||
|
pidLoopCheckerTime = 650,
|
||||||
|
updateAccelerometerTime = 192,
|
||||||
|
handleSerialTime = 30,
|
||||||
|
updateBeeperTime = 1,
|
||||||
|
updateBatteryTime = 1,
|
||||||
|
updateRxCheckTime = 34,
|
||||||
|
updateRxMainTime = 10,
|
||||||
|
processGPSTime = 10,
|
||||||
|
updateCompassTime = 195,
|
||||||
|
updateBaroTime = 201,
|
||||||
|
updateSonarTime = 10,
|
||||||
|
calculateAltitudeTime = 154,
|
||||||
|
updateDisplayTime = 10,
|
||||||
|
telemetryTime = 10,
|
||||||
|
ledStripTime = 10,
|
||||||
|
transponderTime = 10
|
||||||
|
};
|
||||||
|
|
||||||
|
extern "C" {
|
||||||
|
cfTask_t * unittest_scheduler_selectedTask;
|
||||||
|
uint8_t unittest_scheduler_selectedTaskDynPrio;
|
||||||
|
uint16_t unittest_scheduler_waitingTasks;
|
||||||
|
uint32_t unittest_scheduler_timeToNextRealtimeTask;
|
||||||
|
bool unittest_outsideRealtimeGuardInterval;
|
||||||
|
|
||||||
|
// set up micros() to simulate time
|
||||||
|
uint32_t simulatedTime = 0;
|
||||||
|
uint32_t micros(void) {return simulatedTime;}
|
||||||
|
// set up tasks to take a simulated representative time to execute
|
||||||
|
void taskMainPidLoopChecker(void) {simulatedTime+=pidLoopCheckerTime;}
|
||||||
|
void taskUpdateAccelerometer(void) {simulatedTime+=updateAccelerometerTime;}
|
||||||
|
void taskHandleSerial(void) {simulatedTime+=handleSerialTime;}
|
||||||
|
void taskUpdateBeeper(void) {simulatedTime+=updateBeeperTime;}
|
||||||
|
void taskUpdateBattery(void) {simulatedTime+=updateBatteryTime;}
|
||||||
|
bool taskUpdateRxCheck(uint32_t currentDeltaTime) {UNUSED(currentDeltaTime);simulatedTime+=updateRxCheckTime;return false;}
|
||||||
|
void taskUpdateRxMain(void) {simulatedTime+=updateRxMainTime;}
|
||||||
|
void taskProcessGPS(void) {simulatedTime+=processGPSTime;}
|
||||||
|
void taskUpdateCompass(void) {simulatedTime+=updateCompassTime;}
|
||||||
|
void taskUpdateBaro(void) {simulatedTime+=updateBaroTime;}
|
||||||
|
void taskUpdateSonar(void) {simulatedTime+=updateSonarTime;}
|
||||||
|
void taskCalculateAltitude(void) {simulatedTime+=calculateAltitudeTime;}
|
||||||
|
void taskUpdateDisplay(void) {simulatedTime+=updateDisplayTime;}
|
||||||
|
void taskTelemetry(void) {simulatedTime+=telemetryTime;}
|
||||||
|
void taskLedStrip(void) {simulatedTime+=ledStripTime;}
|
||||||
|
void taskTransponder(void) {simulatedTime+=transponderTime;}
|
||||||
|
|
||||||
|
extern cfTask_t* taskQueueArray[];
|
||||||
|
|
||||||
|
extern void queueClear(void);
|
||||||
|
extern int queueSize();
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(SchedulerUnittest, TestPriorites)
|
||||||
|
{
|
||||||
|
EXPECT_EQ(14, TASK_COUNT);
|
||||||
|
// if any of these fail then task priorities have changed and ordering in TestQueue needs to be re-checked
|
||||||
|
EXPECT_EQ(TASK_PRIORITY_HIGH, cfTasks[TASK_SYSTEM].staticPriority);
|
||||||
|
EXPECT_EQ(TASK_PRIORITY_REALTIME, cfTasks[TASK_GYROPID].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].staticPriority);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(SchedulerUnittest, TestQueueInit)
|
||||||
|
{
|
||||||
|
queueClear();
|
||||||
|
EXPECT_EQ(0, queueSize());
|
||||||
|
EXPECT_EQ(0, queueFirst());
|
||||||
|
EXPECT_EQ(0, queueNext());
|
||||||
|
for (int ii = 0; ii <= TASK_COUNT; ++ii) {
|
||||||
|
EXPECT_EQ(0, taskQueueArray[ii]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
cfTask_t *deadBeefPtr = reinterpret_cast<cfTask_t*>(0xDEADBEEF);
|
||||||
|
|
||||||
|
TEST(SchedulerUnittest, TestQueue)
|
||||||
|
{
|
||||||
|
queueClear();
|
||||||
|
taskQueueArray[TASK_COUNT + 1] = deadBeefPtr;
|
||||||
|
|
||||||
|
queueAdd(&cfTasks[TASK_SYSTEM]); // TASK_PRIORITY_HIGH
|
||||||
|
EXPECT_EQ(1, queueSize());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst());
|
||||||
|
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||||
|
|
||||||
|
queueAdd(&cfTasks[TASK_GYROPID]); // TASK_PRIORITY_REALTIME
|
||||||
|
EXPECT_EQ(2, queueSize());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
|
||||||
|
EXPECT_EQ(NULL, queueNext());
|
||||||
|
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||||
|
|
||||||
|
queueAdd(&cfTasks[TASK_SERIAL]); // TASK_PRIORITY_LOW
|
||||||
|
EXPECT_EQ(3, queueSize());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
|
||||||
|
EXPECT_EQ(NULL, queueNext());
|
||||||
|
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||||
|
|
||||||
|
queueAdd(&cfTasks[TASK_BEEPER]); // TASK_PRIORITY_MEDIUM
|
||||||
|
EXPECT_EQ(4, queueSize());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_BEEPER], queueNext());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
|
||||||
|
EXPECT_EQ(NULL, queueNext());
|
||||||
|
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||||
|
|
||||||
|
queueAdd(&cfTasks[TASK_RX]); // TASK_PRIORITY_HIGH
|
||||||
|
EXPECT_EQ(5, queueSize());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_RX], queueNext());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_BEEPER], queueNext());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
|
||||||
|
EXPECT_EQ(NULL, queueNext());
|
||||||
|
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||||
|
|
||||||
|
queueRemove(&cfTasks[TASK_SYSTEM]); // TASK_PRIORITY_HIGH
|
||||||
|
EXPECT_EQ(4, queueSize());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_RX], queueNext());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_BEEPER], queueNext());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
|
||||||
|
EXPECT_EQ(NULL, queueNext());
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(SchedulerUnittest, TestQueueAddAndRemove)
|
||||||
|
{
|
||||||
|
queueClear();
|
||||||
|
taskQueueArray[TASK_COUNT + 1] = deadBeefPtr;
|
||||||
|
|
||||||
|
// fill up the queue
|
||||||
|
for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
|
||||||
|
const bool added = queueAdd(&cfTasks[taskId]);
|
||||||
|
EXPECT_EQ(true, added);
|
||||||
|
EXPECT_EQ(taskId + 1, queueSize());
|
||||||
|
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||||
|
}
|
||||||
|
// double check end of queue
|
||||||
|
EXPECT_EQ(TASK_COUNT, queueSize());
|
||||||
|
EXPECT_NE(static_cast<cfTask_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]);
|
||||||
|
EXPECT_EQ(true, removed);
|
||||||
|
EXPECT_EQ(TASK_COUNT - taskId - 1, queueSize());
|
||||||
|
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - taskId]);
|
||||||
|
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||||
|
}
|
||||||
|
// double check size and end of queue
|
||||||
|
EXPECT_EQ(0, queueSize()); // queue is indeed empty
|
||||||
|
EXPECT_EQ(NULL, taskQueueArray[0]); // there is a null pointer at the end of the queueu
|
||||||
|
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); // no accidental overwrites past end of queue
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(SchedulerUnittest, TestQueueArray)
|
||||||
|
{
|
||||||
|
// test there are no "out by one" errors or buffer overruns when items are added and removed
|
||||||
|
queueClear();
|
||||||
|
taskQueueArray[TASK_COUNT + 1] = deadBeefPtr;
|
||||||
|
|
||||||
|
for (int taskId=0; taskId < TASK_COUNT - 1; ++taskId) {
|
||||||
|
setTaskEnabled(static_cast<cfTaskId_e>(taskId), true);
|
||||||
|
}
|
||||||
|
EXPECT_EQ(TASK_COUNT - 1, queueSize());
|
||||||
|
EXPECT_NE(static_cast<cfTask_t*>(0), taskQueueArray[TASK_COUNT - 2]);
|
||||||
|
const cfTask_t *lastTaskPrev = taskQueueArray[TASK_COUNT - 2];
|
||||||
|
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]);
|
||||||
|
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]);
|
||||||
|
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||||
|
|
||||||
|
setTaskEnabled(TASK_SYSTEM, false);
|
||||||
|
EXPECT_EQ(TASK_COUNT - 2, queueSize());
|
||||||
|
EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 3]);
|
||||||
|
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 2]); // NULL at end of queue
|
||||||
|
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]);
|
||||||
|
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]);
|
||||||
|
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||||
|
|
||||||
|
taskQueueArray[TASK_COUNT - 2] = 0;
|
||||||
|
setTaskEnabled(TASK_SYSTEM, true);
|
||||||
|
EXPECT_EQ(TASK_COUNT - 1, queueSize());
|
||||||
|
EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 2]);
|
||||||
|
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]);
|
||||||
|
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]);
|
||||||
|
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||||
|
|
||||||
|
cfTaskInfo_t taskInfo;
|
||||||
|
getTaskInfo(static_cast<cfTaskId_e>(TASK_COUNT - 1), &taskInfo);
|
||||||
|
EXPECT_EQ(false, taskInfo.isEnabled);
|
||||||
|
setTaskEnabled(static_cast<cfTaskId_e>(TASK_COUNT - 1), true);
|
||||||
|
EXPECT_EQ(TASK_COUNT, queueSize());
|
||||||
|
EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 1]);
|
||||||
|
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); // check no buffer overrun
|
||||||
|
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||||
|
|
||||||
|
setTaskEnabled(TASK_SYSTEM, false);
|
||||||
|
EXPECT_EQ(TASK_COUNT - 1, queueSize());
|
||||||
|
//EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 3]);
|
||||||
|
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]);
|
||||||
|
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]);
|
||||||
|
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||||
|
|
||||||
|
setTaskEnabled(TASK_ACCEL, false);
|
||||||
|
EXPECT_EQ(TASK_COUNT - 2, queueSize());
|
||||||
|
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 2]);
|
||||||
|
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]);
|
||||||
|
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]);
|
||||||
|
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||||
|
|
||||||
|
setTaskEnabled(TASK_BATTERY, false);
|
||||||
|
EXPECT_EQ(TASK_COUNT - 3, queueSize());
|
||||||
|
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 3]);
|
||||||
|
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 2]);
|
||||||
|
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]);
|
||||||
|
EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]);
|
||||||
|
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(SchedulerUnittest, TestSchedulerInit)
|
||||||
|
{
|
||||||
|
schedulerInit();
|
||||||
|
EXPECT_EQ(1, queueSize());
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst());
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(SchedulerUnittest, TestScheduleEmptyQueue)
|
||||||
|
{
|
||||||
|
queueClear();
|
||||||
|
simulatedTime = 4000;
|
||||||
|
// run the with an empty queue
|
||||||
|
scheduler();
|
||||||
|
EXPECT_EQ(NULL, unittest_scheduler_selectedTask);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(SchedulerUnittest, TestSingleTask)
|
||||||
|
{
|
||||||
|
schedulerInit();
|
||||||
|
// disable all tasks except TASK_GYROPID
|
||||||
|
for (int taskId=0; taskId < TASK_COUNT; ++taskId) {
|
||||||
|
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
|
||||||
|
}
|
||||||
|
setTaskEnabled(TASK_GYROPID, true);
|
||||||
|
cfTasks[TASK_GYROPID].lastExecutedAt = 1000;
|
||||||
|
simulatedTime = 4000;
|
||||||
|
// run the scheduler and check the task has executed
|
||||||
|
scheduler();
|
||||||
|
EXPECT_NE(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask);
|
||||||
|
EXPECT_EQ(3000, cfTasks[TASK_GYROPID].taskLatestDeltaTime);
|
||||||
|
EXPECT_EQ(4000, cfTasks[TASK_GYROPID].lastExecutedAt);
|
||||||
|
EXPECT_EQ(pidLoopCheckerTime, cfTasks[TASK_GYROPID].totalExecutionTime);
|
||||||
|
// task has run, so its dynamic priority should have been set to zero
|
||||||
|
EXPECT_EQ(0, cfTasks[TASK_GYROPID].dynamicPriority);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(SchedulerUnittest, TestTwoTasks)
|
||||||
|
{
|
||||||
|
// disable all tasks except TASK_GYROPID and TASK_ACCEL
|
||||||
|
for (int taskId=0; taskId < TASK_COUNT; ++taskId) {
|
||||||
|
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
|
||||||
|
}
|
||||||
|
setTaskEnabled(TASK_ACCEL, true);
|
||||||
|
setTaskEnabled(TASK_GYROPID, true);
|
||||||
|
|
||||||
|
// set it up so that TASK_ACCEL ran just before TASK_GYROPID
|
||||||
|
static const uint32_t startTime = 4000;
|
||||||
|
simulatedTime = startTime;
|
||||||
|
cfTasks[TASK_GYROPID].lastExecutedAt = simulatedTime;
|
||||||
|
cfTasks[TASK_ACCEL].lastExecutedAt = cfTasks[TASK_GYROPID].lastExecutedAt - updateAccelerometerTime;
|
||||||
|
EXPECT_EQ(0, cfTasks[TASK_ACCEL].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);
|
||||||
|
|
||||||
|
// NOTE:
|
||||||
|
// TASK_GYROPID desiredPeriod is 1000 microseconds
|
||||||
|
// TASK_ACCEL desiredPeriod 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(0, unittest_scheduler_waitingTasks);
|
||||||
|
|
||||||
|
// 500 microseconds later, TASK_GYROPID desiredPeriod has elapsed
|
||||||
|
simulatedTime += 500;
|
||||||
|
// TASK_GYROPID should now run
|
||||||
|
scheduler();
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask);
|
||||||
|
EXPECT_EQ(1, unittest_scheduler_waitingTasks);
|
||||||
|
EXPECT_EQ(5000 + pidLoopCheckerTime, simulatedTime);
|
||||||
|
|
||||||
|
simulatedTime += 1000 - pidLoopCheckerTime;
|
||||||
|
scheduler();
|
||||||
|
// TASK_GYROPID should run again
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask);
|
||||||
|
|
||||||
|
scheduler();
|
||||||
|
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
|
||||||
|
EXPECT_EQ(0, unittest_scheduler_waitingTasks);
|
||||||
|
|
||||||
|
simulatedTime = startTime + 10500; // TASK_GYROPID and TASK_ACCEL desiredPeriods have elapsed
|
||||||
|
// of the two TASK_GYROPID should run first
|
||||||
|
scheduler();
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask);
|
||||||
|
// and finally TASK_ACCEL should now run
|
||||||
|
scheduler();
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(SchedulerUnittest, TestRealTimeGuardInNoTaskRun)
|
||||||
|
{
|
||||||
|
// disable all tasks except TASK_GYROPID and TASK_SYSTEM
|
||||||
|
for (int taskId=0; taskId < TASK_COUNT; ++taskId) {
|
||||||
|
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
|
||||||
|
}
|
||||||
|
setTaskEnabled(TASK_GYROPID, true);
|
||||||
|
cfTasks[TASK_GYROPID].lastExecutedAt = 200000;
|
||||||
|
simulatedTime = 200700;
|
||||||
|
|
||||||
|
setTaskEnabled(TASK_SYSTEM, true);
|
||||||
|
cfTasks[TASK_SYSTEM].lastExecutedAt = 100000;
|
||||||
|
|
||||||
|
scheduler();
|
||||||
|
|
||||||
|
EXPECT_EQ(false, unittest_outsideRealtimeGuardInterval);
|
||||||
|
EXPECT_EQ(300, unittest_scheduler_timeToNextRealtimeTask);
|
||||||
|
|
||||||
|
// Nothing should be scheduled in guard period
|
||||||
|
EXPECT_EQ(NULL, unittest_scheduler_selectedTask);
|
||||||
|
EXPECT_EQ(100000, cfTasks[TASK_SYSTEM].lastExecutedAt);
|
||||||
|
|
||||||
|
EXPECT_EQ(200000, cfTasks[TASK_GYROPID].lastExecutedAt);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(SchedulerUnittest, TestRealTimeGuardOutTaskRun)
|
||||||
|
{
|
||||||
|
// disable all tasks except TASK_GYROPID and TASK_SYSTEM
|
||||||
|
for (int taskId=0; taskId < TASK_COUNT; ++taskId) {
|
||||||
|
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
|
||||||
|
}
|
||||||
|
setTaskEnabled(TASK_GYROPID, true);
|
||||||
|
cfTasks[TASK_GYROPID].lastExecutedAt = 200000;
|
||||||
|
simulatedTime = 200699;
|
||||||
|
|
||||||
|
setTaskEnabled(TASK_SYSTEM, true);
|
||||||
|
cfTasks[TASK_SYSTEM].lastExecutedAt = 100000;
|
||||||
|
|
||||||
|
scheduler();
|
||||||
|
|
||||||
|
EXPECT_EQ(true, unittest_outsideRealtimeGuardInterval);
|
||||||
|
EXPECT_EQ(301, unittest_scheduler_timeToNextRealtimeTask);
|
||||||
|
|
||||||
|
// System should be scheduled as not in guard period
|
||||||
|
EXPECT_EQ(&cfTasks[TASK_SYSTEM], unittest_scheduler_selectedTask);
|
||||||
|
EXPECT_EQ(200699, cfTasks[TASK_SYSTEM].lastExecutedAt);
|
||||||
|
|
||||||
|
EXPECT_EQ(200000, cfTasks[TASK_GYROPID].lastExecutedAt);
|
||||||
|
}
|
||||||
|
|
||||||
|
// STUBS
|
||||||
|
extern "C" {
|
||||||
|
}
|
||||||
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue