src/main/scheduler/scheduler.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #define SRC_MAIN_SCHEDULER_C_
  19
  20 #include <stdbool.h>
  21 #include <stdint.h>
  22 #include <string.h>
  23
  24 #include "platform.h"
  25
  26 #include "build/build_config.h"
  27 #include "build/debug.h"
  28
  29 #include "scheduler/scheduler.h"
  30
  31 #include "config/config_unittest.h"
  32
  33 #include "common/maths.h"
  34 #include "common/time.h"
  35 #include "common/utils.h"
  36
  37 #include "drivers/time.h"
  38
  39 // DEBUG_SCHEDULER, timings for:
  40 // 0 - gyroUpdate()
  41 // 1 - pidController()
  42 // 2 - time spent in scheduler
  43 // 3 - time spent executing check function
  44
  45 static FAST_RAM cfTask_t *currentTask = NULL;
  46
  47 static FAST_RAM uint32_t totalWaitingTasks;
  48 static FAST_RAM uint32_t totalWaitingTasksSamples;
  49
  50 static FAST_RAM bool calculateTaskStatistics;
  51 FAST_RAM uint16_t averageSystemLoadPercent = 0;
  52
  53
  54 static FAST_RAM int taskQueuePos = 0;
  55 STATIC_UNIT_TESTED FAST_RAM int taskQueueSize = 0;
  56
  57 // No need for a linked list for the queue, since items are only inserted at startup
  58
  59 STATIC_UNIT_TESTED FAST_RAM cfTask_t* taskQueueArray[TASK_COUNT + 1]; // extra item for NULL pointer at end of queue
  60
  61 void queueClear(void)
  62 {
  63     memset(taskQueueArray, 0, sizeof(taskQueueArray));
  64     taskQueuePos = 0;
  65     taskQueueSize = 0;
  66 }
  67
  68 bool queueContains(cfTask_t *task)
  69 {
  70     for (int ii = 0; ii < taskQueueSize; ++ii) {
  71         if (taskQueueArray[ii] == task) {
  72             return true;
  73         }
  74     }
  75     return false;
  76 }
  77
  78 bool queueAdd(cfTask_t *task)
  79 {
  80     if ((taskQueueSize >= TASK_COUNT) || queueContains(task)) {
  81         return false;
  82     }
  83     for (int ii = 0; ii <= taskQueueSize; ++ii) {
  84         if (taskQueueArray[ii] == NULL || taskQueueArray[ii]->staticPriority < task->staticPriority) {
  85             memmove(&taskQueueArray[ii+1], &taskQueueArray[ii], sizeof(task) * (taskQueueSize - ii));
  86             taskQueueArray[ii] = task;
  87             ++taskQueueSize;
  88             return true;
  89         }
  90     }
  91     return false;
  92 }
  93
  94 bool queueRemove(cfTask_t *task)
  95 {
  96     for (int ii = 0; ii < taskQueueSize; ++ii) {
  97         if (taskQueueArray[ii] == task) {
  98             memmove(&taskQueueArray[ii], &taskQueueArray[ii+1], sizeof(task) * (taskQueueSize - ii));
  99             --taskQueueSize;
 100             return true;
 101         }
 102     }
 103     return false;
 104 }
 105
 106 /*
 107  * Returns first item queue or NULL if queue empty
 108  */
 109 FAST_CODE cfTask_t *queueFirst(void)
 110 {
 111     taskQueuePos = 0;
 112     return taskQueueArray[0]; // guaranteed to be NULL if queue is empty
 113 }
 114
 115 /*
 116  * Returns next item in queue or NULL if at end of queue
 117  */
 118 FAST_CODE cfTask_t *queueNext(void)
 119 {
 120     return taskQueueArray[++taskQueuePos]; // guaranteed to be NULL at end of queue
 121 }
 122
 123 void taskSystem(timeUs_t currentTimeUs)
 124 {
 125     UNUSED(currentTimeUs);
 126
 127     // Calculate system load
 128     if (totalWaitingTasksSamples > 0) {
 129         averageSystemLoadPercent = 100 * totalWaitingTasks / totalWaitingTasksSamples;
 130         totalWaitingTasksSamples = 0;
 131         totalWaitingTasks = 0;
 132     }
 133 #if defined(SIMULATOR_BUILD)
 134     averageSystemLoadPercent = 0;
 135 #endif
 136 }
 137
 138 #ifndef SKIP_TASK_STATISTICS
 139 #define MOVING_SUM_COUNT 32
 140 timeUs_t checkFuncMaxExecutionTime;
 141 timeUs_t checkFuncTotalExecutionTime;
 142 timeUs_t checkFuncMovingSumExecutionTime;
 143
 144 void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo)
 145 {
 146     checkFuncInfo->maxExecutionTime = checkFuncMaxExecutionTime;
 147     checkFuncInfo->totalExecutionTime = checkFuncTotalExecutionTime;
 148     checkFuncInfo->averageExecutionTime = checkFuncMovingSumExecutionTime / MOVING_SUM_COUNT;
 149 }
 150
 151 void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
 152 {
 153     taskInfo->taskName = cfTasks[taskId].taskName;
 154     taskInfo->subTaskName = cfTasks[taskId].subTaskName;
 155     taskInfo->isEnabled = queueContains(&cfTasks[taskId]);
 156     taskInfo->desiredPeriod = cfTasks[taskId].desiredPeriod;
 157     taskInfo->staticPriority = cfTasks[taskId].staticPriority;
 158     taskInfo->maxExecutionTime = cfTasks[taskId].maxExecutionTime;
 159     taskInfo->totalExecutionTime = cfTasks[taskId].totalExecutionTime;
 160     taskInfo->averageExecutionTime = cfTasks[taskId].movingSumExecutionTime / MOVING_SUM_COUNT;
 161     taskInfo->latestDeltaTime = cfTasks[taskId].taskLatestDeltaTime;
 162 }
 163 #endif
 164
 165 void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros)
 166 {
 167     if (taskId == TASK_SELF) {
 168         cfTask_t *task = currentTask;
 169         task->desiredPeriod = MAX(SCHEDULER_DELAY_LIMIT, (timeDelta_t)newPeriodMicros);  // Limit delay to 100us (10 kHz) to prevent scheduler clogging
 170     } else if (taskId < TASK_COUNT) {
 171         cfTask_t *task = &cfTasks[taskId];
 172         task->desiredPeriod = MAX(SCHEDULER_DELAY_LIMIT, (timeDelta_t)newPeriodMicros);  // Limit delay to 100us (10 kHz) to prevent scheduler clogging
 173     }
 174 }
 175
 176 void setTaskEnabled(cfTaskId_e taskId, bool enabled)
 177 {
 178     if (taskId == TASK_SELF || taskId < TASK_COUNT) {
 179         cfTask_t *task = taskId == TASK_SELF ? currentTask : &cfTasks[taskId];
 180         if (enabled && task->taskFunc) {
 181             queueAdd(task);
 182         } else {
 183             queueRemove(task);
 184         }
 185     }
 186 }
 187
 188 timeDelta_t getTaskDeltaTime(cfTaskId_e taskId)
 189 {
 190     if (taskId == TASK_SELF) {
 191         return currentTask->taskLatestDeltaTime;
 192     } else if (taskId < TASK_COUNT) {
 193         return cfTasks[taskId].taskLatestDeltaTime;
 194     } else {
 195         return 0;
 196     }
 197 }
 198
 199 void schedulerSetCalulateTaskStatistics(bool calculateTaskStatisticsToUse)
 200 {
 201     calculateTaskStatistics = calculateTaskStatisticsToUse;
 202 }
 203
 204 void schedulerResetTaskStatistics(cfTaskId_e taskId)
 205 {
 206 #ifdef SKIP_TASK_STATISTICS
 207     UNUSED(taskId);
 208 #else
 209     if (taskId == TASK_SELF) {
 210         currentTask->movingSumExecutionTime = 0;
 211         currentTask->totalExecutionTime = 0;
 212         currentTask->maxExecutionTime = 0;
 213     } else if (taskId < TASK_COUNT) {
 214         cfTasks[taskId].movingSumExecutionTime = 0;
 215         cfTasks[taskId].totalExecutionTime = 0;
 216         cfTasks[taskId].maxExecutionTime = 0;
 217     }
 218 #endif
 219 }
 220
 221 void schedulerInit(void)
 222 {
 223     calculateTaskStatistics = true;
 224     queueClear();
 225     queueAdd(&cfTasks[TASK_SYSTEM]);
 226 }
 227
 228 FAST_CODE void scheduler(void)
 229 {
 230     // Cache currentTime
 231     const timeUs_t currentTimeUs = micros();
 232
 233     // Check for realtime tasks
 234     bool outsideRealtimeGuardInterval = true;
 235     for (const cfTask_t *task = queueFirst(); task != NULL && task->staticPriority >= TASK_PRIORITY_REALTIME; task = queueNext()) {
 236         const timeUs_t nextExecuteAt = task->lastExecutedAt + task->desiredPeriod;
 237         if ((timeDelta_t)(currentTimeUs - nextExecuteAt) >= 0) {
 238             outsideRealtimeGuardInterval = false;
 239             break;
 240         }
 241     }
 242
 243     // The task to be invoked
 244     cfTask_t *selectedTask = NULL;
 245     uint16_t selectedTaskDynamicPriority = 0;
 246
 247     // Update task dynamic priorities
 248     uint16_t waitingTasks = 0;
 249     for (cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) {
 250         // Task has checkFunc - event driven
 251         if (task->checkFunc) {
 252 #if defined(SCHEDULER_DEBUG)
 253             const timeUs_t currentTimeBeforeCheckFuncCall = micros();
 254 #else
 255             const timeUs_t currentTimeBeforeCheckFuncCall = currentTimeUs;
 256 #endif
 257             // Increase priority for event driven tasks
 258             if (task->dynamicPriority > 0) {
 259                 task->taskAgeCycles = 1 + ((currentTimeUs - task->lastSignaledAt) / task->desiredPeriod);
 260                 task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
 261                 waitingTasks++;
 262             } else if (task->checkFunc(currentTimeBeforeCheckFuncCall, currentTimeBeforeCheckFuncCall - task->lastExecutedAt)) {
 263 #if defined(SCHEDULER_DEBUG)
 264                 DEBUG_SET(DEBUG_SCHEDULER, 3, micros() - currentTimeBeforeCheckFuncCall);
 265 #endif
 266 #ifndef SKIP_TASK_STATISTICS
 267                 if (calculateTaskStatistics) {
 268                     const uint32_t checkFuncExecutionTime = micros() - currentTimeBeforeCheckFuncCall;
 269                     checkFuncMovingSumExecutionTime += checkFuncExecutionTime - checkFuncMovingSumExecutionTime / MOVING_SUM_COUNT;
 270                     checkFuncTotalExecutionTime += checkFuncExecutionTime;   // time consumed by scheduler + task
 271                     checkFuncMaxExecutionTime = MAX(checkFuncMaxExecutionTime, checkFuncExecutionTime);
 272                 }
 273 #endif
 274                 task->lastSignaledAt = currentTimeBeforeCheckFuncCall;
 275                 task->taskAgeCycles = 1;
 276                 task->dynamicPriority = 1 + task->staticPriority;
 277                 waitingTasks++;
 278             } else {
 279                 task->taskAgeCycles = 0;
 280             }
 281         } else {
 282             // Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods)
 283             // Task age is calculated from last execution
 284             task->taskAgeCycles = ((currentTimeUs - task->lastExecutedAt) / task->desiredPeriod);
 285             if (task->taskAgeCycles > 0) {
 286                 task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
 287                 waitingTasks++;
 288             }
 289         }
 290
 291         if (task->dynamicPriority > selectedTaskDynamicPriority) {
 292             const bool taskCanBeChosenForScheduling =
 293                 (outsideRealtimeGuardInterval) ||
 294                 (task->taskAgeCycles > 1) ||
 295                 (task->staticPriority == TASK_PRIORITY_REALTIME);
 296             if (taskCanBeChosenForScheduling) {
 297                 selectedTaskDynamicPriority = task->dynamicPriority;
 298                 selectedTask = task;
 299             }
 300         }
 301     }
 302
 303     totalWaitingTasksSamples++;
 304     totalWaitingTasks += waitingTasks;
 305
 306     currentTask = selectedTask;
 307
 308     if (selectedTask) {
 309         // Found a task that should be run
 310         selectedTask->taskLatestDeltaTime = currentTimeUs - selectedTask->lastExecutedAt;
 311         selectedTask->lastExecutedAt = currentTimeUs;
 312         selectedTask->dynamicPriority = 0;
 313
 314         // Execute task
 315 #ifdef SKIP_TASK_STATISTICS
 316         selectedTask->taskFunc(currentTimeUs);
 317 #else
 318         if (calculateTaskStatistics) {
 319             const timeUs_t currentTimeBeforeTaskCall = micros();
 320             selectedTask->taskFunc(currentTimeBeforeTaskCall);
 321             const timeUs_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
 322             selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / MOVING_SUM_COUNT;
 323             selectedTask->totalExecutionTime += taskExecutionTime;   // time consumed by scheduler + task
 324             selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
 325         } else {
 326             selectedTask->taskFunc(currentTimeUs);
 327         }
 328
 329 #endif
 330 #if defined(SCHEDULER_DEBUG)
 331         DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs - taskExecutionTime); // time spent in scheduler
 332     } else {
 333         DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs);
 334 #endif
 335     }
 336
 337     GET_SCHEDULER_LOCALS();
 338 }