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