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