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