src/test/unit/scheduler_unittest.cc

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdint.h>
  19
  20 extern "C" {
  21     #include "drivers/accgyro/accgyro.h"
  22     #include "platform.h"
  23     #include "scheduler/scheduler.h"
  24     #include "scheduler_stubs.h"
  25 }
  26
  27 #include "unittest_macros.h"
  28 #include "gtest/gtest.h"
  29
  30 const int TEST_GYRO_SAMPLE_HZ = 8000;
  31 const int TEST_GYRO_SAMPLE_TIME = 10;
  32 const int TEST_FILTERING_TIME = 40;
  33 const int TEST_PID_LOOP_TIME = 58;
  34 const int TEST_UPDATE_ACCEL_TIME = 32;
  35 const int TEST_UPDATE_ATTITUDE_TIME = 28;
  36 const int TEST_HANDLE_SERIAL_TIME = 30;
  37 const int TEST_UPDATE_BATTERY_TIME = 1;
  38 const int TEST_UPDATE_RX_CHECK_TIME = 34;
  39 const int TEST_UPDATE_RX_MAIN_TIME = 1;
  40 const int TEST_IMU_UPDATE_TIME = 5;
  41 const int TEST_DISPATCH_TIME = 200;
  42 const int TEST_UPDATE_OSD_CHECK_TIME = 5;
  43 const int TEST_UPDATE_OSD_TIME = 30;
  44
  45 #define TASK_COUNT_UNITTEST (TASK_BATTERY_VOLTAGE + 1)
  46 #define TASK_PERIOD_HZ(hz) (1000000 / (hz))
  47
  48 extern "C" {
  49     extern task_t * unittest_scheduler_selectedTask;
  50     extern uint8_t unittest_scheduler_selectedTaskDynPrio;
  51     timeDelta_t unittest_scheduler_taskRequiredTimeUs;
  52     bool taskGyroRan = false;
  53     bool taskFilterRan = false;
  54     bool taskPidRan = false;
  55     bool taskFilterReady = false;
  56     bool taskPidReady = false;
  57     uint8_t activePidLoopDenom = 1;
  58
  59     int16_t debug[1];
  60     uint8_t debugMode = 0;
  61
  62     void rxFrameCheck(timeUs_t, timeDelta_t) {}
  63
  64     // set up micros() to simulate time
  65     uint32_t simulatedTime = 0;
  66     uint32_t micros(void) { return simulatedTime; }
  67     uint32_t millis(void) { return simulatedTime/1000; } // Note simplistic mapping suitable only for short unit tests
  68     int32_t clockCyclesToMicros(int32_t x) { return x/10;}
  69     int32_t clockCyclesTo10thMicros(int32_t x) { return x;}
  70     int32_t clockCyclesTo100thMicros(int32_t x) { return x * 10;}
  71     uint32_t clockMicrosToCycles(uint32_t x) { return x*10;}
  72     uint32_t getCycleCounter(void) {return simulatedTime * 10;}
  73
  74     // set up tasks to take a simulated representative time to execute
  75     bool gyroFilterReady(void) { return taskFilterReady; }
  76     gyroDev_t gyro;
  77     gyroDev_t *gyroActiveDev(void) { return &gyro; }
  78     bool pidLoopReady(void) { return taskPidReady; }
  79     void failsafeCheckDataFailurePeriod(void) {}
  80     void failsafeUpdateState(void) {}
  81     void taskGyroSample(timeUs_t) { simulatedTime += TEST_GYRO_SAMPLE_TIME; taskGyroRan = true; }
  82     void taskFiltering(timeUs_t) { simulatedTime += TEST_FILTERING_TIME; taskFilterRan = true; }
  83     void taskMainPidLoop(timeUs_t) { simulatedTime += TEST_PID_LOOP_TIME; taskPidRan = true; }
  84     void taskUpdateAccelerometer(timeUs_t) { simulatedTime += TEST_UPDATE_ACCEL_TIME; }
  85     void taskHandleSerial(timeUs_t) { simulatedTime += TEST_HANDLE_SERIAL_TIME; }
  86     void taskUpdateBatteryVoltage(timeUs_t) { simulatedTime += TEST_UPDATE_BATTERY_TIME; }
  87     bool rxUpdateCheck(timeUs_t, timeDelta_t) { simulatedTime += TEST_UPDATE_RX_CHECK_TIME; return false; }
  88     void taskUpdateRxMain(timeUs_t) { simulatedTime += TEST_UPDATE_RX_MAIN_TIME; }
  89     void imuUpdateAttitude(timeUs_t) { simulatedTime += TEST_IMU_UPDATE_TIME; }
  90     void dispatchProcess(timeUs_t) { simulatedTime += TEST_DISPATCH_TIME; }
  91     bool osdUpdateCheck(timeUs_t, timeDelta_t) { simulatedTime += TEST_UPDATE_OSD_CHECK_TIME; return false; }
  92     void osdUpdate(timeUs_t) { simulatedTime += TEST_UPDATE_OSD_TIME; }
  93
  94     void resetGyroTaskTestFlags(void) {
  95         taskGyroRan = false;
  96         taskFilterRan = false;
  97         taskPidRan = false;
  98         taskFilterReady = false;
  99         taskPidReady = false;
 100     }
 101
 102     extern int taskQueueSize;
 103     extern task_t* taskQueueArray[];
 104
 105     extern void queueClear(void);
 106     extern bool queueContains(const task_t *task);
 107     extern bool queueAdd(task_t *task);
 108     extern bool queueRemove(task_t *task);
 109     extern task_t *queueFirst(void);
 110     extern task_t *queueNext(void);
 111
 112     task_t tasks[TASK_COUNT];
 113
 114     task_t *getTask(unsigned taskId)
 115     {
 116         return &tasks[taskId];
 117     }
 118 }
 119
 120 TEST(SchedulerUnittest, SetupTasks)
 121 {
 122     for (int i = 0; i < TASK_COUNT; ++i) {
 123         tasks[i].attribute = &task_attributes[i];
 124     }
 125 }
 126
 127
 128 TEST(SchedulerUnittest, TestPriorites)
 129 {
 130     EXPECT_EQ(TASK_PRIORITY_MEDIUM_HIGH, tasks[TASK_SYSTEM].attribute->staticPriority);
 131     EXPECT_EQ(TASK_PRIORITY_REALTIME, tasks[TASK_GYRO].attribute->staticPriority);
 132     EXPECT_EQ(TASK_PRIORITY_MEDIUM, tasks[TASK_ACCEL].attribute->staticPriority);
 133     EXPECT_EQ(TASK_PRIORITY_LOW, tasks[TASK_SERIAL].attribute->staticPriority);
 134     EXPECT_EQ(TASK_PRIORITY_MEDIUM, tasks[TASK_BATTERY_VOLTAGE].attribute->staticPriority);
 135 }
 136
 137 TEST(SchedulerUnittest, TestQueueInit)
 138 {
 139     queueClear();
 140     EXPECT_EQ(0, taskQueueSize);
 141     EXPECT_EQ(0, queueFirst());
 142     EXPECT_EQ(0, queueNext());
 143     for (int ii = 0; ii <= TASK_COUNT; ++ii) {
 144         EXPECT_EQ(0, taskQueueArray[ii]);
 145     }
 146 }
 147
 148 task_t *deadBeefPtr = reinterpret_cast<task_t*>(0xDEADBEEF);
 149
 150 TEST(SchedulerUnittest, TestQueue)
 151 {
 152     queueClear();
 153     taskQueueArray[TASK_COUNT + 1] = deadBeefPtr;
 154
 155     queueAdd(&tasks[TASK_SYSTEM]); // TASK_PRIORITY_MEDIUM_HIGH
 156     EXPECT_EQ(1, taskQueueSize);
 157     EXPECT_EQ(&tasks[TASK_SYSTEM], queueFirst());
 158     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
 159
 160     queueAdd(&tasks[TASK_SERIAL]); // TASK_PRIORITY_LOW
 161     EXPECT_EQ(2, taskQueueSize);
 162     EXPECT_EQ(&tasks[TASK_SYSTEM], queueFirst());
 163     EXPECT_EQ(&tasks[TASK_SERIAL], queueNext());
 164     EXPECT_EQ(NULL, queueNext());
 165     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
 166
 167     queueAdd(&tasks[TASK_BATTERY_VOLTAGE]); // TASK_PRIORITY_MEDIUM
 168     EXPECT_EQ(3, taskQueueSize);
 169     EXPECT_EQ(&tasks[TASK_SYSTEM], queueFirst());
 170     EXPECT_EQ(&tasks[TASK_BATTERY_VOLTAGE], queueNext());
 171     EXPECT_EQ(&tasks[TASK_SERIAL], queueNext());
 172     EXPECT_EQ(NULL, queueNext());
 173     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
 174
 175     queueAdd(&tasks[TASK_RX]); // TASK_PRIORITY_HIGH
 176     EXPECT_EQ(4, taskQueueSize);
 177     EXPECT_EQ(&tasks[TASK_RX], queueFirst());
 178     EXPECT_EQ(&tasks[TASK_SYSTEM], queueNext());
 179     EXPECT_EQ(&tasks[TASK_BATTERY_VOLTAGE], queueNext());
 180     EXPECT_EQ(&tasks[TASK_SERIAL], queueNext());
 181     EXPECT_EQ(NULL, queueNext());
 182     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
 183
 184     queueRemove(&tasks[TASK_SYSTEM]); // TASK_PRIORITY_HIGH
 185     EXPECT_EQ(3, taskQueueSize);
 186     EXPECT_EQ(&tasks[TASK_RX], queueFirst());
 187     EXPECT_EQ(&tasks[TASK_BATTERY_VOLTAGE], queueNext());
 188     EXPECT_EQ(&tasks[TASK_SERIAL], queueNext());
 189     EXPECT_EQ(NULL, queueNext());
 190     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
 191 }
 192
 193 TEST(SchedulerUnittest, TestQueueAddAndRemove)
 194 {
 195     queueClear();
 196     taskQueueArray[TASK_COUNT + 1] = deadBeefPtr;
 197
 198     // fill up the queue
 199     for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
 200         const bool added = queueAdd(&tasks[taskId]);
 201         EXPECT_TRUE(added);
 202         EXPECT_EQ(taskId + 1, taskQueueSize);
 203         EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
 204     }
 205     // double check end of queue
 206     EXPECT_EQ(TASK_COUNT, taskQueueSize);
 207     EXPECT_NE(static_cast<task_t*>(0), taskQueueArray[TASK_COUNT - 1]); // last item was indeed added to queue
 208     EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); // null pointer at end of queue is preserved
 209     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); // there hasn't been an out by one error
 210
 211     // and empty it again
 212     for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
 213         const bool removed = queueRemove(&tasks[taskId]);
 214         EXPECT_TRUE(removed);
 215         EXPECT_EQ(TASK_COUNT - taskId - 1, taskQueueSize);
 216         EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - taskId]);
 217         EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
 218     }
 219
 220     // double check size and end of queue
 221     EXPECT_EQ(0, taskQueueSize); // queue is indeed empty
 222     EXPECT_EQ(NULL, taskQueueArray[0]); // there is a null pointer at the end of the queueu
 223     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); // no accidental overwrites past end of queue
 224 }
 225
 226 TEST(SchedulerUnittest, TestQueueArray)
 227 {
 228     // test there are no "out by one" errors or buffer overruns when items are added and removed
 229     queueClear();
 230     taskQueueArray[TASK_COUNT_UNITTEST + 1] = deadBeefPtr; // note, must set deadBeefPtr after queueClear
 231
 232     unsigned enqueuedTasks = 0;
 233     EXPECT_EQ(enqueuedTasks, taskQueueSize);
 234
 235     for (int taskId = 0; taskId < TASK_COUNT_UNITTEST - 1; ++taskId) {
 236         if (tasks[taskId].attribute->taskFunc) {
 237             setTaskEnabled(static_cast<taskId_e>(taskId), true);
 238             enqueuedTasks++;
 239             EXPECT_EQ(enqueuedTasks, taskQueueSize);
 240             EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
 241         }
 242     }
 243
 244     EXPECT_NE(static_cast<task_t*>(0), taskQueueArray[enqueuedTasks - 1]);
 245     const task_t *lastTaskPrev = taskQueueArray[enqueuedTasks - 1];
 246     EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]);
 247     EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]);
 248     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
 249
 250     setTaskEnabled(TASK_SYSTEM, false);
 251     EXPECT_EQ(enqueuedTasks - 1, taskQueueSize);
 252     EXPECT_EQ(lastTaskPrev, taskQueueArray[enqueuedTasks - 2]);
 253     EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks - 1]); // NULL at end of queue
 254     EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]);
 255     EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]);
 256     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
 257
 258     taskQueueArray[enqueuedTasks - 1] = 0;
 259     setTaskEnabled(TASK_SYSTEM, true);
 260     EXPECT_EQ(enqueuedTasks, taskQueueSize);
 261     EXPECT_EQ(lastTaskPrev, taskQueueArray[enqueuedTasks - 1]);
 262     EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]);
 263     EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]);
 264     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
 265
 266     taskInfo_t taskInfo;
 267     getTaskInfo(static_cast<taskId_e>(enqueuedTasks + 1), &taskInfo);
 268     EXPECT_FALSE(taskInfo.isEnabled);
 269     setTaskEnabled(static_cast<taskId_e>(enqueuedTasks), true);
 270     EXPECT_EQ(enqueuedTasks, taskQueueSize);
 271     EXPECT_EQ(lastTaskPrev, taskQueueArray[enqueuedTasks - 1]);
 272     EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]); // check no buffer overrun
 273     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
 274
 275     setTaskEnabled(TASK_SYSTEM, false);
 276     EXPECT_EQ(enqueuedTasks - 1, taskQueueSize);
 277     EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]);
 278     EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]);
 279     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
 280
 281     setTaskEnabled(TASK_ACCEL, false);
 282     EXPECT_EQ(enqueuedTasks - 2, taskQueueSize);
 283     EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks - 1]);
 284     EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]);
 285     EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]);
 286     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
 287
 288     setTaskEnabled(TASK_BATTERY_VOLTAGE, false);
 289     EXPECT_EQ(enqueuedTasks - 2, taskQueueSize);
 290     EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks - 2]);
 291     EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks - 1]);
 292     EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]);
 293     EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]);
 294     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
 295 }
 296
 297 TEST(SchedulerUnittest, TestSchedulerInit)
 298 {
 299     schedulerInit();
 300     EXPECT_EQ(1, taskQueueSize);
 301     EXPECT_EQ(&tasks[TASK_SYSTEM], queueFirst());
 302 }
 303
 304 TEST(SchedulerUnittest, TestScheduleEmptyQueue)
 305 {
 306     queueClear();
 307     simulatedTime = 4000;
 308     // run the with an empty queue
 309     scheduler();
 310     EXPECT_EQ(NULL, unittest_scheduler_selectedTask);
 311 }
 312
 313 TEST(SchedulerUnittest, TestSingleTask)
 314 {
 315     schedulerInit();
 316     // disable all tasks except TASK_ACCEL
 317     for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
 318         setTaskEnabled(static_cast<taskId_e>(taskId), false);
 319     }
 320     setTaskEnabled(TASK_ACCEL, true);
 321     tasks[TASK_ACCEL].lastExecutedAtUs = 1000;
 322     tasks[TASK_ACCEL].lastStatsAtUs = 1000;
 323     simulatedTime = 2050;
 324     // run the scheduler and check the task has executed
 325     scheduler();
 326     EXPECT_NE(unittest_scheduler_selectedTask, static_cast<task_t*>(0));
 327     EXPECT_EQ(unittest_scheduler_selectedTask, &tasks[TASK_ACCEL]);
 328     EXPECT_EQ(1050, tasks[TASK_ACCEL].taskLatestDeltaTimeUs);
 329     EXPECT_EQ(2050, tasks[TASK_ACCEL].lastExecutedAtUs);
 330     EXPECT_EQ(TEST_UPDATE_ACCEL_TIME, tasks[TASK_ACCEL].totalExecutionTimeUs);
 331     // task has run, so its dynamic priority should have been set to zero
 332     EXPECT_EQ(0, tasks[TASK_GYRO].dynamicPriority);
 333 }
 334
 335 TEST(SchedulerUnittest, TestTwoTasks)
 336 {
 337     // disable all tasks except TASK_ACCEL and TASK_ATTITUDE
 338     for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
 339         setTaskEnabled(static_cast<taskId_e>(taskId), false);
 340     }
 341     setTaskEnabled(TASK_ACCEL, true);
 342     setTaskEnabled(TASK_ATTITUDE, true);
 343
 344     // set it up so that TASK_ACCEL ran just before TASK_ATTITUDE
 345     static const uint32_t startTime = 4000;
 346     simulatedTime = startTime;
 347     tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime;
 348     tasks[TASK_ATTITUDE].lastExecutedAtUs = tasks[TASK_ACCEL].lastExecutedAtUs - TEST_UPDATE_ATTITUDE_TIME;
 349     EXPECT_EQ(0, tasks[TASK_ATTITUDE].taskAgePeriods);
 350     // run the scheduler
 351     scheduler();
 352     // no tasks should have run, since neither task's desired time has elapsed
 353     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
 354
 355     // NOTE:
 356     // TASK_ACCEL    desiredPeriodUs is 1000 microseconds
 357     // TASK_ATTITUDE desiredPeriodUs is 10000 microseconds
 358     // 500 microseconds later
 359     simulatedTime += 500;
 360     // no tasks should run, since neither task's desired time has elapsed
 361     scheduler();
 362     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
 363
 364     // 500 microseconds later, TASK_ACCEL desiredPeriodUs has elapsed
 365     simulatedTime += 500;
 366     // TASK_ACCEL should now run
 367     scheduler();
 368     EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
 369     EXPECT_EQ(5000 + TEST_UPDATE_ACCEL_TIME, simulatedTime);
 370
 371     simulatedTime += 1000 - TEST_UPDATE_ACCEL_TIME;
 372     scheduler();
 373     // TASK_ACCEL should run again
 374     EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
 375
 376     scheduler();
 377     // No task should have run
 378     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
 379
 380     simulatedTime = startTime + 10500; // TASK_ACCEL and TASK_ATTITUDE desiredPeriodUss have elapsed
 381     // of the two TASK_ACCEL should run first
 382     scheduler();
 383     EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
 384     // and finally TASK_ATTITUDE should now run
 385     scheduler();
 386     EXPECT_EQ(&tasks[TASK_ATTITUDE], unittest_scheduler_selectedTask);
 387 }
 388
 389 TEST(SchedulerUnittest, TestPriorityBump)
 390 {
 391     // disable all tasks except TASK_ACCEL and TASK_ATTITUDE
 392     for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
 393         setTaskEnabled(static_cast<taskId_e>(taskId), false);
 394     }
 395     setTaskEnabled(TASK_ACCEL, true);
 396     setTaskEnabled(TASK_DISPATCH, true);
 397
 398     // Both tasks have an update rate of 1kHz, but TASK_DISPATCH has TASK_PRIORITY_HIGH whereas TASK_ACCEL has TASK_PRIORITY_MEDIUM
 399     static const uint32_t startTime = 4000;
 400     simulatedTime = startTime;
 401     tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime;
 402     tasks[TASK_DISPATCH].lastExecutedAtUs = tasks[TASK_ACCEL].lastExecutedAtUs;
 403     EXPECT_EQ(0, tasks[TASK_DISPATCH].taskAgePeriods);
 404
 405     // Set expectation for execution time of TEST_DISPATCH_TIME us
 406     tasks[TASK_DISPATCH].anticipatedExecutionTime = TEST_DISPATCH_TIME << TASK_EXEC_TIME_SHIFT;
 407
 408     // run the scheduler
 409     scheduler();
 410     // no tasks should have run, since neither task's desired time has elapsed
 411     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
 412
 413     // NOTE:
 414     // TASK_ACCEL    desiredPeriodUs is 1000 microseconds
 415     // TASK_DISPATCH desiredPeriodUs is 1000 microseconds
 416     // 500 microseconds later
 417     simulatedTime += 500;
 418     // no tasks should run, since neither task's desired time has elapsed
 419     scheduler();
 420     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
 421
 422     // 500 microseconds later, 1000 desiredPeriodUs has elapsed
 423     simulatedTime += 500;
 424     // TASK_ACCEL should now run as there is not enough time to run the higher priority TASK_DISPATCH
 425     scheduler();
 426     EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
 427     EXPECT_EQ(5000 + TEST_UPDATE_ACCEL_TIME, simulatedTime);
 428
 429     simulatedTime += 1000 - TEST_UPDATE_ACCEL_TIME;
 430     // TASK_ACCEL should now run as there is not enough time to run the higher priority TASK_DISPATCH
 431     scheduler();
 432     EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
 433     EXPECT_EQ(6000 + TEST_UPDATE_ACCEL_TIME, simulatedTime);
 434
 435     simulatedTime += 1000 - TEST_UPDATE_ACCEL_TIME;
 436     // TASK_ACCEL should now run as there is not enough time to run the higher priority TASK_DISPATCH
 437     scheduler();
 438     EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
 439     EXPECT_EQ(7000 + TEST_UPDATE_ACCEL_TIME, simulatedTime);
 440
 441     simulatedTime += 1000 - TEST_UPDATE_ACCEL_TIME;
 442     // TASK_ACCEL should now run as there is not enough time to run the higher priority TASK_DISPATCH
 443     scheduler();
 444     EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
 445     EXPECT_EQ(8000 + TEST_UPDATE_ACCEL_TIME, simulatedTime);
 446
 447     simulatedTime += 1000 - TEST_UPDATE_ACCEL_TIME;
 448     // TASK_ACCEL should now run as there is not enough time to run the higher priority TASK_DISPATCH
 449     scheduler();
 450     EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
 451     EXPECT_EQ(9000 + TEST_UPDATE_ACCEL_TIME, simulatedTime);
 452
 453     // TASK_DISPATCH has aged whilst not being run
 454     EXPECT_EQ(5, tasks[TASK_DISPATCH].taskAgePeriods);
 455     simulatedTime += 1000 - TEST_UPDATE_ACCEL_TIME;
 456     // TASK_TASK_DISPATCH should now run as the scheduler is on its eighth loop. Note that this is affected by prior test count.
 457     scheduler();
 458     EXPECT_EQ(&tasks[TASK_DISPATCH], unittest_scheduler_selectedTask);
 459     EXPECT_EQ(10000 + TEST_DISPATCH_TIME, simulatedTime);
 460     // TASK_DISPATCH still hasn't been executed
 461     EXPECT_EQ(6, tasks[TASK_DISPATCH].taskAgePeriods);
 462
 463     simulatedTime += 1000 - TEST_DISPATCH_TIME;
 464     // TASK_ACCEL should now run again as there is not enough time to run the higher priority TASK_DISPATCH
 465     scheduler();
 466     EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
 467     EXPECT_EQ(11000 + TEST_UPDATE_ACCEL_TIME, simulatedTime);
 468 }
 469
 470 TEST(SchedulerUnittest, TestGyroTask)
 471 {
 472     static const uint32_t startTime = 4000;
 473
 474     // enable the gyro
 475     schedulerEnableGyro();
 476
 477     // disable all tasks except TASK_GYRO, TASK_FILTER and TASK_PID
 478     for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
 479         setTaskEnabled(static_cast<taskId_e>(taskId), false);
 480     }
 481     setTaskEnabled(TASK_GYRO, true);
 482     setTaskEnabled(TASK_FILTER, true);
 483     setTaskEnabled(TASK_PID, true);
 484
 485     // First set it up so TASK_GYRO just ran
 486     simulatedTime = startTime;
 487     tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime;
 488     // reset the flags
 489     resetGyroTaskTestFlags();
 490
 491     // run the scheduler
 492     scheduler();
 493     // no tasks should have run
 494     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
 495     // also the gyro, filter and PID task indicators should be false
 496     EXPECT_FALSE(taskGyroRan);
 497     EXPECT_FALSE(taskFilterRan);
 498     EXPECT_FALSE(taskPidRan);
 499
 500     /* Test the gyro task running but not triggering the filtering or PID */
 501     // set the TASK_GYRO last executed time to be one period earlier
 502     simulatedTime = startTime;
 503     tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
 504
 505     // reset the flags
 506     resetGyroTaskTestFlags();
 507
 508     // run the scheduler
 509     scheduler();
 510
 511     // the gyro task indicator should be true and the TASK_FILTER and TASK_PID indicators should be false
 512     EXPECT_TRUE(taskGyroRan);
 513     EXPECT_FALSE(taskFilterRan);
 514     EXPECT_FALSE(taskPidRan);
 515     // expect that no other tasks other than TASK_GYRO should have run
 516     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
 517
 518     /* Test the gyro task running and triggering the filtering task */
 519     // set the TASK_GYRO last executed time to be one period earlier
 520     simulatedTime = startTime;
 521     tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
 522
 523     // reset the flags
 524     resetGyroTaskTestFlags();
 525     taskFilterReady = true;
 526
 527     // run the scheduler
 528     scheduler();
 529     // the gyro and filter task indicators should be true and TASK_PID indicator should be false
 530     EXPECT_TRUE(taskGyroRan);
 531     EXPECT_TRUE(taskFilterRan);
 532     EXPECT_FALSE(taskPidRan);
 533     // expect that no other tasks other tasks should have run
 534     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
 535
 536     /* Test the gyro task running and triggering the PID task */
 537     // set the TASK_GYRO last executed time to be one period earlier
 538     simulatedTime = startTime;
 539     tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
 540
 541     // reset the flags
 542     resetGyroTaskTestFlags();
 543     taskPidReady = true;
 544
 545     // run the scheduler
 546     scheduler();
 547     // the gyro and PID task indicators should be true and TASK_FILTER indicator should be false
 548     EXPECT_TRUE(taskGyroRan);
 549     EXPECT_FALSE(taskFilterRan);
 550     EXPECT_TRUE(taskPidRan);
 551     // expect that no other tasks other tasks should have run
 552     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
 553 }
 554