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