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 "build/build_config.h"
  30 #include "build/debug.h"
  31
  32 #include "scheduler/scheduler.h"
  33
  34 #include "config/config_unittest.h"
  35
  36 #include "common/maths.h"
  37 #include "common/time.h"
  38 #include "common/utils.h"
  39
  40 #include "drivers/time.h"
  41
  42 // DEBUG_SCHEDULER, timings for:
  43 // 0 - gyroUpdate()
  44 // 1 - pidController()
  45 // 2 - time spent in scheduler
  46 // 3 - time spent executing check function
  47
  48 static FAST_RAM_ZERO_INIT cfTask_t *currentTask = NULL;
  49
  50 static FAST_RAM_ZERO_INIT uint32_t totalWaitingTasks;
  51 static FAST_RAM_ZERO_INIT uint32_t totalWaitingTasksSamples;
  52
  53 static FAST_RAM_ZERO_INIT bool calculateTaskStatistics;
  54 FAST_RAM_ZERO_INIT uint16_t averageSystemLoadPercent = 0;
  55
  56 static FAST_RAM_ZERO_INIT int taskQueuePos = 0;
  57 STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT int taskQueueSize = 0;
  58
  59 static FAST_RAM int periodCalculationBasisOffset = offsetof(cfTask_t, lastExecutedAt);
  60
  61 // No need for a linked list for the queue, since items are only inserted at startup
  62
  63 STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT cfTask_t* taskQueueArray[TASK_COUNT + 1]; // extra item for NULL pointer at end of queue
  64
  65 void queueClear(void)
  66 {
  67     memset(taskQueueArray, 0, sizeof(taskQueueArray));
  68     taskQueuePos = 0;
  69     taskQueueSize = 0;
  70 }
  71
  72 bool queueContains(cfTask_t *task)
  73 {
  74     for (int ii = 0; ii < taskQueueSize; ++ii) {
  75         if (taskQueueArray[ii] == task) {
  76             return true;
  77         }
  78     }
  79     return false;
  80 }
  81
  82 bool queueAdd(cfTask_t *task)
  83 {
  84     if ((taskQueueSize >= TASK_COUNT) || queueContains(task)) {
  85         return false;
  86     }
  87     for (int ii = 0; ii <= taskQueueSize; ++ii) {
  88         if (taskQueueArray[ii] == NULL || taskQueueArray[ii]->staticPriority < task->staticPriority) {
  89             memmove(&taskQueueArray[ii+1], &taskQueueArray[ii], sizeof(task) * (taskQueueSize - ii));
  90             taskQueueArray[ii] = task;
  91             ++taskQueueSize;
  92             return true;
  93         }
  94     }
  95     return false;
  96 }
  97
  98 bool queueRemove(cfTask_t *task)
  99 {
 100     for (int ii = 0; ii < taskQueueSize; ++ii) {
 101         if (taskQueueArray[ii] == task) {
 102             memmove(&taskQueueArray[ii], &taskQueueArray[ii+1], sizeof(task) * (taskQueueSize - ii));
 103             --taskQueueSize;
 104             return true;
 105         }
 106     }
 107     return false;
 108 }
 109
 110 /*
 111  * Returns first item queue or NULL if queue empty
 112  */
 113 FAST_CODE cfTask_t *queueFirst(void)
 114 {
 115     taskQueuePos = 0;
 116     return taskQueueArray[0]; // guaranteed to be NULL if queue is empty
 117 }
 118
 119 /*
 120  * Returns next item in queue or NULL if at end of queue
 121  */
 122 FAST_CODE cfTask_t *queueNext(void)
 123 {
 124     return taskQueueArray[++taskQueuePos]; // guaranteed to be NULL at end of queue
 125 }
 126
 127 void taskSystemLoad(timeUs_t currentTimeUs)
 128 {
 129     UNUSED(currentTimeUs);
 130
 131     // Calculate system load
 132     if (totalWaitingTasksSamples > 0) {
 133         averageSystemLoadPercent = 100 * totalWaitingTasks / totalWaitingTasksSamples;
 134         totalWaitingTasksSamples = 0;
 135         totalWaitingTasks = 0;
 136     }
 137 #if defined(SIMULATOR_BUILD)
 138     averageSystemLoadPercent = 0;
 139 #endif
 140 }
 141
 142 #if defined(USE_TASK_STATISTICS)
 143 #define MOVING_SUM_COUNT 32
 144 timeUs_t checkFuncMaxExecutionTime;
 145 timeUs_t checkFuncTotalExecutionTime;
 146 timeUs_t checkFuncMovingSumExecutionTime;
 147 timeUs_t checkFuncMovingSumDeltaTime;
 148
 149 void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo)
 150 {
 151     checkFuncInfo->maxExecutionTime = checkFuncMaxExecutionTime;
 152     checkFuncInfo->totalExecutionTime = checkFuncTotalExecutionTime;
 153     checkFuncInfo->averageExecutionTime = checkFuncMovingSumExecutionTime / MOVING_SUM_COUNT;
 154     checkFuncInfo->averageDeltaTime = checkFuncMovingSumDeltaTime / MOVING_SUM_COUNT;
 155 }
 156 #endif
 157
 158 void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
 159 {
 160     taskInfo->isEnabled = queueContains(&cfTasks[taskId]);
 161     taskInfo->desiredPeriod = cfTasks[taskId].desiredPeriod;
 162     taskInfo->staticPriority = cfTasks[taskId].staticPriority;
 163 #if defined(USE_TASK_STATISTICS)
 164     taskInfo->taskName = cfTasks[taskId].taskName;
 165     taskInfo->subTaskName = cfTasks[taskId].subTaskName;
 166     taskInfo->maxExecutionTime = cfTasks[taskId].maxExecutionTime;
 167     taskInfo->totalExecutionTime = cfTasks[taskId].totalExecutionTime;
 168     taskInfo->averageExecutionTime = cfTasks[taskId].movingSumExecutionTime / MOVING_SUM_COUNT;
 169     taskInfo->averageDeltaTime = cfTasks[taskId].movingSumDeltaTime / MOVING_SUM_COUNT;
 170     taskInfo->latestDeltaTime = cfTasks[taskId].taskLatestDeltaTime;
 171     taskInfo->movingAverageCycleTime = cfTasks[taskId].movingAverageCycleTime;
 172 #endif
 173 }
 174
 175 void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros)
 176 {
 177     if (taskId == TASK_SELF) {
 178         cfTask_t *task = currentTask;
 179         task->desiredPeriod = MAX(SCHEDULER_DELAY_LIMIT, (timeDelta_t)newPeriodMicros);  // Limit delay to 100us (10 kHz) to prevent scheduler clogging
 180     } else if (taskId < TASK_COUNT) {
 181         cfTask_t *task = &cfTasks[taskId];
 182         task->desiredPeriod = MAX(SCHEDULER_DELAY_LIMIT, (timeDelta_t)newPeriodMicros);  // Limit delay to 100us (10 kHz) to prevent scheduler clogging
 183     }
 184 }
 185
 186 void setTaskEnabled(cfTaskId_e taskId, bool enabled)
 187 {
 188     if (taskId == TASK_SELF || taskId < TASK_COUNT) {
 189         cfTask_t *task = taskId == TASK_SELF ? currentTask : &cfTasks[taskId];
 190         if (enabled && task->taskFunc) {
 191             queueAdd(task);
 192         } else {
 193             queueRemove(task);
 194         }
 195     }
 196 }
 197
 198 timeDelta_t getTaskDeltaTime(cfTaskId_e taskId)
 199 {
 200     if (taskId == TASK_SELF) {
 201         return currentTask->taskLatestDeltaTime;
 202     } else if (taskId < TASK_COUNT) {
 203         return cfTasks[taskId].taskLatestDeltaTime;
 204     } else {
 205         return 0;
 206     }
 207 }
 208
 209 void schedulerSetCalulateTaskStatistics(bool calculateTaskStatisticsToUse)
 210 {
 211     calculateTaskStatistics = calculateTaskStatisticsToUse;
 212 }
 213
 214 void schedulerResetTaskStatistics(cfTaskId_e taskId)
 215 {
 216 #if defined(USE_TASK_STATISTICS)
 217     if (taskId == TASK_SELF) {
 218         currentTask->movingSumExecutionTime = 0;
 219         currentTask->movingSumDeltaTime = 0;
 220         currentTask->totalExecutionTime = 0;
 221         currentTask->maxExecutionTime = 0;
 222     } else if (taskId < TASK_COUNT) {
 223         cfTasks[taskId].movingSumExecutionTime = 0;
 224         cfTasks[taskId].movingSumDeltaTime = 0;
 225         cfTasks[taskId].totalExecutionTime = 0;
 226         cfTasks[taskId].maxExecutionTime = 0;
 227     }
 228 #else
 229     UNUSED(taskId);
 230 #endif
 231 }
 232
 233 void schedulerResetTaskMaxExecutionTime(cfTaskId_e taskId)
 234 {
 235 #if defined(USE_TASK_STATISTICS)
 236     if (taskId == TASK_SELF) {
 237         currentTask->maxExecutionTime = 0;
 238     } else if (taskId < TASK_COUNT) {
 239         cfTasks[taskId].maxExecutionTime = 0;
 240     }
 241 #else
 242     UNUSED(taskId);
 243 #endif
 244 }
 245
 246 void schedulerInit(void)
 247 {
 248     calculateTaskStatistics = true;
 249     queueClear();
 250     queueAdd(&cfTasks[TASK_SYSTEM]);
 251 }
 252
 253 void schedulerOptimizeRate(bool optimizeRate)
 254 {
 255     periodCalculationBasisOffset = optimizeRate ? offsetof(cfTask_t, lastDesiredAt) : offsetof(cfTask_t, lastExecutedAt);
 256 }
 257
 258 inline static timeUs_t getPeriodCalculationBasis(const cfTask_t* task)
 259 {
 260     return *(timeUs_t*)((uint8_t*)task + periodCalculationBasisOffset);
 261 }
 262
 263 FAST_CODE void scheduler(void)
 264 {
 265     // Cache currentTime
 266     const timeUs_t currentTimeUs = micros();
 267
 268     // Check for realtime tasks
 269     bool outsideRealtimeGuardInterval = true;
 270     for (const cfTask_t *task = queueFirst(); task != NULL && task->staticPriority >= TASK_PRIORITY_REALTIME; task = queueNext()) {
 271         const timeUs_t nextExecuteAt = getPeriodCalculationBasis(task) + task->desiredPeriod;
 272         if ((timeDelta_t)(currentTimeUs - nextExecuteAt) >= 0) {
 273             outsideRealtimeGuardInterval = false;
 274             break;
 275         }
 276     }
 277
 278     // The task to be invoked
 279     cfTask_t *selectedTask = NULL;
 280     uint16_t selectedTaskDynamicPriority = 0;
 281
 282     // Update task dynamic priorities
 283     uint16_t waitingTasks = 0;
 284     for (cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) {
 285         // Task has checkFunc - event driven
 286         if (task->checkFunc) {
 287 #if defined(SCHEDULER_DEBUG)
 288             const timeUs_t currentTimeBeforeCheckFuncCall = micros();
 289 #else
 290             const timeUs_t currentTimeBeforeCheckFuncCall = currentTimeUs;
 291 #endif
 292             // Increase priority for event driven tasks
 293             if (task->dynamicPriority > 0) {
 294                 task->taskAgeCycles = 1 + ((currentTimeUs - task->lastSignaledAt) / task->desiredPeriod);
 295                 task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
 296                 waitingTasks++;
 297             } else if (task->checkFunc(currentTimeBeforeCheckFuncCall, currentTimeBeforeCheckFuncCall - task->lastExecutedAt)) {
 298 #if defined(SCHEDULER_DEBUG)
 299                 DEBUG_SET(DEBUG_SCHEDULER, 3, micros() - currentTimeBeforeCheckFuncCall);
 300 #endif
 301 #if defined(USE_TASK_STATISTICS)
 302                 if (calculateTaskStatistics) {
 303                     const uint32_t checkFuncExecutionTime = micros() - currentTimeBeforeCheckFuncCall;
 304                     checkFuncMovingSumExecutionTime += checkFuncExecutionTime - checkFuncMovingSumExecutionTime / MOVING_SUM_COUNT;
 305                     checkFuncMovingSumDeltaTime += task->taskLatestDeltaTime - checkFuncMovingSumDeltaTime / MOVING_SUM_COUNT;
 306                     checkFuncTotalExecutionTime += checkFuncExecutionTime;   // time consumed by scheduler + task
 307                     checkFuncMaxExecutionTime = MAX(checkFuncMaxExecutionTime, checkFuncExecutionTime);
 308                 }
 309 #endif
 310                 task->lastSignaledAt = currentTimeBeforeCheckFuncCall;
 311                 task->taskAgeCycles = 1;
 312                 task->dynamicPriority = 1 + task->staticPriority;
 313                 waitingTasks++;
 314             } else {
 315                 task->taskAgeCycles = 0;
 316             }
 317         } else {
 318             // Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods)
 319             // Task age is calculated from last execution
 320             task->taskAgeCycles = ((currentTimeUs - getPeriodCalculationBasis(task)) / task->desiredPeriod);
 321             if (task->taskAgeCycles > 0) {
 322                 task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
 323                 waitingTasks++;
 324             }
 325         }
 326
 327         if (task->dynamicPriority > selectedTaskDynamicPriority) {
 328             const bool taskCanBeChosenForScheduling =
 329                 (outsideRealtimeGuardInterval) ||
 330                 (task->taskAgeCycles > 1) ||
 331                 (task->staticPriority == TASK_PRIORITY_REALTIME);
 332             if (taskCanBeChosenForScheduling) {
 333                 selectedTaskDynamicPriority = task->dynamicPriority;
 334                 selectedTask = task;
 335             }
 336         }
 337     }
 338
 339     totalWaitingTasksSamples++;
 340     totalWaitingTasks += waitingTasks;
 341
 342     currentTask = selectedTask;
 343
 344     if (selectedTask) {
 345         // Found a task that should be run
 346         selectedTask->taskLatestDeltaTime = currentTimeUs - selectedTask->lastExecutedAt;
 347         float period = currentTimeUs - selectedTask->lastExecutedAt;
 348         selectedTask->lastExecutedAt = currentTimeUs;
 349         selectedTask->lastDesiredAt += (cmpTimeUs(currentTimeUs, selectedTask->lastDesiredAt) / selectedTask->desiredPeriod) * selectedTask->desiredPeriod;
 350         selectedTask->dynamicPriority = 0;
 351
 352         // Execute task
 353 #if defined(USE_TASK_STATISTICS)
 354         if (calculateTaskStatistics) {
 355             const timeUs_t currentTimeBeforeTaskCall = micros();
 356             selectedTask->taskFunc(currentTimeBeforeTaskCall);
 357             const timeUs_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
 358             selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / MOVING_SUM_COUNT;
 359             selectedTask->movingSumDeltaTime += selectedTask->taskLatestDeltaTime - selectedTask->movingSumDeltaTime / MOVING_SUM_COUNT;
 360             selectedTask->totalExecutionTime += taskExecutionTime;   // time consumed by scheduler + task
 361             selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
 362             selectedTask->movingAverageCycleTime += 0.05f * (period - selectedTask->movingAverageCycleTime);
 363         } else
 364 #endif
 365         {
 366             selectedTask->taskFunc(currentTimeUs);
 367         }
 368
 369 #if defined(SCHEDULER_DEBUG)
 370         DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs - taskExecutionTime); // time spent in scheduler
 371     } else {
 372         DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs);
 373 #endif
 374     }
 375
 376     GET_SCHEDULER_LOCALS();
 377 }