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     tasks[TASK_ACCEL].lastStatsAtUs = 1000;
 357     simulatedTime = 2050;
 358     // run the scheduler and check the task has executed
 359     scheduler();
 360     EXPECT_NE(unittest_scheduler_selectedTask, static_cast<task_t*>(0));
 361     EXPECT_EQ(unittest_scheduler_selectedTask, &tasks[TASK_ACCEL]);
 362     EXPECT_EQ(1050, tasks[TASK_ACCEL].taskLatestDeltaTimeUs);
 363     EXPECT_EQ(2050, tasks[TASK_ACCEL].lastExecutedAtUs);
 364     EXPECT_EQ(TEST_UPDATE_ACCEL_TIME, tasks[TASK_ACCEL].totalExecutionTimeUs);
 365     // task has run, so its dynamic priority should have been set to zero
 366     EXPECT_EQ(0, tasks[TASK_GYRO].dynamicPriority);
 367 }
 368
 369 TEST(SchedulerUnittest, TestTwoTasks)
 370 {
 371     // disable all tasks except TASK_ACCEL and TASK_ATTITUDE
 372     for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
 373         setTaskEnabled(static_cast<taskId_e>(taskId), false);
 374     }
 375     setTaskEnabled(TASK_ACCEL, true);
 376     setTaskEnabled(TASK_ATTITUDE, true);
 377
 378     // set it up so that TASK_ACCEL ran just before TASK_ATTITUDE
 379     static const uint32_t startTime = 4000;
 380     simulatedTime = startTime;
 381     tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime;
 382     tasks[TASK_ATTITUDE].lastExecutedAtUs = tasks[TASK_ACCEL].lastExecutedAtUs - TEST_UPDATE_ATTITUDE_TIME;
 383     EXPECT_EQ(0, tasks[TASK_ATTITUDE].taskAgeCycles);
 384     // run the scheduler
 385     scheduler();
 386     // no tasks should have run, since neither task's desired time has elapsed
 387     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
 388
 389     // NOTE:
 390     // TASK_ACCEL    desiredPeriodUs is 1000 microseconds
 391     // TASK_ATTITUDE desiredPeriodUs is 10000 microseconds
 392     // 500 microseconds later
 393     simulatedTime += 500;
 394     // no tasks should run, since neither task's desired time has elapsed
 395     scheduler();
 396     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
 397     EXPECT_EQ(0, unittest_scheduler_waitingTasks);
 398
 399     // 500 microseconds later, TASK_ACCEL desiredPeriodUs has elapsed
 400     simulatedTime += 500;
 401     // TASK_ACCEL should now run
 402     scheduler();
 403     EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
 404     EXPECT_EQ(1, unittest_scheduler_waitingTasks);
 405     EXPECT_EQ(5000 + TEST_UPDATE_ACCEL_TIME, simulatedTime);
 406
 407     simulatedTime += 1000 - TEST_UPDATE_ACCEL_TIME;
 408     scheduler();
 409     // TASK_ACCEL should run again
 410     EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
 411
 412     scheduler();
 413     // No task should have run
 414     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
 415     EXPECT_EQ(0, unittest_scheduler_waitingTasks);
 416
 417     simulatedTime = startTime + 10500; // TASK_ACCEL and TASK_ATTITUDE desiredPeriodUss have elapsed
 418     // of the two TASK_ACCEL should run first
 419     scheduler();
 420     EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
 421     // and finally TASK_ATTITUDE should now run
 422     scheduler();
 423     EXPECT_EQ(&tasks[TASK_ATTITUDE], unittest_scheduler_selectedTask);
 424 }
 425
 426 TEST(SchedulerUnittest, TestGyroTask)
 427 {
 428     static const uint32_t startTime = 4000;
 429
 430     // enable the gyro
 431     schedulerEnableGyro();
 432
 433     // disable all tasks except TASK_GYRO, TASK_FILTER and TASK_PID
 434     for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
 435         setTaskEnabled(static_cast<taskId_e>(taskId), false);
 436     }
 437     setTaskEnabled(TASK_GYRO, true);
 438     setTaskEnabled(TASK_FILTER, true);
 439     setTaskEnabled(TASK_PID, true);
 440
 441     // First set it up so TASK_GYRO just ran
 442     simulatedTime = startTime;
 443     tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime;
 444     // reset the flags
 445     resetGyroTaskTestFlags();
 446
 447     // run the scheduler
 448     scheduler();
 449     // no tasks should have run
 450     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
 451     // also the gyro, filter and PID task indicators should be false
 452     EXPECT_FALSE(taskGyroRan);
 453     EXPECT_FALSE(taskFilterRan);
 454     EXPECT_FALSE(taskPidRan);
 455
 456     /* Test the gyro task running but not triggering the filtering or PID */
 457     // set the TASK_GYRO last executed time to be one period earlier
 458     simulatedTime = startTime;
 459     tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
 460
 461     // reset the flags
 462     resetGyroTaskTestFlags();
 463
 464     // run the scheduler
 465     scheduler();
 466     // the gyro task indicator should be true and the TASK_FILTER and TASK_PID indicators should be false
 467     EXPECT_TRUE(taskGyroRan);
 468     EXPECT_FALSE(taskFilterRan);
 469     EXPECT_FALSE(taskPidRan);
 470     // expect that no other tasks other than TASK_GYRO should have run
 471     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
 472
 473     /* Test the gyro task running and triggering the filtering task */
 474     // set the TASK_GYRO last executed time to be one period earlier
 475     simulatedTime = startTime;
 476     tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
 477
 478     // reset the flags
 479     resetGyroTaskTestFlags();
 480     taskFilterReady = true;
 481
 482     // run the scheduler
 483     scheduler();
 484     // the gyro and filter task indicators should be true and TASK_PID indicator should be false
 485     EXPECT_TRUE(taskGyroRan);
 486     EXPECT_TRUE(taskFilterRan);
 487     EXPECT_FALSE(taskPidRan);
 488     // expect that no other tasks other tasks should have run
 489     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
 490
 491     /* Test the gyro task running and triggering the PID task */
 492     // set the TASK_GYRO last executed time to be one period earlier
 493     simulatedTime = startTime;
 494     tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
 495
 496     // reset the flags
 497     resetGyroTaskTestFlags();
 498     taskPidReady = true;
 499
 500     // run the scheduler
 501     scheduler();
 502     // the gyro and PID task indicators should be true and TASK_FILTER indicator should be false
 503     EXPECT_TRUE(taskGyroRan);
 504     EXPECT_FALSE(taskFilterRan);
 505     EXPECT_TRUE(taskPidRan);
 506     // expect that no other tasks other tasks should have run
 507     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
 508 }
 509
 510 // Test the scheduling logic that prevents other tasks from running if they
 511 // might interfere with the timing of the next gyro task.
 512 TEST(SchedulerUnittest, TestGyroLookahead)
 513 {
 514     static const uint32_t startTime = 4000;
 515
 516     // enable task statistics
 517     schedulerSetCalulateTaskStatistics(true);
 518
 519     // disable scheduler optimize rate
 520     schedulerOptimizeRate(false);
 521
 522     // enable the gyro
 523     schedulerEnableGyro();
 524
 525     // disable all tasks except TASK_GYRO, TASK_ACCEL
 526     for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
 527         setTaskEnabled(static_cast<taskId_e>(taskId), false);
 528     }
 529     setTaskEnabled(TASK_GYRO, true);
 530     setTaskEnabled(TASK_ACCEL, true);
 531
 532 #if defined(USE_TASK_STATISTICS)
 533     // set the average run time for TASK_ACCEL
 534     tasks[TASK_ACCEL].movingSumExecutionTimeUs = TEST_UPDATE_ACCEL_TIME * TASK_STATS_MOVING_SUM_COUNT;
 535 #endif
 536
 537     /* Test that another task will run if there's plenty of time till the next gyro sample time */
 538     // set it up so TASK_GYRO just ran and TASK_ACCEL is ready to run
 539     simulatedTime = startTime;
 540     tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime;
 541     tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(1000);
 542     // reset the flags
 543     resetGyroTaskTestFlags();
 544
 545     // run the scheduler
 546     scheduler();
 547     // the gyro, filter and PID task indicators should be false
 548     EXPECT_FALSE(taskGyroRan);
 549     EXPECT_FALSE(taskFilterRan);
 550     EXPECT_FALSE(taskPidRan);
 551     // TASK_ACCEL should have run
 552     EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
 553
 554     /* Test that another task won't run if the time till the gyro task is less than the guard interval */
 555     // set it up so TASK_GYRO will run soon and TASK_ACCEL is ready to run
 556     simulatedTime = startTime;
 557     tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + GYRO_TASK_GUARD_INTERVAL_US / 2;
 558     tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(1000);
 559     // reset the flags
 560     resetGyroTaskTestFlags();
 561
 562     // run the scheduler
 563     scheduler();
 564     // the gyro, filter and PID task indicators should be false
 565     EXPECT_FALSE(taskGyroRan);
 566     EXPECT_FALSE(taskFilterRan);
 567     EXPECT_FALSE(taskPidRan);
 568     // TASK_ACCEL should not have run
 569     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
 570
 571     /* Test that another task won't run if the time till the gyro task is less than the average task interval */
 572     // set it up so TASK_GYRO will run soon and TASK_ACCEL is ready to run
 573     simulatedTime = startTime;
 574     tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + TEST_UPDATE_ACCEL_TIME / 2;
 575     tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(1000);
 576     // reset the flags
 577     resetGyroTaskTestFlags();
 578
 579     // run the scheduler
 580     scheduler();
 581     // the gyro, filter and PID task indicators should be false
 582     EXPECT_FALSE(taskGyroRan);
 583     EXPECT_FALSE(taskFilterRan);
 584     EXPECT_FALSE(taskPidRan);
 585     // TASK_ACCEL should not have run
 586     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
 587
 588     /* Test that another task will run if the gyro task gets executed */
 589     // set it up so TASK_GYRO will run now and TASK_ACCEL is ready to run
 590     simulatedTime = startTime;
 591     tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
 592     tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(1000);
 593     // reset the flags
 594     resetGyroTaskTestFlags();
 595
 596     // make the TASK_FILTER and TASK_PID ready to run
 597     taskFilterReady = true;
 598     taskPidReady = true;
 599
 600     // run the scheduler
 601     scheduler();
 602     // TASK_GYRO, TASK_FILTER, and TASK_PID should all run
 603     EXPECT_TRUE(taskGyroRan);
 604     EXPECT_TRUE(taskFilterRan);
 605     EXPECT_TRUE(taskPidRan);
 606     // TASK_ACCEL should have run
 607     EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
 608 }