mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 08:15: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/encoding.c \
|
||||
common/filter.c \
|
||||
scheduler.c \
|
||||
scheduler_tasks.c \
|
||||
scheduler/scheduler.c \
|
||||
scheduler/scheduler_tasks.c \
|
||||
main.c \
|
||||
mw.c \
|
||||
flight/failsafe.c \
|
||||
|
|
|
|||
|
|
@ -21,10 +21,15 @@
|
|||
#define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2*!!(condition)]))
|
||||
|
||||
#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
|
||||
#else
|
||||
#define STATIC_UNIT_TESTED static
|
||||
#define STATIC_INLINE_UNIT_TESTED static inline
|
||||
#define INLINE_UNIT_TESTED inline
|
||||
#define UNIT_TESTED
|
||||
#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 "platform.h"
|
||||
#include "scheduler.h"
|
||||
#include "version.h"
|
||||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
#include "build_config.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
|
|
@ -2442,11 +2443,13 @@ static void cliTasks(char *cmdline)
|
|||
cfTaskId_e taskId;
|
||||
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++) {
|
||||
getTaskInfo(taskId, &taskInfo);
|
||||
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 "debug.h"
|
||||
#include "platform.h"
|
||||
#include "scheduler.h"
|
||||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/color.h"
|
||||
|
|
|
|||
|
|
@ -21,7 +21,8 @@
|
|||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
#include "scheduler.h"
|
||||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/color.h"
|
||||
|
|
@ -542,16 +543,15 @@ int main(void) {
|
|||
init();
|
||||
|
||||
/* Setup scheduler */
|
||||
if (masterConfig.gyroSync) {
|
||||
rescheduleTask(TASK_GYROPID, targetLooptime - INTERRUPT_WAIT_TIME);
|
||||
}
|
||||
else {
|
||||
rescheduleTask(TASK_GYROPID, targetLooptime);
|
||||
}
|
||||
schedulerInit();
|
||||
|
||||
rescheduleTask(TASK_GYROPID, targetLooptime);
|
||||
setTaskEnabled(TASK_GYROPID, true);
|
||||
|
||||
setTaskEnabled(TASK_SERIAL, true);
|
||||
#ifdef BEEPER
|
||||
setTaskEnabled(TASK_BEEPER, true);
|
||||
#endif
|
||||
setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER));
|
||||
setTaskEnabled(TASK_RX, true);
|
||||
#ifdef GPS
|
||||
|
|
|
|||
|
|
@ -21,9 +21,10 @@
|
|||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
#include "scheduler.h"
|
||||
#include "debug.h"
|
||||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
#include "common/maths.h"
|
||||
#include "common/axis.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 totalExecutionTime;
|
||||
uint32_t averageExecutionTime;
|
||||
uint32_t latestDeltaTime;
|
||||
} cfTaskInfo_t;
|
||||
|
||||
typedef enum {
|
||||
|
|
@ -81,18 +82,17 @@ typedef struct {
|
|||
const char * taskName;
|
||||
bool (*checkFunc)(uint32_t currentDeltaTime);
|
||||
void (*taskFunc)(void);
|
||||
bool isEnabled;
|
||||
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 */
|
||||
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 lastSignaledAt; // time of invocation event for event-driven tasks
|
||||
uint16_t taskAgeCycles;
|
||||
|
||||
/* 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; //
|
||||
#ifndef SKIP_TASK_STATISTICS
|
||||
uint32_t maxExecutionTime;
|
||||
|
|
@ -109,6 +109,7 @@ void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros);
|
|||
void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState);
|
||||
uint32_t getTaskDeltaTime(cfTaskId_e taskId);
|
||||
|
||||
void schedulerInit(void);
|
||||
void scheduler(void);
|
||||
|
||||
#define isSystemOverloaded() (averageSystemLoadPercent >= 100)
|
||||
|
|
@ -39,7 +39,6 @@ void taskSystem(void);
|
|||
|
||||
cfTask_t cfTasks[TASK_COUNT] = {
|
||||
[TASK_SYSTEM] = {
|
||||
.isEnabled = true,
|
||||
.taskName = "SYSTEM",
|
||||
.taskFunc = taskSystem,
|
||||
.desiredPeriod = 1000000 / 10, // run every 100 ms
|
||||
|
|
@ -20,7 +20,8 @@
|
|||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
#include "scheduler.h"
|
||||
|
||||
#include "scheduler/scheduler.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