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