src/main/scheduler/scheduler.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #define SRC_MAIN_SCHEDULER_C_
  22
  23 #include <stdbool.h>
  24 #include <stdint.h>
  25 #include <string.h>
  26 #include <limits.h>
  27 #include <math.h>
  28
  29 #include "platform.h"
  30
  31 #include "drivers/accgyro/accgyro.h"
  32
  33 #include "build/build_config.h"
  34 #include "build/debug.h"
  35
  36 #include "common/maths.h"
  37 #include "common/time.h"
  38 #include "common/utils.h"
  39
  40 #include "drivers/time.h"
  41 #include "drivers/accgyro/accgyro.h"
  42 #include "drivers/system.h"
  43
  44 #include "fc/core.h"
  45 #include "fc/tasks.h"
  46
  47 #include "rx/rx.h"
  48 #include "flight/failsafe.h"
  49
  50 #include "scheduler.h"
  51
  52 #include "sensors/gyro_init.h"
  53
  54 // DEBUG_SCHEDULER, timings for:
  55 // 0 - Average time spent executing check function
  56 // 1 - Time spent priortising
  57 // 2 - time spent in scheduler
  58
  59 // DEBUG_SCHEDULER_DETERMINISM, requires USE_LATE_TASK_STATISTICS to be defined
  60 // 0 - Gyro task start cycle time in 10th of a us
  61 // 1 - ID of late task
  62 // 2 - Amount task is late in 10th of a us
  63 // 3 - Gyro lock skew in 10th of a us
  64 // 4 - Minimum Gyro period in 100th of a us
  65 // 5 - Maximum Gyro period in 100th of a us
  66 // 6 - Span of Gyro period in 100th of a us
  67 // 7 - Standard deviation of gyro cycle time in 100th of a us
  68
  69 // DEBUG_TIMING_ACCURACY, requires USE_LATE_TASK_STATISTICS to be defined
  70 // 0 - % CPU busy
  71 // 1 - Tasks late in last second
  72 // 2 - Total lateness in last second in 10ths us
  73 // 3 - Total tasks run in last second
  74 // 4 - 10ths % of tasks late in last second
  75 // 7 - Standard deviation of gyro cycle time in 100th of a us
  76
  77 // DEBUG_TASK, requires USE_LATE_TASK_STATISTICS to be defined
  78 // 0 - Value of scheduler_debug_task setting
  79 // 1 - rate (Hz)
  80 // 2 - max (us)
  81 // 3 - average (us)
  82 // 4 - estimated execution time (us)
  83 // 5 - actual execution time (us)
  84 // 6 - difference between estimated and actual execution time
  85 // 7 - late count
  86
  87 extern task_t tasks[];
  88
  89 static FAST_DATA_ZERO_INIT task_t *currentTask = NULL;
  90 static FAST_DATA_ZERO_INIT bool ignoreCurrentTaskExecRate;
  91 static FAST_DATA_ZERO_INIT bool ignoreCurrentTaskExecTime;
  92
  93 int32_t schedLoopStartCycles;
  94 static int32_t schedLoopStartMinCycles;
  95 static int32_t schedLoopStartMaxCycles;
  96 static uint32_t schedLoopStartDeltaDownCycles;
  97 static uint32_t schedLoopStartDeltaUpCycles;
  98
  99 int32_t taskGuardCycles;
 100 static int32_t taskGuardMinCycles;
 101 static int32_t taskGuardMaxCycles;
 102 static uint32_t taskGuardDeltaDownCycles;
 103 static uint32_t taskGuardDeltaUpCycles;
 104
 105 FAST_DATA_ZERO_INIT uint16_t averageSystemLoadPercent = 0;
 106
 107 static FAST_DATA_ZERO_INIT int taskQueuePos = 0;
 108 STATIC_UNIT_TESTED FAST_DATA_ZERO_INIT int taskQueueSize = 0;
 109
 110 static FAST_DATA_ZERO_INIT bool gyroEnabled;
 111
 112 static int32_t desiredPeriodCycles;
 113 static uint32_t lastTargetCycles;
 114
 115 static uint8_t skippedRxAttempts = 0;
 116 #ifdef USE_OSD
 117 static uint8_t skippedOSDAttempts = 0;
 118 #endif
 119
 120 #if defined(USE_LATE_TASK_STATISTICS)
 121 static int16_t lateTaskCount = 0;
 122 static uint32_t lateTaskTotal = 0;
 123 static int16_t taskCount = 0;
 124 static uint32_t lateTaskPercentage = 0;
 125 static uint32_t nextTimingCycles;
 126 static int32_t gyroCyclesNow;
 127 #endif
 128
 129 static timeMs_t lastFailsafeCheckMs = 0;
 130
 131 // No need for a linked list for the queue, since items are only inserted at startup
 132 #ifdef UNIT_TEST
 133 #define TASK_QUEUE_RESERVE 1
 134 #else
 135 #define TASK_QUEUE_RESERVE 0
 136 #endif
 137 STATIC_UNIT_TESTED FAST_DATA_ZERO_INIT task_t* taskQueueArray[TASK_COUNT + 1 + TASK_QUEUE_RESERVE]; // extra item for NULL pointer at end of queue (+ overflow check in UNTT_TEST)
 138
 139 void queueClear(void)
 140 {
 141     memset(taskQueueArray, 0, sizeof(taskQueueArray));
 142     taskQueuePos = 0;
 143     taskQueueSize = 0;
 144 }
 145
 146 bool queueContains(const task_t *task)
 147 {
 148     for (int ii = 0; ii < taskQueueSize; ++ii) {
 149         if (taskQueueArray[ii] == task) {
 150             return true;
 151         }
 152     }
 153     return false;
 154 }
 155
 156 bool queueAdd(task_t *task)
 157 {
 158     if ((taskQueueSize >= TASK_COUNT) || queueContains(task)) {
 159         return false;
 160     }
 161     for (int ii = 0; ii <= taskQueueSize; ++ii) {
 162         if (taskQueueArray[ii] == NULL || taskQueueArray[ii]->attribute->staticPriority < task->attribute->staticPriority) {
 163             memmove(&taskQueueArray[ii+1], &taskQueueArray[ii], sizeof(task) * (taskQueueSize - ii));
 164             taskQueueArray[ii] = task;
 165             ++taskQueueSize;
 166             return true;
 167         }
 168     }
 169     return false;
 170 }
 171
 172 bool queueRemove(task_t *task)
 173 {
 174     for (int ii = 0; ii < taskQueueSize; ++ii) {
 175         if (taskQueueArray[ii] == task) {
 176             memmove(&taskQueueArray[ii], &taskQueueArray[ii+1], sizeof(task) * (taskQueueSize - ii));
 177             --taskQueueSize;
 178             return true;
 179         }
 180     }
 181     return false;
 182 }
 183
 184 /*
 185  * Returns first item queue or NULL if queue empty
 186  */
 187 FAST_CODE task_t *queueFirst(void)
 188 {
 189     taskQueuePos = 0;
 190     return taskQueueArray[0]; // guaranteed to be NULL if queue is empty
 191 }
 192
 193 /*
 194  * Returns next item in queue or NULL if at end of queue
 195  */
 196 FAST_CODE task_t *queueNext(void)
 197 {
 198     return taskQueueArray[++taskQueuePos]; // guaranteed to be NULL at end of queue
 199 }
 200
 201 static timeUs_t taskTotalExecutionTime = 0;
 202
 203 void taskSystemLoad(timeUs_t currentTimeUs)
 204 {
 205     static timeUs_t lastExecutedAtUs;
 206     timeDelta_t deltaTime = cmpTimeUs(currentTimeUs, lastExecutedAtUs);
 207
 208     // Calculate system load
 209     if (deltaTime) {
 210         averageSystemLoadPercent = 100 * taskTotalExecutionTime / deltaTime;
 211         taskTotalExecutionTime = 0;
 212         lastExecutedAtUs = currentTimeUs;
 213     } else {
 214         schedulerIgnoreTaskExecTime();
 215     }
 216
 217 #if defined(SIMULATOR_BUILD)
 218     averageSystemLoadPercent = 0;
 219 #endif
 220 }
 221
 222 uint32_t getCpuPercentageLate(void)
 223 {
 224 #if defined(USE_LATE_TASK_STATISTICS)
 225     return lateTaskPercentage;
 226 #else
 227     return 0;
 228 #endif
 229 }
 230
 231 timeUs_t checkFuncMaxExecutionTimeUs;
 232 timeUs_t checkFuncTotalExecutionTimeUs;
 233 timeUs_t checkFuncMovingSumExecutionTimeUs;
 234 timeUs_t checkFuncMovingSumDeltaTimeUs;
 235
 236 void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo)
 237 {
 238     checkFuncInfo->maxExecutionTimeUs = checkFuncMaxExecutionTimeUs;
 239     checkFuncInfo->totalExecutionTimeUs = checkFuncTotalExecutionTimeUs;
 240     checkFuncInfo->averageExecutionTimeUs = checkFuncMovingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
 241     checkFuncInfo->averageDeltaTimeUs = checkFuncMovingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
 242 }
 243
 244 void getTaskInfo(taskId_e taskId, taskInfo_t * taskInfo)
 245 {
 246     taskInfo->isEnabled = queueContains(getTask(taskId));
 247     taskInfo->desiredPeriodUs = getTask(taskId)->attribute->desiredPeriodUs;
 248     taskInfo->staticPriority = getTask(taskId)->attribute->staticPriority;
 249     taskInfo->taskName = getTask(taskId)->attribute->taskName;
 250     taskInfo->subTaskName = getTask(taskId)->attribute->subTaskName;
 251     taskInfo->maxExecutionTimeUs = getTask(taskId)->maxExecutionTimeUs;
 252     taskInfo->totalExecutionTimeUs = getTask(taskId)->totalExecutionTimeUs;
 253     taskInfo->averageExecutionTime10thUs = getTask(taskId)->movingSumExecutionTime10thUs / TASK_STATS_MOVING_SUM_COUNT;
 254     taskInfo->averageDeltaTime10thUs = getTask(taskId)->movingSumDeltaTime10thUs / TASK_STATS_MOVING_SUM_COUNT;
 255     taskInfo->latestDeltaTimeUs = getTask(taskId)->taskLatestDeltaTimeUs;
 256     taskInfo->movingAverageCycleTimeUs = getTask(taskId)->movingAverageCycleTimeUs;
 257 #if defined(USE_LATE_TASK_STATISTICS)
 258     taskInfo->lateCount = getTask(taskId)->lateCount;
 259     taskInfo->runCount = getTask(taskId)->runCount;
 260     taskInfo->execTime = getTask(taskId)->execTime;
 261 #endif
 262 }
 263
 264 void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs)
 265 {
 266     task_t *task;
 267
 268     if (taskId == TASK_SELF) {
 269         task = currentTask;
 270     } else if (taskId < TASK_COUNT) {
 271         task = getTask(taskId);
 272     } else {
 273         return;
 274     }
 275     task->attribute->desiredPeriodUs = MAX(SCHEDULER_DELAY_LIMIT, newPeriodUs);  // Limit delay to 100us (10 kHz) to prevent scheduler clogging
 276
 277     // Catch the case where the gyro loop is adjusted
 278     if (taskId == TASK_GYRO) {
 279         desiredPeriodCycles = (int32_t)clockMicrosToCycles((uint32_t)getTask(TASK_GYRO)->attribute->desiredPeriodUs);
 280     }
 281 }
 282
 283 void setTaskEnabled(taskId_e taskId, bool enabled)
 284 {
 285     if (taskId == TASK_SELF || taskId < TASK_COUNT) {
 286         task_t *task = taskId == TASK_SELF ? currentTask : getTask(taskId);
 287         if (enabled && task->attribute->taskFunc) {
 288             queueAdd(task);
 289         } else {
 290             queueRemove(task);
 291         }
 292     }
 293 }
 294
 295 timeDelta_t getTaskDeltaTimeUs(taskId_e taskId)
 296 {
 297     if (taskId == TASK_SELF) {
 298         return currentTask->taskLatestDeltaTimeUs;
 299     } else if (taskId < TASK_COUNT) {
 300         return getTask(taskId)->taskLatestDeltaTimeUs;
 301     } else {
 302         return 0;
 303     }
 304 }
 305
 306 // Called by tasks executing what are known to be short states
 307 void schedulerIgnoreTaskStateTime(void)
 308 {
 309     ignoreCurrentTaskExecRate = true;
 310     ignoreCurrentTaskExecTime = true;
 311 }
 312
 313 // Called by tasks with state machines to only count one state as determining rate
 314 void schedulerIgnoreTaskExecRate(void)
 315 {
 316     ignoreCurrentTaskExecRate = true;
 317 }
 318
 319 // Called by tasks without state machines executing in what is known to be a shorter time than peak
 320 void schedulerIgnoreTaskExecTime(void)
 321 {
 322     ignoreCurrentTaskExecTime = true;
 323 }
 324
 325 bool schedulerGetIgnoreTaskExecTime(void)
 326 {
 327     return ignoreCurrentTaskExecTime;
 328 }
 329
 330 void schedulerResetTaskStatistics(taskId_e taskId)
 331 {
 332     if (taskId == TASK_SELF) {
 333         currentTask->anticipatedExecutionTime = 0;
 334         currentTask->movingSumDeltaTime10thUs = 0;
 335         currentTask->totalExecutionTimeUs = 0;
 336         currentTask->maxExecutionTimeUs = 0;
 337     } else if (taskId < TASK_COUNT) {
 338         getTask(taskId)->anticipatedExecutionTime = 0;
 339         getTask(taskId)->movingSumDeltaTime10thUs = 0;
 340         getTask(taskId)->totalExecutionTimeUs = 0;
 341         getTask(taskId)->maxExecutionTimeUs = 0;
 342     }
 343 }
 344
 345 void schedulerResetTaskMaxExecutionTime(taskId_e taskId)
 346 {
 347     if (taskId == TASK_SELF) {
 348         currentTask->maxExecutionTimeUs = 0;
 349     } else if (taskId < TASK_COUNT) {
 350         task_t *task = getTask(taskId);
 351         task->maxExecutionTimeUs = 0;
 352 #if defined(USE_LATE_TASK_STATISTICS)
 353         task->lateCount = 0;
 354         task->runCount = 0;
 355 #endif
 356     }
 357 }
 358
 359 void schedulerResetCheckFunctionMaxExecutionTime(void)
 360 {
 361     checkFuncMaxExecutionTimeUs = 0;
 362 }
 363
 364 void schedulerInit(void)
 365 {
 366     queueClear();
 367     queueAdd(getTask(TASK_SYSTEM));
 368
 369     schedLoopStartMinCycles = clockMicrosToCycles(SCHED_START_LOOP_MIN_US);
 370     schedLoopStartMaxCycles = clockMicrosToCycles(SCHED_START_LOOP_MAX_US);
 371     schedLoopStartCycles = schedLoopStartMinCycles;
 372     schedLoopStartDeltaDownCycles = clockMicrosToCycles(1) / SCHED_START_LOOP_DOWN_STEP;
 373     schedLoopStartDeltaUpCycles = clockMicrosToCycles(1) / SCHED_START_LOOP_UP_STEP;
 374
 375     taskGuardMinCycles = clockMicrosToCycles(TASK_GUARD_MARGIN_MIN_US);
 376     taskGuardMaxCycles = clockMicrosToCycles(TASK_GUARD_MARGIN_MAX_US);
 377     taskGuardCycles = taskGuardMinCycles;
 378     taskGuardDeltaDownCycles = clockMicrosToCycles(1) / TASK_GUARD_MARGIN_DOWN_STEP;
 379     taskGuardDeltaUpCycles = clockMicrosToCycles(1) / TASK_GUARD_MARGIN_UP_STEP;
 380
 381     desiredPeriodCycles = (int32_t)clockMicrosToCycles((uint32_t)getTask(TASK_GYRO)->attribute->desiredPeriodUs);
 382
 383     lastTargetCycles = getCycleCounter();
 384
 385 #if defined(USE_LATE_TASK_STATISTICS)
 386     nextTimingCycles = lastTargetCycles;
 387 #endif
 388
 389     for (taskId_e taskId = 0; taskId < TASK_COUNT; taskId++) {
 390         schedulerResetTaskStatistics(taskId);
 391     }
 392 }
 393
 394 static timeDelta_t taskNextStateTime;
 395
 396 FAST_CODE void schedulerSetNextStateTime(timeDelta_t nextStateTime)
 397 {
 398     taskNextStateTime = nextStateTime;
 399 }
 400
 401 FAST_CODE timeDelta_t schedulerGetNextStateTime(void)
 402 {
 403     return currentTask->anticipatedExecutionTime >> TASK_EXEC_TIME_SHIFT;
 404 }
 405
 406 FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTimeUs)
 407 {
 408     timeUs_t taskExecutionTimeUs = 0;
 409
 410     if (selectedTask) {
 411         currentTask = selectedTask;
 412         ignoreCurrentTaskExecRate = false;
 413         ignoreCurrentTaskExecTime = false;
 414         taskNextStateTime = -1;
 415         float period = currentTimeUs - selectedTask->lastExecutedAtUs;
 416         selectedTask->lastExecutedAtUs = currentTimeUs;
 417         selectedTask->lastDesiredAt += selectedTask->attribute->desiredPeriodUs;
 418         selectedTask->dynamicPriority = 0;
 419
 420         // Execute task
 421         const timeUs_t currentTimeBeforeTaskCallUs = micros();
 422 #if defined(USE_LATE_TASK_STATISTICS)
 423         const timeUs_t estimatedExecutionUs = selectedTask->execTime;
 424 #endif
 425         selectedTask->attribute->taskFunc(currentTimeBeforeTaskCallUs);
 426         taskExecutionTimeUs = micros() - currentTimeBeforeTaskCallUs;
 427         taskTotalExecutionTime += taskExecutionTimeUs;
 428         selectedTask->movingSumExecutionTime10thUs += (taskExecutionTimeUs * 10) - selectedTask->movingSumExecutionTime10thUs / TASK_STATS_MOVING_SUM_COUNT;
 429         if (!ignoreCurrentTaskExecRate) {
 430             // Record task execution rate and max execution time
 431             selectedTask->taskLatestDeltaTimeUs = cmpTimeUs(currentTimeUs, selectedTask->lastStatsAtUs);
 432             selectedTask->movingSumDeltaTime10thUs += (selectedTask->taskLatestDeltaTimeUs * 10) - selectedTask->movingSumDeltaTime10thUs / TASK_STATS_MOVING_SUM_COUNT;
 433             selectedTask->lastStatsAtUs = currentTimeUs;
 434         }
 435
 436         // Update estimate of expected task duration
 437         if (taskNextStateTime != -1) {
 438             selectedTask->anticipatedExecutionTime = taskNextStateTime << TASK_EXEC_TIME_SHIFT;
 439         } else if (!ignoreCurrentTaskExecTime) {
 440             if (taskExecutionTimeUs > (selectedTask->anticipatedExecutionTime >> TASK_EXEC_TIME_SHIFT)) {
 441                 selectedTask->anticipatedExecutionTime = taskExecutionTimeUs << TASK_EXEC_TIME_SHIFT;
 442             } else if (selectedTask->anticipatedExecutionTime > 1) {
 443                 // Slowly decay the max time
 444                 selectedTask->anticipatedExecutionTime--;
 445             }
 446         }
 447
 448         if (!ignoreCurrentTaskExecTime) {
 449             selectedTask->maxExecutionTimeUs = MAX(selectedTask->maxExecutionTimeUs, taskExecutionTimeUs);
 450         }
 451
 452         selectedTask->totalExecutionTimeUs += taskExecutionTimeUs;   // time consumed by scheduler + task
 453         selectedTask->movingAverageCycleTimeUs += 0.05f * (period - selectedTask->movingAverageCycleTimeUs);
 454 #if defined(USE_LATE_TASK_STATISTICS)
 455         selectedTask->runCount++;
 456
 457         if ((selectedTask - tasks) == schedulerConfig()->debugTask) {
 458             timeUs_t averageDeltaTime10thUs = selectedTask->movingSumDeltaTime10thUs / TASK_STATS_MOVING_SUM_COUNT;
 459             int16_t taskFrequency = averageDeltaTime10thUs == 0 ? 0 : lrintf(1e7f / averageDeltaTime10thUs);
 460             DEBUG_SET(DEBUG_TASK, 0, schedulerConfig()->debugTask);
 461             DEBUG_SET(DEBUG_TASK, 1, taskFrequency);
 462             DEBUG_SET(DEBUG_TASK, 2, selectedTask->maxExecutionTimeUs);
 463             DEBUG_SET(DEBUG_TASK, 3, selectedTask->movingSumExecutionTime10thUs / TASK_STATS_MOVING_SUM_COUNT / 10);
 464             DEBUG_SET(DEBUG_TASK, 4, estimatedExecutionUs);
 465             DEBUG_SET(DEBUG_TASK, 5, taskExecutionTimeUs);
 466             DEBUG_SET(DEBUG_TASK, 6, estimatedExecutionUs - taskExecutionTimeUs);
 467             DEBUG_SET(DEBUG_TASK, 7, selectedTask->lateCount);
 468         }
 469 #endif
 470     }
 471
 472     return taskExecutionTimeUs;
 473 }
 474
 475 #if defined(UNIT_TEST)
 476 STATIC_UNIT_TESTED task_t *unittest_scheduler_selectedTask;
 477 STATIC_UNIT_TESTED uint8_t unittest_scheduler_selectedTaskDynamicPriority;
 478
 479 static void readSchedulerLocals(task_t *selectedTask, uint8_t selectedTaskDynamicPriority)
 480 {
 481     unittest_scheduler_selectedTask = selectedTask;
 482     unittest_scheduler_selectedTaskDynamicPriority = selectedTaskDynamicPriority;
 483 }
 484 #endif
 485
 486 FAST_CODE void scheduler(void)
 487 {
 488     static uint32_t checkCycles = 0;
 489     static uint32_t scheduleCount = 0;
 490 #if defined(USE_LATE_TASK_STATISTICS)
 491     static uint32_t gyroCyclesMean = 0;
 492     static uint32_t gyroCyclesCount = 0;
 493     static uint64_t gyroCyclesTotal = 0;
 494     static float devSquared = 0.0f;
 495 #endif
 496
 497 #if !defined(UNIT_TEST)
 498     const timeUs_t schedulerStartTimeUs = micros();
 499 #endif
 500     timeUs_t currentTimeUs;
 501     uint32_t nowCycles;
 502     timeUs_t taskExecutionTimeUs = 0;
 503     task_t *selectedTask = NULL;
 504     uint16_t selectedTaskDynamicPriority = 0;
 505     uint32_t nextTargetCycles = 0;
 506     int32_t schedLoopRemainingCycles;
 507     bool firstSchedulingOpportunity = false;
 508
 509 #if defined(UNIT_TEST)
 510     if (nextTargetCycles == 0) {
 511         lastTargetCycles = getCycleCounter();
 512         nextTargetCycles = lastTargetCycles + desiredPeriodCycles;
 513     }
 514 #endif
 515
 516     if (gyroEnabled) {
 517         // Realtime gyro/filtering/PID tasks get complete priority
 518         task_t *gyroTask = getTask(TASK_GYRO);
 519         nowCycles = getCycleCounter();
 520 #if defined(UNIT_TEST)
 521         lastTargetCycles = clockMicrosToCycles(gyroTask->lastExecutedAtUs);
 522 #endif
 523         nextTargetCycles = lastTargetCycles + desiredPeriodCycles;
 524         schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
 525
 526         if (schedLoopRemainingCycles < -desiredPeriodCycles) {
 527             /* A task has so grossly overrun that at entire gyro cycle has been skipped
 528              * This is most likely to occur when connected to the configurator via USB as the serial
 529              * task is non-deterministic
 530              * Recover as best we can, advancing scheduling by a whole number of cycles
 531              */
 532             nextTargetCycles += desiredPeriodCycles * (1 + (schedLoopRemainingCycles / -desiredPeriodCycles));
 533             schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
 534         }
 535
 536         // Tune out the time lost between completing the last task execution and re-entering the scheduler
 537         if ((schedLoopRemainingCycles < schedLoopStartMinCycles) &&
 538             (schedLoopStartCycles < schedLoopStartMaxCycles)) {
 539             schedLoopStartCycles += schedLoopStartDeltaUpCycles;
 540         }
 541
 542         // Once close to the timing boundary, poll for it's arrival
 543         if (schedLoopRemainingCycles < schedLoopStartCycles) {
 544             if (schedLoopStartCycles > schedLoopStartMinCycles) {
 545                 schedLoopStartCycles -= schedLoopStartDeltaDownCycles;
 546             }
 547 #if !defined(UNIT_TEST)
 548             while (schedLoopRemainingCycles > 0) {
 549                 nowCycles = getCycleCounter();
 550                 schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
 551             }
 552 #endif
 553             currentTimeUs = micros();
 554             taskExecutionTimeUs += schedulerExecuteTask(gyroTask, currentTimeUs);
 555
 556             if (gyroFilterReady()) {
 557                 taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_FILTER), currentTimeUs);
 558             }
 559             if (pidLoopReady()) {
 560                 taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_PID), currentTimeUs);
 561             }
 562
 563             // Check for incoming RX data. Don't do this in the checker as that is called repeatedly within
 564             // a given gyro loop, and ELRS takes a long time to process this and so can only be safely processed
 565             // before the checkers
 566             rxFrameCheck(currentTimeUs, cmpTimeUs(currentTimeUs, getTask(TASK_RX)->lastExecutedAtUs));
 567
 568             // Check for failsafe conditions without reliance on the RX task being well behaved
 569             if (cmp32(millis(), lastFailsafeCheckMs) > PERIOD_RXDATA_FAILURE) {
 570                 // This is very low cost taking less that 4us every 10ms
 571                 failsafeCheckDataFailurePeriod();
 572                 failsafeUpdateState();
 573                 lastFailsafeCheckMs = millis();
 574             }
 575
 576             // This is the first scheduling opportunity after the realtime tasks have run
 577             firstSchedulingOpportunity = true;
 578
 579 #if defined(USE_LATE_TASK_STATISTICS)
 580             gyroCyclesNow = cmpTimeCycles(nowCycles, lastTargetCycles);
 581             gyroCyclesTotal += gyroCyclesNow;
 582             gyroCyclesCount++;
 583             DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 0, clockCyclesTo10thMicros(gyroCyclesNow));
 584             int32_t deviationCycles = gyroCyclesNow - gyroCyclesMean;
 585             devSquared += deviationCycles * deviationCycles;
 586
 587             // % CPU busy
 588             DEBUG_SET(DEBUG_TIMING_ACCURACY, 0, getAverageSystemLoadPercent());
 589
 590             if (cmpTimeCycles(nextTimingCycles, nowCycles) < 0) {
 591                 nextTimingCycles += clockMicrosToCycles(1000000);
 592
 593                 // Tasks late in last second
 594                 DEBUG_SET(DEBUG_TIMING_ACCURACY, 1, lateTaskCount);
 595                 // Total lateness in last second in us
 596                 DEBUG_SET(DEBUG_TIMING_ACCURACY, 2, clockCyclesTo10thMicros(lateTaskTotal));
 597                 // Total tasks run in last second
 598                 DEBUG_SET(DEBUG_TIMING_ACCURACY, 3, taskCount);
 599
 600                 lateTaskPercentage = 1000 * (uint32_t)lateTaskCount / taskCount;
 601                 // 10ths % of tasks late in last second
 602                 DEBUG_SET(DEBUG_TIMING_ACCURACY, 4, lateTaskPercentage);
 603
 604                 float gyroCyclesStdDev = sqrt(devSquared/gyroCyclesCount);
 605                 int32_t gyroCyclesStdDev100thus = clockCyclesTo100thMicros((int32_t)gyroCyclesStdDev);
 606                 DEBUG_SET(DEBUG_TIMING_ACCURACY, 7, gyroCyclesStdDev100thus);
 607                 DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 7, gyroCyclesStdDev100thus);
 608
 609                 gyroCyclesMean = gyroCyclesTotal/gyroCyclesCount;
 610
 611                 devSquared = 0.0f;
 612                 gyroCyclesTotal = 0;
 613                 gyroCyclesCount = 0;
 614                 lateTaskCount = 0;
 615                 lateTaskTotal = 0;
 616                 taskCount = 0;
 617             }
 618 #endif
 619             lastTargetCycles = nextTargetCycles;
 620
 621             gyroDev_t *gyro = gyroActiveDev();
 622
 623             // Bring the scheduler into lock with the gyro
 624             if (gyro->gyroModeSPI != GYRO_EXTI_NO_INT) {
 625                 // Track the actual gyro rate over given number of cycle times and set the expected timebase
 626                 static uint32_t terminalGyroRateCount = 0;
 627                 static int32_t sampleRateStartCycles;
 628
 629                 if (terminalGyroRateCount == 0) {
 630                     terminalGyroRateCount = gyro->detectedEXTI + GYRO_RATE_COUNT;
 631                     sampleRateStartCycles = nowCycles;
 632                 }
 633
 634                 if (gyro->detectedEXTI >= terminalGyroRateCount) {
 635                     // Calculate the number of clock cycles on average between gyro interrupts
 636                     uint32_t sampleCycles = nowCycles - sampleRateStartCycles;
 637                     desiredPeriodCycles = sampleCycles / GYRO_RATE_COUNT;
 638                     sampleRateStartCycles = nowCycles;
 639                     terminalGyroRateCount += GYRO_RATE_COUNT;
 640                 }
 641
 642                 // Track the actual gyro rate over given number of cycle times and remove skew
 643                 static uint32_t terminalGyroLockCount = 0;
 644                 static int32_t accGyroSkew = 0;
 645
 646                 int32_t gyroSkew = cmpTimeCycles(nextTargetCycles, gyro->gyroSyncEXTI) % desiredPeriodCycles;
 647                 if (gyroSkew > (desiredPeriodCycles / 2)) {
 648                     gyroSkew -= desiredPeriodCycles;
 649                 }
 650
 651                 accGyroSkew += gyroSkew;
 652
 653 #if defined(USE_LATE_TASK_STATISTICS)
 654                 static int32_t minGyroPeriod = (int32_t)INT_MAX;
 655                 static int32_t maxGyroPeriod = (int32_t)INT_MIN;
 656                 static uint32_t lastGyroSyncEXTI;
 657
 658                 gyroCyclesNow = cmpTimeCycles(gyro->gyroSyncEXTI, lastGyroSyncEXTI);
 659
 660                 if (gyroCyclesNow) {
 661                     lastGyroSyncEXTI = gyro->gyroSyncEXTI;
 662                     // If we're syncing to a short cycle, divide by eight
 663                     if (gyro->gyroShortPeriod != 0) {
 664                         gyroCyclesNow /= 8;
 665                     }
 666                     if (gyroCyclesNow < minGyroPeriod) {
 667                         minGyroPeriod = gyroCyclesNow;
 668                     }
 669                     // Crude detection of missed cycles caused by configurator traffic
 670                     if ((gyroCyclesNow > maxGyroPeriod) && (gyroCyclesNow < (1.5 * minGyroPeriod))) {
 671                         maxGyroPeriod = gyroCyclesNow;
 672                     }
 673                 }
 674 #endif
 675
 676                 if (terminalGyroLockCount == 0) {
 677                     terminalGyroLockCount = gyro->detectedEXTI + GYRO_LOCK_COUNT;
 678                 }
 679
 680                 if (gyro->detectedEXTI >= terminalGyroLockCount) {
 681                     terminalGyroLockCount += GYRO_LOCK_COUNT;
 682
 683                     // Move the desired start time of the gyroTask
 684                     lastTargetCycles -= (accGyroSkew/GYRO_LOCK_COUNT);
 685
 686 #if defined(USE_LATE_TASK_STATISTICS)
 687                     DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 3, clockCyclesTo10thMicros(accGyroSkew/GYRO_LOCK_COUNT));
 688                     DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 4, clockCyclesTo100thMicros(minGyroPeriod));
 689                     DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 5, clockCyclesTo100thMicros(maxGyroPeriod));
 690                     DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 6, clockCyclesTo100thMicros(maxGyroPeriod - minGyroPeriod));
 691                     minGyroPeriod = INT_MAX;
 692                     maxGyroPeriod = INT_MIN;
 693 #endif
 694                     accGyroSkew = 0;
 695                 }
 696             }
 697        }
 698     }
 699
 700     nowCycles = getCycleCounter();
 701     schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
 702
 703     if (!gyroEnabled || (schedLoopRemainingCycles > (int32_t)clockMicrosToCycles(CHECK_GUARD_MARGIN_US))) {
 704         currentTimeUs = micros();
 705
 706         // Update task dynamic priorities
 707         for (task_t *task = queueFirst(); task != NULL; task = queueNext()) {
 708             if (task->attribute->staticPriority != TASK_PRIORITY_REALTIME) {
 709                 // Task has checkFunc - event driven
 710                 if (task->attribute->checkFunc) {
 711                     // Increase priority for event driven tasks
 712                     if (task->dynamicPriority > 0) {
 713                         task->taskAgePeriods = 1 + (cmpTimeUs(currentTimeUs, task->lastSignaledAtUs) / task->attribute->desiredPeriodUs);
 714                         task->dynamicPriority = 1 + task->attribute->staticPriority * task->taskAgePeriods;
 715                     } else if (task->attribute->checkFunc(currentTimeUs, cmpTimeUs(currentTimeUs, task->lastExecutedAtUs))) {
 716                         const uint32_t checkFuncExecutionTimeUs = cmpTimeUs(micros(), currentTimeUs);
 717                         checkFuncMovingSumExecutionTimeUs += checkFuncExecutionTimeUs - checkFuncMovingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
 718                         checkFuncMovingSumDeltaTimeUs += task->taskLatestDeltaTimeUs - checkFuncMovingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
 719                         checkFuncTotalExecutionTimeUs += checkFuncExecutionTimeUs;   // time consumed by scheduler + task
 720                         checkFuncMaxExecutionTimeUs = MAX(checkFuncMaxExecutionTimeUs, checkFuncExecutionTimeUs);
 721                         task->lastSignaledAtUs = currentTimeUs;
 722                         task->taskAgePeriods = 1;
 723                         task->dynamicPriority = 1 + task->attribute->staticPriority;
 724                     } else {
 725                         task->taskAgePeriods = 0;
 726                     }
 727                 } else {
 728                     // Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods)
 729                     // Task age is calculated from last execution
 730                     task->taskAgePeriods = (cmpTimeUs(currentTimeUs, task->lastExecutedAtUs) / task->attribute->desiredPeriodUs);
 731                     if (task->taskAgePeriods > 0) {
 732                         task->dynamicPriority = 1 + task->attribute->staticPriority * task->taskAgePeriods;
 733                     }
 734                 }
 735
 736                 if (task->dynamicPriority > selectedTaskDynamicPriority) {
 737                     timeDelta_t taskRequiredTimeUs = task->anticipatedExecutionTime >> TASK_EXEC_TIME_SHIFT;
 738                     int32_t taskRequiredTimeCycles = (int32_t)clockMicrosToCycles((uint32_t)taskRequiredTimeUs);
 739                     // Allow a little extra time
 740                     taskRequiredTimeCycles += checkCycles + taskGuardCycles;
 741
 742                     // If there's no time to run the task, discount it from prioritisation unless aged sufficiently
 743                     // Don't block the SERIAL task.
 744                     if ((taskRequiredTimeCycles < schedLoopRemainingCycles) ||
 745                         ((scheduleCount & SCHED_TASK_DEFER_MASK) == 0) ||
 746                         ((task - tasks) == TASK_SERIAL)) {
 747                         selectedTaskDynamicPriority = task->dynamicPriority;
 748                         selectedTask = task;
 749                     }
 750                 }
 751             }
 752
 753         }
 754
 755         // The number of cycles taken to run the checkers is quite consistent with some higher spikes, but
 756         // that doesn't defeat its use
 757         checkCycles = cmpTimeCycles(getCycleCounter(), nowCycles);
 758
 759         if (selectedTask) {
 760             // Recheck the available time as checkCycles is only approximate
 761             timeDelta_t taskRequiredTimeUs = selectedTask->anticipatedExecutionTime >> TASK_EXEC_TIME_SHIFT;
 762 #if defined(USE_LATE_TASK_STATISTICS)
 763             selectedTask->execTime = taskRequiredTimeUs;
 764 #endif
 765             int32_t taskRequiredTimeCycles = (int32_t)clockMicrosToCycles((uint32_t)taskRequiredTimeUs);
 766
 767             nowCycles = getCycleCounter();
 768             schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
 769
 770             // Allow a little extra time
 771             taskRequiredTimeCycles += taskGuardCycles;
 772
 773             if (!gyroEnabled || firstSchedulingOpportunity || (taskRequiredTimeCycles < schedLoopRemainingCycles)) {
 774                 uint32_t antipatedEndCycles = nowCycles + taskRequiredTimeCycles;
 775                 taskExecutionTimeUs += schedulerExecuteTask(selectedTask, currentTimeUs);
 776                 nowCycles = getCycleCounter();
 777                 int32_t cyclesOverdue = cmpTimeCycles(nowCycles, antipatedEndCycles);
 778
 779 #if defined(USE_LATE_TASK_STATISTICS)
 780                 if (cyclesOverdue > 0) {
 781                     if ((currentTask - tasks) != TASK_SERIAL) {
 782                         DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 1, currentTask - tasks);
 783                         DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 2, clockCyclesTo10thMicros(cyclesOverdue));
 784                         currentTask->lateCount++;
 785                         lateTaskCount++;
 786                         lateTaskTotal += cyclesOverdue;
 787                     }
 788                 }
 789 #endif  // USE_LATE_TASK_STATISTICS
 790
 791                 if ((currentTask - tasks) == TASK_RX) {
 792                     skippedRxAttempts = 0;
 793                 }
 794 #ifdef USE_OSD
 795                 else if ((currentTask - tasks) == TASK_OSD) {
 796                     skippedOSDAttempts = 0;
 797                 }
 798 #endif
 799
 800                 if ((cyclesOverdue > 0) || (-cyclesOverdue < taskGuardMinCycles)) {
 801                     if (taskGuardCycles < taskGuardMaxCycles) {
 802                         taskGuardCycles += taskGuardDeltaUpCycles;
 803                     }
 804                 } else if (taskGuardCycles > taskGuardMinCycles) {
 805                     taskGuardCycles -= taskGuardDeltaDownCycles;
 806                 }
 807 #if defined(USE_LATE_TASK_STATISTICS)
 808                 taskCount++;
 809 #endif  // USE_LATE_TASK_STATISTICS
 810             } else if ((selectedTask->taskAgePeriods > TASK_AGE_EXPEDITE_COUNT) ||
 811 #ifdef USE_OSD
 812                        (((selectedTask - tasks) == TASK_OSD) && (TASK_AGE_EXPEDITE_OSD != 0) && (++skippedOSDAttempts > TASK_AGE_EXPEDITE_OSD)) ||
 813 #endif
 814                        (((selectedTask - tasks) == TASK_RX) && (TASK_AGE_EXPEDITE_RX != 0) && (++skippedRxAttempts > TASK_AGE_EXPEDITE_RX))) {
 815                 // If a task has been unable to run, then reduce it's recorded estimated run time to ensure
 816                 // it's ultimate scheduling
 817                 selectedTask->anticipatedExecutionTime *= TASK_AGE_EXPEDITE_SCALE;
 818             }
 819         }
 820     }
 821
 822 #if defined(UNIT_TEST)
 823     readSchedulerLocals(selectedTask, selectedTaskDynamicPriority);
 824     UNUSED(taskExecutionTimeUs);
 825 #else
 826     DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - schedulerStartTimeUs - taskExecutionTimeUs); // time spent in scheduler
 827 #endif
 828
 829     scheduleCount++;
 830 }
 831
 832 void schedulerEnableGyro(void)
 833 {
 834     gyroEnabled = true;
 835 }
 836
 837 uint16_t getAverageSystemLoadPercent(void)
 838 {
 839     return averageSystemLoadPercent;
 840 }
 841
 842 float schedulerGetCycleTimeMultiplier(void)
 843 {
 844     return (float)clockMicrosToCycles(getTask(TASK_GYRO)->attribute->desiredPeriodUs) / desiredPeriodCycles;
 845 }