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 //#define SCHEDULER_DEBUG
  31
  32 cfTaskId_e currentTaskId = TASK_NONE;
  33
  34 static uint32_t totalWaitingTasks;
  35 static uint32_t totalWaitingTasksSamples;
  36 static uint32_t realtimeGuardInterval;
  37
  38 uint32_t currentTime = 0;
  39 uint16_t averageWaitingTasks100 = 0;
  40
  41 typedef struct {
  42     /* Configuration */
  43     const char * taskName;
  44     bool (*checkFunc)(uint32_t currentDeltaTime);
  45     void (*taskFunc)(void);
  46     bool isEnabled;
  47     uint32_t desiredPeriod;     // target period of execution
  48     uint8_t staticPriority;     // dynamicPriority grows in steps of this size, shouldn't be zero
  49
  50     /* Scheduling */
  51     uint8_t dynamicPriority;    // measurement of how old task was last executed, used to avoid task starvation
  52     uint32_t lastExecutedAt;    // last time of invocation
  53     uint32_t lastSignaledAt;    // time of invocation event for event-driven tasks
  54     uint16_t taskAgeCycles;
  55
  56     /* Statistics */
  57     uint32_t averageExecutionTime;  // Moving averate over 6 samples, used to calculate guard interval
  58     uint32_t taskLatestDeltaTime;   //
  59 #ifndef SKIP_TASK_STATISTICS
  60     uint32_t maxExecutionTime;
  61     uint32_t totalExecutionTime;    // total time consumed by task since boot
  62 #endif
  63 } cfTask_t;
  64
  65 void taskMainPidLoopCheck(void);
  66 void taskUpdateAccelerometer(void);
  67 void taskHandleSerial(void);
  68 void taskUpdateBeeper(void);
  69 void taskUpdateBattery(void);
  70 bool taskUpdateRxCheck(uint32_t currentDeltaTime);
  71 void taskUpdateRxMain(void);
  72 void taskProcessGPS(void);
  73 void taskUpdateCompass(void);
  74 void taskUpdateBaro(void);
  75 void taskUpdateSonar(void);
  76 void taskCalculateAltitude(void);
  77 void taskUpdateDisplay(void);
  78 void taskTelemetry(void);
  79 void taskLedStrip(void);
  80 void taskSystem(void);
  81 #ifdef USE_BST
  82 void taskBstProcess(void);
  83 #endif
  84
  85 static cfTask_t cfTasks[TASK_COUNT] = {
  86     [TASK_SYSTEM] = {
  87         .isEnabled = true,
  88         .taskName = "SYSTEM",
  89         .taskFunc = taskSystem,
  90         .desiredPeriod = 1000000 / 10,              // run every 100 ms
  91         .staticPriority = TASK_PRIORITY_HIGH,
  92     },
  93
  94     [TASK_GYROPID] = {
  95         .taskName = "GYRO/PID",
  96         .taskFunc = taskMainPidLoopCheck,
  97         .desiredPeriod = 1000,
  98         .staticPriority = TASK_PRIORITY_REALTIME,
  99     },
 100
 101     [TASK_ACCEL] = {
 102         .taskName = "ACCEL",
 103         .taskFunc = taskUpdateAccelerometer,
 104         .desiredPeriod = 1000000 / 100,
 105         .staticPriority = TASK_PRIORITY_MEDIUM,
 106     },
 107
 108     [TASK_SERIAL] = {
 109         .taskName = "SERIAL",
 110         .taskFunc = taskHandleSerial,
 111         .desiredPeriod = 1000000 / 100,     // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud
 112         .staticPriority = TASK_PRIORITY_LOW,
 113     },
 114
 115     [TASK_BEEPER] = {
 116         .taskName = "BEEPER",
 117         .taskFunc = taskUpdateBeeper,
 118         .desiredPeriod = 1000000 / 100,     // 100 Hz
 119         .staticPriority = TASK_PRIORITY_MEDIUM,
 120     },
 121
 122     [TASK_BATTERY] = {
 123         .taskName = "BATTERY",
 124         .taskFunc = taskUpdateBattery,
 125         .desiredPeriod = 1000000 / 50,      // 50 Hz
 126         .staticPriority = TASK_PRIORITY_MEDIUM,
 127     },
 128
 129     [TASK_RX] = {
 130         .taskName = "RX",
 131         .checkFunc = taskUpdateRxCheck,
 132         .taskFunc = taskUpdateRxMain,
 133         .desiredPeriod = 1000000 / 50,      // If event-based scheduling doesn't work, fallback to periodic scheduling
 134         .staticPriority = TASK_PRIORITY_HIGH,
 135     },
 136
 137 #ifdef GPS
 138     [TASK_GPS] = {
 139         .taskName = "GPS",
 140         .taskFunc = taskProcessGPS,
 141         .desiredPeriod = 1000000 / 10,      // GPS usually don't go raster than 10Hz
 142         .staticPriority = TASK_PRIORITY_MEDIUM,
 143     },
 144 #endif
 145
 146 #ifdef MAG
 147     [TASK_COMPASS] = {
 148         .taskName = "COMPASS",
 149         .taskFunc = taskUpdateCompass,
 150         .desiredPeriod = 1000000 / 10,      // Compass is updated at 10 Hz
 151         .staticPriority = TASK_PRIORITY_MEDIUM,
 152     },
 153 #endif
 154
 155 #ifdef BARO
 156     [TASK_BARO] = {
 157         .taskName = "BARO",
 158         .taskFunc = taskUpdateBaro,
 159         .desiredPeriod = 1000000 / 20,
 160         .staticPriority = TASK_PRIORITY_MEDIUM,
 161     },
 162 #endif
 163
 164 #ifdef SONAR
 165     [TASK_SONAR] = {
 166         .taskName = "SONAR",
 167         .taskFunc = taskUpdateSonar,
 168         .desiredPeriod = 1000000 / 20,
 169         .staticPriority = TASK_PRIORITY_MEDIUM,
 170     },
 171 #endif
 172
 173 #if defined(BARO) || defined(SONAR)
 174     [TASK_ALTITUDE] = {
 175         .taskName = "ALTITUDE",
 176         .taskFunc = taskCalculateAltitude,
 177         .desiredPeriod = 1000000 / 40,
 178         .staticPriority = TASK_PRIORITY_MEDIUM,
 179     },
 180 #endif
 181
 182 #ifdef DISPLAY
 183     [TASK_DISPLAY] = {
 184         .taskName = "DISPLAY",
 185         .taskFunc = taskUpdateDisplay,
 186         .desiredPeriod = 1000000 / 10,
 187         .staticPriority = TASK_PRIORITY_LOW,
 188     },
 189 #endif
 190
 191 #ifdef TELEMETRY
 192     [TASK_TELEMETRY] = {
 193         .taskName = "TELEMETRY",
 194         .taskFunc = taskTelemetry,
 195         .desiredPeriod = 1000000 / 250,         // 250 Hz
 196         .staticPriority = TASK_PRIORITY_IDLE,
 197     },
 198 #endif
 199
 200 #ifdef LED_STRIP
 201     [TASK_LEDSTRIP] = {
 202         .taskName = "LEDSTRIP",
 203         .taskFunc = taskLedStrip,
 204         .desiredPeriod = 1000000 / 100,         // 100 Hz
 205         .staticPriority = TASK_PRIORITY_IDLE,
 206     },
 207 #endif
 208
 209 #ifdef USE_BST
 210     [TASK_BST_PROCESS] = {
 211         .taskName = "BST_PROCESS",
 212         .taskFunc = taskBstProcess,
 213         .desiredPeriod = 1000000 / 50,          // 50 Hz
 214         .staticPriority = TASK_PRIORITY_IDLE,
 215     },
 216 #endif
 217 };
 218
 219 #define REALTIME_GUARD_INTERVAL_MIN     10
 220 #define REALTIME_GUARD_INTERVAL_MAX     300
 221
 222 void taskSystem(void)
 223 {
 224     uint8_t taskId;
 225
 226     /* Calculate system load */
 227     if (totalWaitingTasksSamples > 0) {
 228         averageWaitingTasks100 = 100 * totalWaitingTasks / totalWaitingTasksSamples;
 229         totalWaitingTasksSamples = 0;
 230         totalWaitingTasks = 0;
 231     }
 232
 233     /* Calculate guard interval */
 234     uint32_t maxNonRealtimeTaskTime = 0;
 235     for (taskId = 0; taskId < TASK_COUNT; taskId++) {
 236         if (cfTasks[taskId].staticPriority != TASK_PRIORITY_REALTIME) {
 237             maxNonRealtimeTaskTime = MAX(maxNonRealtimeTaskTime, cfTasks[taskId].averageExecutionTime);
 238         }
 239     }
 240
 241     realtimeGuardInterval = constrain(maxNonRealtimeTaskTime, REALTIME_GUARD_INTERVAL_MIN, REALTIME_GUARD_INTERVAL_MAX);
 242 #if defined SCHEDULER_DEBUG
 243     debug[2] = realtimeGuardInterval;
 244 #endif
 245 }
 246
 247 #ifndef SKIP_TASK_STATISTICS
 248 void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
 249 {
 250     taskInfo->taskName = cfTasks[taskId].taskName;
 251     taskInfo->isEnabled= cfTasks[taskId].isEnabled;
 252     taskInfo->desiredPeriod = cfTasks[taskId].desiredPeriod;
 253     taskInfo->staticPriority = cfTasks[taskId].staticPriority;
 254     taskInfo->maxExecutionTime = cfTasks[taskId].maxExecutionTime;
 255     taskInfo->totalExecutionTime = cfTasks[taskId].totalExecutionTime;
 256     taskInfo->averageExecutionTime = cfTasks[taskId].averageExecutionTime;
 257 }
 258 #endif
 259
 260 void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros)
 261 {
 262     if (taskId == TASK_SELF)
 263         taskId = currentTaskId;
 264
 265     if (taskId < TASK_COUNT) {
 266         cfTasks[taskId].desiredPeriod = MAX(100, newPeriodMicros);  // Limit delay to 100us (10 kHz) to prevent scheduler clogging
 267     }
 268 }
 269
 270 void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState)
 271 {
 272     if (taskId == TASK_SELF)
 273         taskId = currentTaskId;
 274
 275     if (taskId < TASK_COUNT) {
 276         cfTasks[taskId].isEnabled = newEnabledState;
 277     }
 278 }
 279
 280 uint32_t getTaskDeltaTime(cfTaskId_e taskId)
 281 {
 282     if (taskId == TASK_SELF)
 283         taskId = currentTaskId;
 284
 285     if (taskId < TASK_COUNT) {
 286         return cfTasks[taskId].taskLatestDeltaTime;
 287     }
 288     else {
 289         return 0;
 290     }
 291 }
 292
 293 void scheduler(void)
 294 {
 295     uint8_t taskId;
 296     uint8_t selectedTaskId;
 297     uint8_t selectedTaskDynPrio;
 298     uint16_t waitingTasks = 0;
 299     uint32_t timeToNextRealtimeTask = UINT32_MAX;
 300
 301     /* Cache currentTime */
 302     currentTime = micros();
 303
 304     /* The task to be invoked */
 305     selectedTaskId = TASK_NONE;
 306     selectedTaskDynPrio = 0;
 307
 308     /* Check for realtime tasks */
 309     for (taskId = 0; taskId < TASK_COUNT; taskId++) {
 310         if (cfTasks[taskId].staticPriority == TASK_PRIORITY_REALTIME) {
 311             uint32_t nextExecuteAt = cfTasks[taskId].lastExecutedAt + cfTasks[taskId].desiredPeriod;
 312             if ((int32_t)(currentTime - nextExecuteAt) >= 0) {
 313                 timeToNextRealtimeTask = 0;
 314             }
 315             else {
 316                 uint32_t newTimeInterval = nextExecuteAt - currentTime;
 317                 timeToNextRealtimeTask = MIN(timeToNextRealtimeTask, newTimeInterval);
 318             }
 319         }
 320     }
 321
 322     bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > realtimeGuardInterval);
 323
 324     /* Update task dynamic priorities */
 325     for (taskId = 0; taskId < TASK_COUNT; taskId++) {
 326         if (cfTasks[taskId].isEnabled) {
 327             /* Task has checkFunc - event driven */
 328             if (cfTasks[taskId].checkFunc != NULL) {
 329                 /* Increase priority for event driven tasks */
 330                 if (cfTasks[taskId].dynamicPriority > 0) {
 331                     cfTasks[taskId].taskAgeCycles = 1 + ((currentTime - cfTasks[taskId].lastSignaledAt) / cfTasks[taskId].desiredPeriod);
 332                     cfTasks[taskId].dynamicPriority = 1 + cfTasks[taskId].staticPriority * cfTasks[taskId].taskAgeCycles;
 333                     waitingTasks++;
 334                 }
 335                 else if (cfTasks[taskId].checkFunc(currentTime - cfTasks[taskId].lastExecutedAt)) {
 336                     cfTasks[taskId].lastSignaledAt = currentTime;
 337                     cfTasks[taskId].taskAgeCycles = 1;
 338                     cfTasks[taskId].dynamicPriority = 1 + cfTasks[taskId].staticPriority;
 339                     waitingTasks++;
 340                 }
 341                 else {
 342                     cfTasks[taskId].taskAgeCycles = 0;
 343                 }
 344             }
 345             /* Task is time-driven, dynamicPriority is last execution age measured in desiredPeriods) */
 346             else {
 347                 // Task age is calculated from last execution
 348                 cfTasks[taskId].taskAgeCycles = ((currentTime - cfTasks[taskId].lastExecutedAt) / cfTasks[taskId].desiredPeriod);
 349                 if (cfTasks[taskId].taskAgeCycles > 0) {
 350                     cfTasks[taskId].dynamicPriority = 1 + cfTasks[taskId].staticPriority * cfTasks[taskId].taskAgeCycles;
 351                     waitingTasks++;
 352                 }
 353             }
 354
 355             /* limit new priority to avoid overflow of uint8_t */
 356             cfTasks[taskId].dynamicPriority = MIN(cfTasks[taskId].dynamicPriority, TASK_PRIORITY_MAX);;
 357
 358             bool taskCanBeChosenForScheduling =
 359                 (outsideRealtimeGuardInterval) ||
 360                 (cfTasks[taskId].taskAgeCycles > 1) ||
 361                 (cfTasks[taskId].staticPriority == TASK_PRIORITY_REALTIME);
 362
 363             if (taskCanBeChosenForScheduling && (cfTasks[taskId].dynamicPriority > selectedTaskDynPrio)) {
 364                 selectedTaskDynPrio = cfTasks[taskId].dynamicPriority;
 365                 selectedTaskId = taskId;
 366             }
 367         }
 368     }
 369
 370     totalWaitingTasksSamples += 1;
 371     totalWaitingTasks += waitingTasks;
 372
 373     /* Found a task that should be run */
 374     if (selectedTaskId != TASK_NONE) {
 375         cfTasks[selectedTaskId].taskLatestDeltaTime = currentTime - cfTasks[selectedTaskId].lastExecutedAt;
 376         cfTasks[selectedTaskId].lastExecutedAt = currentTime;
 377         cfTasks[selectedTaskId].dynamicPriority = 0;
 378
 379         currentTaskId = selectedTaskId;
 380
 381         uint32_t currentTimeBeforeTaskCall = micros();
 382
 383         /* Execute task */
 384         if (cfTasks[selectedTaskId].taskFunc != NULL) {
 385             cfTasks[selectedTaskId].taskFunc();
 386         }
 387
 388         uint32_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
 389
 390         cfTasks[selectedTaskId].averageExecutionTime = ((uint32_t)cfTasks[selectedTaskId].averageExecutionTime * 31 + taskExecutionTime) / 32;
 391 #ifndef SKIP_TASK_STATISTICS
 392         cfTasks[selectedTaskId].totalExecutionTime += taskExecutionTime;   // time consumed by scheduler + task
 393         cfTasks[selectedTaskId].maxExecutionTime = MAX(cfTasks[selectedTaskId].maxExecutionTime, taskExecutionTime);
 394 #endif
 395 #if defined SCHEDULER_DEBUG
 396         debug[3] = (micros() - currentTime) - taskExecutionTime;
 397 #endif
 398     }
 399     else {
 400         currentTaskId = TASK_NONE;
 401 #if defined SCHEDULER_DEBUG
 402         debug[3] = (micros() - currentTime);
 403 #endif
 404     }
 405 }