diff --git a/src/main/scheduler.c b/src/main/scheduler.c index f0c2ebff0..54ba18062 100755 --- a/src/main/scheduler.c +++ b/src/main/scheduler.c @@ -18,12 +18,16 @@ #define SRC_MAIN_SCHEDULER_C_ #include -#include #include #include -#include - +#ifdef UNIT_TEST +// cannot include platform.h in UNIT_TEST build, since if included the build #defines (eg MAG, GPS, etc) +// are set differently to test code +typedef enum {TEST_IRQ = 0 } IRQn_Type; +#else +#include "platform.h" +#endif #include "scheduler.h" #include "debug.h" #include "build_config.h" @@ -80,7 +84,7 @@ STATIC_UNIT_TESTED bool queueContains(cfTask_t *task) STATIC_UNIT_TESTED void queueAdd(cfTask_t *task) { - if ((taskQueueSize >= TASK_COUNT - 1) || queueContains(task)) { + if ((taskQueueSize >= TASK_COUNT) || queueContains(task)) { return; } for (int ii = 0; ii <= taskQueueSize; ++ii) { @@ -195,49 +199,46 @@ void schedulerInit(void) void scheduler(void) { - /* Cache currentTime */ + // Cache currentTime currentTime = micros(); - /* Check for realtime tasks */ + // Check for realtime tasks 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 { + } else { const uint32_t newTimeInterval = nextExecuteAt - currentTime; timeToNextRealtimeTask = MIN(timeToNextRealtimeTask, newTimeInterval); } } + const bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > realtimeGuardInterval); - /* The task to be invoked */ - uint8_t selectedTaskDynPrio = 0; + // The task to be invoked + uint32_t selectedTaskDynPrio = 0; cfTask_t *selectedTask = NULL; - /* Update task dynamic priorities */ + // Update task dynamic priorities uint16_t waitingTasks = 0; for (cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) { - /* Task has checkFunc - event driven */ + // Task has checkFunc - event driven if (task->checkFunc != NULL) { - /* Increase priority for event driven tasks */ + // 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)) { + } else if (task->checkFunc(currentTime - task->lastExecutedAt)) { task->lastSignaledAt = currentTime; task->taskAgeCycles = 1; task->dynamicPriority = 1 + task->staticPriority; waitingTasks++; - } - else { + } else { task->taskAgeCycles = 0; } - } - /* Task is time-driven, dynamicPriority is last execution age measured in desiredPeriods) */ - else { + } 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) { @@ -246,11 +247,7 @@ void scheduler(void) } } - /* limit new priority to avoid overflow of uint8_t */ - task->dynamicPriority = MIN(task->dynamicPriority, TASK_PRIORITY_MAX);; - if (task->dynamicPriority > selectedTaskDynPrio) { - const bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > realtimeGuardInterval); const bool taskCanBeChosenForScheduling = (outsideRealtimeGuardInterval) || (task->taskAgeCycles > 1) || @@ -267,13 +264,13 @@ void scheduler(void) currentTask = selectedTask; - /* Found a task that should be run */ + // Found a task that should be run if (selectedTask != NULL) { selectedTask->taskLatestDeltaTime = currentTime - selectedTask->lastExecutedAt; selectedTask->lastExecutedAt = currentTime; selectedTask->dynamicPriority = 0; - /* Execute task */ + // Execute task const uint32_t currentTimeBeforeTaskCall = micros(); selectedTask->taskFunc(); const uint32_t taskExecutionTime = micros() - currentTimeBeforeTaskCall; diff --git a/src/main/scheduler.h b/src/main/scheduler.h index bbea64f93..b642368b5 100755 --- a/src/main/scheduler.h +++ b/src/main/scheduler.h @@ -90,17 +90,17 @@ typedef struct { const char * taskName; bool (*checkFunc)(uint32_t currentDeltaTime); void (*taskFunc)(void); - uint32_t desiredPeriod; // target period of execution - const uint8_t staticPriority;// dynamicPriority grows in steps of this size, shouldn't be zero + uint32_t desiredPeriod; // target period of execution + 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 - uint32_t lastExecutedAt; // last time of invocation - uint32_t lastSignaledAt; // time of invocation event for event-driven tasks + 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 /* 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; diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc index 1619a21cf..2356d29a9 100644 --- a/src/test/unit/scheduler_unittest.cc +++ b/src/test/unit/scheduler_unittest.cc @@ -184,32 +184,56 @@ TEST(SchedulerUnittest, TestQueueArray) } EXPECT_EQ(TASK_COUNT - 1, queueSize()); EXPECT_NE(static_cast(0), taskQueueArray[TASK_COUNT - 2]); - cfTask_t *lastTaskPrev = taskQueueArray[TASK_COUNT - 2]; - EXPECT_EQ(static_cast(0), taskQueueArray[TASK_COUNT - 1]); - EXPECT_EQ(static_cast(0), taskQueueArray[TASK_COUNT]); + 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(static_cast(0), taskQueueArray[TASK_COUNT - 1]); - EXPECT_EQ(static_cast(0), taskQueueArray[TASK_COUNT]); + 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(static_cast(0), taskQueueArray[TASK_COUNT - 1]); - EXPECT_EQ(static_cast(0), taskQueueArray[TASK_COUNT]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); - // now there are TASK_COUNT items in the array + cfTaskInfo_t taskInfo; + getTaskInfo(static_cast(TASK_COUNT - 1), &taskInfo); + EXPECT_EQ(false, taskInfo.isEnabled); setTaskEnabled(static_cast(TASK_COUNT - 1), true); EXPECT_EQ(TASK_COUNT, queueSize()); EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 1]); - EXPECT_EQ(static_cast(0), taskQueueArray[TASK_COUNT]); // check no buffer overrun + 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]); } @@ -226,7 +250,7 @@ TEST(SchedulerUnittest, TestScheduleEmptyQueue) simulatedTime = 4000; // run the with an empty queue scheduler(); - EXPECT_EQ(static_cast(0), unittest_scheduler_selectedTask); + EXPECT_EQ(NULL, unittest_scheduler_selectedTask); } TEST(SchedulerUnittest, TestSingleTask) @@ -268,7 +292,7 @@ TEST(SchedulerUnittest, TestTwoTasks) // run the scheduler scheduler(); // no tasks should have run, since neither task's desired time has elapsed - EXPECT_EQ(TASK_NONE, unittest_scheduler_selectedTaskId); + EXPECT_EQ(static_cast(0), unittest_scheduler_selectedTask); // NOTE: // TASK_GYROPID desiredPeriod is 1000 microseconds @@ -277,33 +301,33 @@ TEST(SchedulerUnittest, TestTwoTasks) simulatedTime += 500; // no tasks should run, since neither task's desired time has elapsed scheduler(); - EXPECT_EQ(TASK_NONE, unittest_scheduler_selectedTaskId); + EXPECT_EQ(static_cast(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(TASK_GYROPID, unittest_scheduler_selectedTaskId); + 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(TASK_GYROPID, unittest_scheduler_selectedTaskId); + EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); scheduler(); - EXPECT_EQ(TASK_NONE, unittest_scheduler_selectedTaskId); + EXPECT_EQ(static_cast(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(TASK_GYROPID, unittest_scheduler_selectedTaskId); + EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); // and finally TASK_ACCEL should now run scheduler(); - EXPECT_EQ(TASK_ACCEL, unittest_scheduler_selectedTaskId); + EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask); } TEST(SchedulerUnittest, TestRealTimeGuardInNoTaskRun)