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 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21
  22 #include "platform.h"
  23
  24 #include "scheduler.h"
  25
  26 #include "build/build_config.h"
  27 #include "build/debug.h"
  28
  29 #include "common/maths.h"
  30 #include "common/time.h"
  31 #include "common/utils.h"
  32
  33 #include "drivers/time.h"
  34
  35 STATIC_FASTRAM cfTask_t *currentTask = NULL;
  36
  37 STATIC_FASTRAM uint32_t totalWaitingTasks;
  38 STATIC_FASTRAM uint32_t totalWaitingTasksSamples;
  39
  40 FASTRAM uint16_t averageSystemLoadPercent = 0;
  41
  42
  43 STATIC_FASTRAM int taskQueuePos = 0;
  44 STATIC_FASTRAM int taskQueueSize = 0;
  45 // No need for a linked list for the queue, since items are only inserted at startup
  46 #ifdef UNIT_TEST
  47 STATIC_FASTRAM_UNIT_TESTED cfTask_t* taskQueueArray[TASK_COUNT + 2]; // 1 extra space so test code can check for buffer overruns
  48 #else
  49 STATIC_FASTRAM cfTask_t* taskQueueArray[TASK_COUNT + 1]; // extra item for NULL pointer at end of queue
  50 #endif
  51 STATIC_UNIT_TESTED void queueClear(void)
  52 {
  53     memset(taskQueueArray, 0, sizeof(taskQueueArray));
  54     taskQueuePos = 0;
  55     taskQueueSize = 0;
  56 }
  57
  58 #ifdef UNIT_TEST
  59 STATIC_UNIT_TESTED int queueSize(void)
  60 {
  61     return taskQueueSize;
  62 }
  63 #endif
  64
  65 STATIC_UNIT_TESTED bool queueContains(cfTask_t *task)
  66 {
  67     for (int ii = 0; ii < taskQueueSize; ++ii) {
  68         if (taskQueueArray[ii] == task) {
  69             return true;
  70         }
  71     }
  72     return false;
  73 }
  74
  75 STATIC_UNIT_TESTED bool queueAdd(cfTask_t *task)
  76 {
  77     if ((taskQueueSize >= TASK_COUNT) || queueContains(task)) {
  78         return false;
  79     }
  80     for (int ii = 0; ii <= taskQueueSize; ++ii) {
  81         if (taskQueueArray[ii] == NULL || taskQueueArray[ii]->staticPriority < task->staticPriority) {
  82             memmove(&taskQueueArray[ii+1], &taskQueueArray[ii], sizeof(task) * (taskQueueSize - ii));
  83             taskQueueArray[ii] = task;
  84             ++taskQueueSize;
  85             return true;
  86         }
  87     }
  88     return false;
  89 }
  90
  91 STATIC_UNIT_TESTED bool queueRemove(cfTask_t *task)
  92 {
  93     for (int ii = 0; ii < taskQueueSize; ++ii) {
  94         if (taskQueueArray[ii] == task) {
  95             memmove(&taskQueueArray[ii], &taskQueueArray[ii+1], sizeof(task) * (taskQueueSize - ii));
  96             --taskQueueSize;
  97             return true;
  98         }
  99     }
 100     return false;
 101 }
 102
 103 /*
 104  * Returns first item queue or NULL if queue empty
 105  */
 106 STATIC_INLINE_UNIT_TESTED cfTask_t *queueFirst(void)
 107 {
 108     taskQueuePos = 0;
 109     return taskQueueArray[0]; // guaranteed to be NULL if queue is empty
 110 }
 111
 112 /*
 113  * Returns next item in queue or NULL if at end of queue
 114  */
 115 STATIC_INLINE_UNIT_TESTED cfTask_t *queueNext(void)
 116 {
 117     return taskQueueArray[++taskQueuePos]; // guaranteed to be NULL at end of queue
 118 }
 119
 120 void taskSystem(timeUs_t currentTimeUs)
 121 {
 122     UNUSED(currentTimeUs);
 123
 124     // Calculate system load
 125     if (totalWaitingTasksSamples > 0) {
 126         averageSystemLoadPercent = 100 * totalWaitingTasks / totalWaitingTasksSamples;
 127         totalWaitingTasksSamples = 0;
 128         totalWaitingTasks = 0;
 129     }
 130 }
 131
 132 #define TASK_MOVING_SUM_COUNT           32
 133 FASTRAM timeUs_t checkFuncMaxExecutionTime;
 134 FASTRAM timeUs_t checkFuncTotalExecutionTime;
 135 FASTRAM timeUs_t checkFuncMovingSumExecutionTime;
 136
 137 void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo)
 138 {
 139     checkFuncInfo->maxExecutionTime = checkFuncMaxExecutionTime;
 140     checkFuncInfo->totalExecutionTime = checkFuncTotalExecutionTime;
 141     checkFuncInfo->averageExecutionTime = checkFuncMovingSumExecutionTime / TASK_MOVING_SUM_COUNT;
 142 }
 143
 144 void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
 145 {
 146     taskInfo->taskName = cfTasks[taskId].taskName;
 147     taskInfo->isEnabled = queueContains(&cfTasks[taskId]);
 148     taskInfo->desiredPeriod = cfTasks[taskId].desiredPeriod;
 149     taskInfo->staticPriority = cfTasks[taskId].staticPriority;
 150     taskInfo->maxExecutionTime = cfTasks[taskId].maxExecutionTime;
 151     taskInfo->totalExecutionTime = cfTasks[taskId].totalExecutionTime;
 152     taskInfo->averageExecutionTime = cfTasks[taskId].movingSumExecutionTime / TASK_MOVING_SUM_COUNT;
 153     taskInfo->latestDeltaTime = cfTasks[taskId].taskLatestDeltaTime;
 154 }
 155
 156 void rescheduleTask(cfTaskId_e taskId, timeDelta_t newPeriodUs)
 157 {
 158     if (taskId == TASK_SELF) {
 159         cfTask_t *task = currentTask;
 160         task->desiredPeriod = MAX(SCHEDULER_DELAY_LIMIT, newPeriodUs);  // Limit delay to 100us (10 kHz) to prevent scheduler clogging
 161     } else if (taskId < TASK_COUNT) {
 162         cfTask_t *task = &cfTasks[taskId];
 163         task->desiredPeriod = MAX(SCHEDULER_DELAY_LIMIT, newPeriodUs);  // Limit delay to 100us (10 kHz) to prevent scheduler clogging
 164     }
 165 }
 166
 167 void setTaskEnabled(cfTaskId_e taskId, bool enabled)
 168 {
 169     if (taskId == TASK_SELF || taskId < TASK_COUNT) {
 170         cfTask_t *task = taskId == TASK_SELF ? currentTask : &cfTasks[taskId];
 171         if (enabled && task->taskFunc) {
 172             queueAdd(task);
 173         } else {
 174             queueRemove(task);
 175         }
 176     }
 177 }
 178
 179 timeDelta_t getTaskDeltaTime(cfTaskId_e taskId)
 180 {
 181     if (taskId == TASK_SELF) {
 182         return currentTask->taskLatestDeltaTime;
 183     } else if (taskId < TASK_COUNT) {
 184         return cfTasks[taskId].taskLatestDeltaTime;
 185     } else {
 186         return 0;
 187     }
 188 }
 189
 190 void schedulerResetTaskStatistics(cfTaskId_e taskId)
 191 {
 192     if (taskId == TASK_SELF) {
 193         currentTask->movingSumExecutionTime = 0;
 194         currentTask->totalExecutionTime = 0;
 195         currentTask->maxExecutionTime = 0;
 196     } else if (taskId < TASK_COUNT) {
 197         cfTasks[taskId].movingSumExecutionTime = 0;
 198         cfTasks[taskId].totalExecutionTime = 0;
 199     }
 200 }
 201
 202 void schedulerInit(void)
 203 {
 204     queueClear();
 205     queueAdd(&cfTasks[TASK_SYSTEM]);
 206 }
 207
 208 void FAST_CODE NOINLINE scheduler(void)
 209 {
 210     // Cache currentTime
 211     const timeUs_t currentTimeUs = micros();
 212
 213     // The task to be invoked
 214     cfTask_t *selectedTask = NULL;
 215     uint16_t selectedTaskDynamicPriority = 0;
 216     bool forcedRealTimeTask = false;
 217
 218     // Update task dynamic priorities
 219     uint16_t waitingTasks = 0;
 220     for (cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) {
 221         // Task has checkFunc - event driven
 222         if (task->checkFunc) {
 223             const timeUs_t currentTimeBeforeCheckFuncCallUs = micros();
 224
 225             // Increase priority for event driven tasks
 226             if (task->dynamicPriority > 0) {
 227                 task->taskAgeCycles = 1 + ((timeDelta_t)(currentTimeUs - task->lastSignaledAt)) / task->desiredPeriod;
 228                 task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
 229                 waitingTasks++;
 230             } else if (task->checkFunc(currentTimeBeforeCheckFuncCallUs, currentTimeBeforeCheckFuncCallUs - task->lastExecutedAt)) {
 231                 const timeUs_t checkFuncExecutionTime = micros() - currentTimeBeforeCheckFuncCallUs;
 232                 checkFuncMovingSumExecutionTime -= checkFuncMovingSumExecutionTime / TASK_MOVING_SUM_COUNT;
 233                 checkFuncMovingSumExecutionTime += checkFuncExecutionTime;
 234                 checkFuncTotalExecutionTime += checkFuncExecutionTime;   // time consumed by scheduler + task
 235                 checkFuncMaxExecutionTime = MAX(checkFuncMaxExecutionTime, checkFuncExecutionTime);
 236                 task->lastSignaledAt = currentTimeBeforeCheckFuncCallUs;
 237                 task->taskAgeCycles = 1;
 238                 task->dynamicPriority = 1 + task->staticPriority;
 239                 waitingTasks++;
 240             } else {
 241                 task->taskAgeCycles = 0;
 242             }
 243         } else if (task->staticPriority == TASK_PRIORITY_REALTIME) {
 244             //realtime tasks take absolute priority. Any RT tasks that is overdue, should be execute immediately
 245             if (((timeDelta_t)(currentTimeUs - task->lastExecutedAt)) > task->desiredPeriod) {
 246                 selectedTaskDynamicPriority = task->dynamicPriority;
 247                 selectedTask = task;
 248                 waitingTasks++;
 249                 forcedRealTimeTask = true;
 250             }
 251         } else {
 252             // Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods)
 253             // Task age is calculated from last execution
 254             task->taskAgeCycles = ((timeDelta_t)(currentTimeUs - task->lastExecutedAt)) / task->desiredPeriod;
 255             if (task->taskAgeCycles > 0) {
 256                 task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
 257                 waitingTasks++;
 258             }
 259         }
 260
 261         if (!forcedRealTimeTask && task->dynamicPriority > selectedTaskDynamicPriority) {
 262             selectedTaskDynamicPriority = task->dynamicPriority;
 263             selectedTask = task;
 264         }
 265     }
 266
 267     totalWaitingTasksSamples++;
 268     totalWaitingTasks += waitingTasks;
 269
 270     currentTask = selectedTask;
 271
 272     if (selectedTask) {
 273         // Found a task that should be run
 274         selectedTask->taskLatestDeltaTime = (timeDelta_t)(currentTimeUs - selectedTask->lastExecutedAt);
 275         selectedTask->lastExecutedAt = currentTimeUs;
 276         selectedTask->dynamicPriority = 0;
 277
 278         // Execute task
 279         const timeUs_t currentTimeBeforeTaskCall = micros();
 280         selectedTask->taskFunc(currentTimeBeforeTaskCall);
 281         const timeUs_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
 282         selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / TASK_MOVING_SUM_COUNT;
 283         selectedTask->totalExecutionTime += taskExecutionTime;   // time consumed by scheduler + task
 284         selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
 285     }
 286
 287     if (!selectedTask || forcedRealTimeTask) {
 288         // Execute system real-time callbacks and account for them to SYSTEM account
 289         const timeUs_t currentTimeBeforeTaskCall = micros();
 290         taskRunRealtimeCallbacks(currentTimeBeforeTaskCall);
 291         selectedTask = &cfTasks[TASK_SYSTEM];
 292         const timeUs_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
 293         selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / TASK_MOVING_SUM_COUNT;
 294         selectedTask->totalExecutionTime += taskExecutionTime;   // time consumed by scheduler + task
 295         selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
 296     }
 297 }