src/main/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 <stdlib.h>
  20 #include <stdint.h>
  21
  22 #include "platform.h"
  23 #include "scheduler.h"
  24 #include "debug.h"
  25
  26 #include "common/maths.h"
  27
  28 #include "drivers/system.h"
  29
  30 cfTaskId_e currentTaskId = TASK_NONE;
  31
  32 static uint32_t totalWaitingTasks;
  33 static uint32_t totalWaitingTasksSamples;
  34 static uint32_t realtimeGuardInterval;
  35
  36 uint32_t currentTime = 0;
  37 uint16_t averageSystemLoadPercent = 0;
  38
  39 #define REALTIME_GUARD_INTERVAL_MIN     10
  40 #define REALTIME_GUARD_INTERVAL_MAX     300
  41 #define REALTIME_GUARD_INTERVAL_MARGIN  25
  42
  43 void taskSystem(void)
  44 {
  45     uint8_t taskId;
  46
  47     /* Calculate system load */
  48     if (totalWaitingTasksSamples > 0) {
  49         averageSystemLoadPercent = 100 * totalWaitingTasks / totalWaitingTasksSamples;
  50         totalWaitingTasksSamples = 0;
  51         totalWaitingTasks = 0;
  52     }
  53
  54     /* Calculate guard interval */
  55     uint32_t maxNonRealtimeTaskTime = 0;
  56     for (taskId = 0; taskId < TASK_COUNT; taskId++) {
  57         if (cfTasks[taskId].staticPriority != TASK_PRIORITY_REALTIME) {
  58             maxNonRealtimeTaskTime = MAX(maxNonRealtimeTaskTime, cfTasks[taskId].averageExecutionTime);
  59         }
  60     }
  61
  62     realtimeGuardInterval = constrain(maxNonRealtimeTaskTime, REALTIME_GUARD_INTERVAL_MIN, REALTIME_GUARD_INTERVAL_MAX) + REALTIME_GUARD_INTERVAL_MARGIN;
  63 #if defined SCHEDULER_DEBUG
  64     debug[2] = realtimeGuardInterval;
  65 #endif
  66 }
  67
  68 #ifndef SKIP_TASK_STATISTICS
  69 void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
  70 {
  71     taskInfo->taskName = cfTasks[taskId].taskName;
  72     taskInfo->isEnabled= cfTasks[taskId].isEnabled;
  73     taskInfo->desiredPeriod = cfTasks[taskId].desiredPeriod;
  74     taskInfo->staticPriority = cfTasks[taskId].staticPriority;
  75     taskInfo->maxExecutionTime = cfTasks[taskId].maxExecutionTime;
  76     taskInfo->totalExecutionTime = cfTasks[taskId].totalExecutionTime;
  77     taskInfo->averageExecutionTime = cfTasks[taskId].averageExecutionTime;
  78 }
  79 #endif
  80
  81 void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros)
  82 {
  83     if (taskId == TASK_SELF)
  84         taskId = currentTaskId;
  85
  86     if (taskId < TASK_COUNT) {
  87         cfTasks[taskId].desiredPeriod = MAX(100, newPeriodMicros);  // Limit delay to 100us (10 kHz) to prevent scheduler clogging
  88     }
  89 }
  90
  91 void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState)
  92 {
  93     if (taskId == TASK_SELF)
  94         taskId = currentTaskId;
  95
  96     if (taskId < TASK_COUNT) {
  97         cfTasks[taskId].isEnabled = newEnabledState;
  98     }
  99 }
 100
 101 uint32_t getTaskDeltaTime(cfTaskId_e taskId)
 102 {
 103     if (taskId == TASK_SELF)
 104         taskId = currentTaskId;
 105
 106     if (taskId < TASK_COUNT) {
 107         return cfTasks[taskId].taskLatestDeltaTime;
 108     }
 109     else {
 110         return 0;
 111     }
 112 }
 113
 114 void scheduler(void)
 115 {
 116     uint8_t taskId;
 117     uint8_t selectedTaskId;
 118     uint8_t selectedTaskDynPrio;
 119     uint16_t waitingTasks = 0;
 120     uint32_t timeToNextRealtimeTask = UINT32_MAX;
 121
 122     /* Cache currentTime */
 123     currentTime = micros();
 124
 125     /* The task to be invoked */
 126     selectedTaskId = TASK_NONE;
 127     selectedTaskDynPrio = 0;
 128
 129     /* Check for realtime tasks */
 130     for (taskId = 0; taskId < TASK_COUNT; taskId++) {
 131         if (cfTasks[taskId].staticPriority == TASK_PRIORITY_REALTIME) {
 132             uint32_t nextExecuteAt = cfTasks[taskId].lastExecutedAt + cfTasks[taskId].desiredPeriod;
 133             if ((int32_t)(currentTime - nextExecuteAt) >= 0) {
 134                 timeToNextRealtimeTask = 0;
 135             }
 136             else {
 137                 uint32_t newTimeInterval = nextExecuteAt - currentTime;
 138                 timeToNextRealtimeTask = MIN(timeToNextRealtimeTask, newTimeInterval);
 139             }
 140         }
 141     }
 142
 143     bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > realtimeGuardInterval);
 144
 145     /* Update task dynamic priorities */
 146     for (taskId = 0; taskId < TASK_COUNT; taskId++) {
 147         if (cfTasks[taskId].isEnabled) {
 148             /* Task has checkFunc - event driven */
 149             if (cfTasks[taskId].checkFunc != NULL) {
 150                 /* Increase priority for event driven tasks */
 151                 if (cfTasks[taskId].dynamicPriority > 0) {
 152                     cfTasks[taskId].taskAgeCycles = 1 + ((currentTime - cfTasks[taskId].lastSignaledAt) / cfTasks[taskId].desiredPeriod);
 153                     cfTasks[taskId].dynamicPriority = 1 + cfTasks[taskId].staticPriority * cfTasks[taskId].taskAgeCycles;
 154                     waitingTasks++;
 155                 }
 156                 else if (cfTasks[taskId].checkFunc(currentTime - cfTasks[taskId].lastExecutedAt)) {
 157                     cfTasks[taskId].lastSignaledAt = currentTime;
 158                     cfTasks[taskId].taskAgeCycles = 1;
 159                     cfTasks[taskId].dynamicPriority = 1 + cfTasks[taskId].staticPriority;
 160                     waitingTasks++;
 161                 }
 162                 else {
 163                     cfTasks[taskId].taskAgeCycles = 0;
 164                 }
 165             }
 166             /* Task is time-driven, dynamicPriority is last execution age measured in desiredPeriods) */
 167             else {
 168                 // Task age is calculated from last execution
 169                 cfTasks[taskId].taskAgeCycles = ((currentTime - cfTasks[taskId].lastExecutedAt) / cfTasks[taskId].desiredPeriod);
 170                 if (cfTasks[taskId].taskAgeCycles > 0) {
 171                     cfTasks[taskId].dynamicPriority = 1 + cfTasks[taskId].staticPriority * cfTasks[taskId].taskAgeCycles;
 172                     waitingTasks++;
 173                 }
 174             }
 175
 176             /* limit new priority to avoid overflow of uint8_t */
 177             cfTasks[taskId].dynamicPriority = MIN(cfTasks[taskId].dynamicPriority, TASK_PRIORITY_MAX);;
 178
 179             bool taskCanBeChosenForScheduling =
 180                 (outsideRealtimeGuardInterval) ||
 181                 (cfTasks[taskId].taskAgeCycles > 1) ||
 182                 (cfTasks[taskId].staticPriority == TASK_PRIORITY_REALTIME);
 183
 184             if (taskCanBeChosenForScheduling && (cfTasks[taskId].dynamicPriority > selectedTaskDynPrio)) {
 185                 selectedTaskDynPrio = cfTasks[taskId].dynamicPriority;
 186                 selectedTaskId = taskId;
 187             }
 188         }
 189     }
 190
 191     totalWaitingTasksSamples += 1;
 192     totalWaitingTasks += waitingTasks;
 193
 194     /* Found a task that should be run */
 195     if (selectedTaskId != TASK_NONE) {
 196         cfTasks[selectedTaskId].taskLatestDeltaTime = currentTime - cfTasks[selectedTaskId].lastExecutedAt;
 197         cfTasks[selectedTaskId].lastExecutedAt = currentTime;
 198         cfTasks[selectedTaskId].dynamicPriority = 0;
 199
 200         currentTaskId = selectedTaskId;
 201
 202         uint32_t currentTimeBeforeTaskCall = micros();
 203
 204         /* Execute task */
 205         if (cfTasks[selectedTaskId].taskFunc != NULL) {
 206             cfTasks[selectedTaskId].taskFunc();
 207         }
 208
 209         uint32_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
 210
 211         cfTasks[selectedTaskId].averageExecutionTime = ((uint32_t)cfTasks[selectedTaskId].averageExecutionTime * 31 + taskExecutionTime) / 32;
 212 #ifndef SKIP_TASK_STATISTICS
 213         cfTasks[selectedTaskId].totalExecutionTime += taskExecutionTime;   // time consumed by scheduler + task
 214         cfTasks[selectedTaskId].maxExecutionTime = MAX(cfTasks[selectedTaskId].maxExecutionTime, taskExecutionTime);
 215 #endif
 216 #if defined SCHEDULER_DEBUG
 217         debug[3] = (micros() - currentTime) - taskExecutionTime;
 218 #endif
 219     }
 220     else {
 221         currentTaskId = TASK_NONE;
 222 #if defined SCHEDULER_DEBUG
 223         debug[3] = (micros() - currentTime);
 224 #endif
 225     }
 226 }