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