src/main/fc/tasks.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 #include <stdbool.h>
  22 #include <stdlib.h>
  23 #include <stdint.h>
  24
  25 #include "platform.h"
  26
  27 #include "build/debug.h"
  28
  29 #include "cli/cli.h"
  30
  31 #include "cms/cms.h"
  32
  33 #include "common/color.h"
  34 #include "common/utils.h"
  35
  36 #include "config/feature.h"
  37
  38 #include "drivers/accgyro/accgyro.h"
  39 #include "drivers/camera_control_impl.h"
  40 #include "drivers/compass/compass.h"
  41 #include "drivers/sensor.h"
  42 #include "drivers/serial.h"
  43 #include "drivers/serial_usb_vcp.h"
  44 #include "drivers/stack_check.h"
  45 #include "drivers/transponder_ir.h"
  46 #include "drivers/usb_io.h"
  47 #include "drivers/vtx_common.h"
  48
  49 #include "config/config.h"
  50 #include "fc/core.h"
  51 #include "fc/rc.h"
  52 #include "fc/dispatch.h"
  53 #include "fc/rc_controls.h"
  54 #include "fc/runtime_config.h"
  55
  56 #include "flight/gps_rescue.h"
  57 #include "flight/imu.h"
  58 #include "flight/mixer.h"
  59 #include "flight/pid.h"
  60 #include "flight/position.h"
  61
  62 #include "io/asyncfatfs/asyncfatfs.h"
  63 #include "io/beeper.h"
  64 #include "io/dashboard.h"
  65 #include "io/flashfs.h"
  66 #include "io/gps.h"
  67 #include "io/ledstrip.h"
  68 #include "io/piniobox.h"
  69 #include "io/serial.h"
  70 #include "io/transponder_ir.h"
  71 #include "io/vtx_tramp.h" // Will be gone
  72 #include "io/rcdevice_cam.h"
  73 #include "io/usb_cdc_hid.h"
  74 #include "io/vtx.h"
  75
  76 #include "msp/msp.h"
  77 #include "msp/msp_serial.h"
  78
  79 #include "osd/osd.h"
  80
  81 #include "pg/rx.h"
  82 #include "pg/motor.h"
  83
  84 #include "rx/rx.h"
  85 #include "rx/rc_stats.h"
  86
  87 #include "scheduler/scheduler.h"
  88
  89 #include "sensors/acceleration.h"
  90 #include "sensors/adcinternal.h"
  91 #include "sensors/barometer.h"
  92 #include "sensors/battery.h"
  93 #include "sensors/compass.h"
  94 #include "sensors/esc_sensor.h"
  95 #include "sensors/gyro.h"
  96 #include "sensors/sensors.h"
  97 #include "sensors/rangefinder.h"
  98
  99 #include "telemetry/telemetry.h"
 100 #include "telemetry/crsf.h"
 101
 102 #ifdef USE_BST
 103 #include "i2c_bst.h"
 104 #endif
 105
 106 #ifdef USE_USB_CDC_HID
 107 #include "io/usb_cdc_hid.h"
 108 #endif
 109
 110 #include "tasks.h"
 111
 112 // taskUpdateRxMain() has occasional peaks in execution time so normal moving average duration estimation doesn't work
 113 // Decay the estimated max task duration by 1/(1 << RX_TASK_DECAY_SHIFT) on every invocation
 114 #define RX_TASK_DECAY_SHIFT 6
 115 // Add a margin to the task duration estimation
 116 #define RX_TASK_MARGIN 1
 117
 118 static void taskMain(timeUs_t currentTimeUs)
 119 {
 120     UNUSED(currentTimeUs);
 121
 122 #ifdef USE_SDCARD
 123     afatfs_poll();
 124 #endif
 125
 126 #ifdef USE_FLASHFS
 127     flashfsEraseAsync();
 128 #endif
 129 }
 130
 131 static void taskHandleSerial(timeUs_t currentTimeUs)
 132 {
 133     UNUSED(currentTimeUs);
 134
 135 #if defined(USE_VCP)
 136     DEBUG_SET(DEBUG_USB, 0, usbCableIsInserted());
 137     DEBUG_SET(DEBUG_USB, 1, usbVcpIsConnected());
 138 #endif
 139
 140 #ifdef USE_CLI
 141     // in cli mode, all serial stuff goes to here. enter cli mode by sending #
 142     if (cliMode) {
 143         cliProcess();
 144         return;
 145     }
 146 #endif
 147     bool evaluateMspData = ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA;
 148     mspSerialProcess(evaluateMspData, mspFcProcessCommand, mspFcProcessReply);
 149 }
 150
 151 static void taskBatteryAlerts(timeUs_t currentTimeUs)
 152 {
 153     if (!ARMING_FLAG(ARMED)) {
 154         // the battery *might* fall out in flight, but if that happens the FC will likely be off too unless the user has battery backup.
 155         batteryUpdatePresence();
 156     }
 157     batteryUpdateStates(currentTimeUs);
 158     batteryUpdateAlarms();
 159 }
 160
 161 #ifdef USE_ACC
 162 static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
 163 {
 164     accUpdate(currentTimeUs);
 165 }
 166 #endif
 167
 168 typedef enum {
 169     RX_STATE_CHECK,
 170     RX_STATE_MODES,
 171     RX_STATE_UPDATE,
 172     RX_STATE_COUNT
 173 } rxState_e;
 174
 175 static rxState_e rxState = RX_STATE_CHECK;
 176
 177 bool taskUpdateRxMainInProgress(void)
 178 {
 179     return (rxState != RX_STATE_CHECK);
 180 }
 181
 182 static void taskUpdateRxMain(timeUs_t currentTimeUs)
 183 {
 184     static timeDelta_t rxStateDurationFractionUs[RX_STATE_COUNT];
 185     timeDelta_t executeTimeUs;
 186     rxState_e oldRxState = rxState;
 187     timeDelta_t anticipatedExecutionTime;
 188
 189     // Where we are using a state machine call schedulerIgnoreTaskExecRate() for all states bar one
 190     if (rxState != RX_STATE_UPDATE) {
 191         schedulerIgnoreTaskExecRate();
 192     }
 193
 194     switch (rxState) {
 195     default:
 196     case RX_STATE_CHECK:
 197         if (!processRx(currentTimeUs)) {
 198             rxState = RX_STATE_CHECK;
 199             break;
 200         }
 201         rxState = RX_STATE_MODES;
 202         break;
 203
 204     case RX_STATE_MODES:
 205         processRxModes(currentTimeUs);
 206         rxState = RX_STATE_UPDATE;
 207         break;
 208
 209     case RX_STATE_UPDATE:
 210         // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
 211         updateRcCommands();
 212         updateArmingStatus();
 213
 214 #ifdef USE_USB_CDC_HID
 215         if (!ARMING_FLAG(ARMED)) {
 216             sendRcDataToHid();
 217         }
 218 #endif
 219         rxState = RX_STATE_CHECK;
 220         break;
 221     }
 222
 223     if (!schedulerGetIgnoreTaskExecTime()) {
 224         executeTimeUs = micros() - currentTimeUs + RX_TASK_MARGIN;
 225
 226         // If the scheduler has reduced the anticipatedExecutionTime due to task aging, pick that up
 227         anticipatedExecutionTime = schedulerGetNextStateTime();
 228         if (anticipatedExecutionTime != (rxStateDurationFractionUs[oldRxState] >> RX_TASK_DECAY_SHIFT)) {
 229             rxStateDurationFractionUs[oldRxState] = anticipatedExecutionTime << RX_TASK_DECAY_SHIFT;
 230         }
 231
 232         if (executeTimeUs > (rxStateDurationFractionUs[oldRxState] >> RX_TASK_DECAY_SHIFT)) {
 233             rxStateDurationFractionUs[oldRxState] = executeTimeUs << RX_TASK_DECAY_SHIFT;
 234         } else {
 235             // Slowly decay the max time
 236             rxStateDurationFractionUs[oldRxState]--;
 237         }
 238     }
 239
 240     if (debugMode == DEBUG_RX_STATE_TIME) {
 241         debug[oldRxState] = rxStateDurationFractionUs[oldRxState] >> RX_TASK_DECAY_SHIFT;
 242     }
 243
 244     schedulerSetNextStateTime(rxStateDurationFractionUs[rxState] >> RX_TASK_DECAY_SHIFT);
 245 }
 246
 247 #ifdef USE_GPS_RESCUE
 248 static void taskGpsRescue(timeUs_t currentTimeUs)
 249 {
 250     UNUSED(currentTimeUs);
 251
 252     if (gpsRescueIsConfigured()) {
 253         gpsRescueUpdate();
 254     }
 255 }
 256 #endif
 257
 258 #ifdef USE_BARO
 259 static void taskUpdateBaro(timeUs_t currentTimeUs)
 260 {
 261     UNUSED(currentTimeUs);
 262
 263     if (sensors(SENSOR_BARO)) {
 264         const uint32_t newDeadline = baroUpdate(currentTimeUs);
 265         if (newDeadline != 0) {
 266             rescheduleTask(TASK_SELF, newDeadline);
 267         }
 268     }
 269 }
 270 #endif
 271
 272 #ifdef USE_MAG
 273 static void taskUpdateMag(timeUs_t currentTimeUs)
 274 {
 275     UNUSED(currentTimeUs);
 276
 277     if (sensors(SENSOR_MAG)) {
 278         const uint32_t newDeadline = compassUpdate(currentTimeUs);
 279         if (newDeadline != 0) {
 280             rescheduleTask(TASK_SELF, newDeadline);
 281         }
 282     }
 283 }
 284 #endif
 285
 286 #if defined(USE_BARO) || defined(USE_GPS)
 287 static void taskCalculateAltitude(timeUs_t currentTimeUs)
 288 {
 289     UNUSED(currentTimeUs);
 290     calculateEstimatedAltitude();
 291 }
 292 #endif // USE_BARO || USE_GPS
 293
 294 #if defined(USE_RANGEFINDER)
 295 void taskUpdateRangefinder(timeUs_t currentTimeUs)
 296 {
 297     UNUSED(currentTimeUs);
 298
 299     if (!sensors(SENSOR_RANGEFINDER)) {
 300         return;
 301     }
 302
 303     rangefinderUpdate();
 304
 305     rangefinderProcess(getCosTiltAngle());
 306 }
 307 #endif
 308
 309 #ifdef USE_TELEMETRY
 310 static void taskTelemetry(timeUs_t currentTimeUs)
 311 {
 312     if (!cliMode && featureIsEnabled(FEATURE_TELEMETRY)) {
 313         subTaskTelemetryPollSensors(currentTimeUs);
 314
 315         telemetryProcess(currentTimeUs);
 316     }
 317 }
 318 #endif
 319
 320 #ifdef USE_CAMERA_CONTROL
 321 static void taskCameraControl(uint32_t currentTime)
 322 {
 323     if (ARMING_FLAG(ARMED)) {
 324         return;
 325     }
 326
 327     cameraControlProcess(currentTime);
 328 }
 329 #endif
 330
 331 #define DEFINE_TASK(taskNameParam, subTaskNameParam, checkFuncParam, taskFuncParam, desiredPeriodParam, staticPriorityParam) {  \
 332     .taskName = taskNameParam, \
 333     .subTaskName = subTaskNameParam, \
 334     .checkFunc = checkFuncParam, \
 335     .taskFunc = taskFuncParam, \
 336     .desiredPeriodUs = desiredPeriodParam, \
 337     .staticPriority = staticPriorityParam \
 338 }
 339
 340 // Task info in .bss (unitialised data)
 341 task_t tasks[TASK_COUNT];
 342
 343 // Task ID data in .data (initialised data)
 344 task_attribute_t task_attributes[TASK_COUNT] = {
 345
 346     [TASK_SYSTEM] = DEFINE_TASK("SYSTEM", "LOAD", NULL, taskSystemLoad, TASK_PERIOD_HZ(10), TASK_PRIORITY_MEDIUM_HIGH),
 347     [TASK_MAIN] = DEFINE_TASK("SYSTEM", "UPDATE", NULL, taskMain, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM_HIGH),
 348     [TASK_SERIAL] = DEFINE_TASK("SERIAL", NULL, NULL, taskHandleSerial, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW), // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud
 349     [TASK_BATTERY_ALERTS] = DEFINE_TASK("BATTERY_ALERTS", NULL, NULL, taskBatteryAlerts, TASK_PERIOD_HZ(5), TASK_PRIORITY_MEDIUM),
 350     [TASK_BATTERY_VOLTAGE] = DEFINE_TASK("BATTERY_VOLTAGE", NULL, NULL, batteryUpdateVoltage, TASK_PERIOD_HZ(SLOW_VOLTAGE_TASK_FREQ_HZ), TASK_PRIORITY_MEDIUM), // Freq may be updated in tasksInit
 351     [TASK_BATTERY_CURRENT] = DEFINE_TASK("BATTERY_CURRENT", NULL, NULL, batteryUpdateCurrentMeter, TASK_PERIOD_HZ(50), TASK_PRIORITY_MEDIUM),
 352
 353 #ifdef USE_TRANSPONDER
 354     [TASK_TRANSPONDER] = DEFINE_TASK("TRANSPONDER", NULL, NULL, transponderUpdate, TASK_PERIOD_HZ(250), TASK_PRIORITY_LOW),
 355 #endif
 356
 357 #ifdef USE_STACK_CHECK
 358     [TASK_STACK_CHECK] = DEFINE_TASK("STACKCHECK", NULL, NULL, taskStackCheck, TASK_PERIOD_HZ(10), TASK_PRIORITY_LOWEST),
 359 #endif
 360
 361     [TASK_GYRO] = DEFINE_TASK("GYRO", NULL, NULL, taskGyroSample, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
 362     [TASK_FILTER] = DEFINE_TASK("FILTER", NULL, NULL, taskFiltering, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
 363     [TASK_PID] = DEFINE_TASK("PID", NULL, NULL, taskMainPidLoop, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
 364
 365 #ifdef USE_ACC
 366     [TASK_ACCEL] = DEFINE_TASK("ACC", NULL, NULL, taskUpdateAccelerometer, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM),
 367     [TASK_ATTITUDE] = DEFINE_TASK("ATTITUDE", NULL, NULL, imuUpdateAttitude, TASK_PERIOD_HZ(100), TASK_PRIORITY_MEDIUM),
 368 #endif
 369
 370     [TASK_RX] = DEFINE_TASK("RX", NULL, rxUpdateCheck, taskUpdateRxMain, TASK_PERIOD_HZ(33), TASK_PRIORITY_HIGH), // If event-based scheduling doesn't work, fallback to periodic scheduling
 371     [TASK_DISPATCH] = DEFINE_TASK("DISPATCH", NULL, NULL, dispatchProcess, TASK_PERIOD_HZ(1000), TASK_PRIORITY_HIGH),
 372
 373 #ifdef USE_BEEPER
 374     [TASK_BEEPER] = DEFINE_TASK("BEEPER", NULL, NULL, beeperUpdate, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW),
 375 #endif
 376
 377 #ifdef USE_GPS
 378     [TASK_GPS] = DEFINE_TASK("GPS", NULL, NULL, gpsUpdate, TASK_PERIOD_HZ(TASK_GPS_RATE), TASK_PRIORITY_MEDIUM), // Required to prevent buffer overruns if running at 115200 baud (115 bytes / period < 256 bytes buffer)
 379 #endif
 380
 381 #ifdef USE_GPS_RESCUE
 382     [TASK_GPS_RESCUE] = DEFINE_TASK("GPS_RESCUE", NULL, NULL, taskGpsRescue, TASK_PERIOD_HZ(TASK_GPS_RESCUE_RATE_HZ), TASK_PRIORITY_MEDIUM),
 383 #endif
 384
 385 #ifdef USE_MAG
 386     [TASK_COMPASS] = DEFINE_TASK("COMPASS", NULL, NULL, taskUpdateMag, TASK_PERIOD_HZ(TASK_COMPASS_RATE_HZ), TASK_PRIORITY_LOW),
 387 #endif
 388
 389 #ifdef USE_BARO
 390     [TASK_BARO] = DEFINE_TASK("BARO", NULL, NULL, taskUpdateBaro, TASK_PERIOD_HZ(TASK_BARO_RATE_HZ), TASK_PRIORITY_LOW),
 391 #endif
 392
 393 #if defined(USE_BARO) || defined(USE_GPS)
 394     [TASK_ALTITUDE] = DEFINE_TASK("ALTITUDE", NULL, NULL, taskCalculateAltitude, TASK_PERIOD_HZ(TASK_ALTITUDE_RATE_HZ), TASK_PRIORITY_LOW),
 395 #endif
 396
 397 #ifdef USE_DASHBOARD
 398     [TASK_DASHBOARD] = DEFINE_TASK("DASHBOARD", NULL, NULL, dashboardUpdate, TASK_PERIOD_HZ(10), TASK_PRIORITY_LOW),
 399 #endif
 400
 401 #ifdef USE_OSD
 402     [TASK_OSD] = DEFINE_TASK("OSD", NULL, osdUpdateCheck, osdUpdate, TASK_PERIOD_HZ(OSD_FRAMERATE_DEFAULT_HZ), TASK_PRIORITY_LOW),
 403 #endif
 404
 405 #ifdef USE_TELEMETRY
 406     [TASK_TELEMETRY] = DEFINE_TASK("TELEMETRY", NULL, NULL, taskTelemetry, TASK_PERIOD_HZ(250), TASK_PRIORITY_LOW),
 407 #endif
 408
 409 #ifdef USE_LED_STRIP
 410     [TASK_LEDSTRIP] = DEFINE_TASK("LEDSTRIP", NULL, NULL, ledStripUpdate, TASK_PERIOD_HZ(TASK_LEDSTRIP_RATE_HZ), TASK_PRIORITY_LOW),
 411 #endif
 412
 413 #ifdef USE_BST
 414     [TASK_BST_MASTER_PROCESS] = DEFINE_TASK("BST_MASTER_PROCESS", NULL, NULL, taskBstMasterProcess, TASK_PERIOD_HZ(50), TASK_PRIORITY_LOWEST),
 415 #endif
 416
 417 #ifdef USE_ESC_SENSOR
 418     [TASK_ESC_SENSOR] = DEFINE_TASK("ESC_SENSOR", NULL, NULL, escSensorProcess, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW),
 419 #endif
 420
 421 #ifdef USE_CMS
 422     [TASK_CMS] = DEFINE_TASK("CMS", NULL, NULL, cmsHandler, TASK_PERIOD_HZ(20), TASK_PRIORITY_LOW),
 423 #endif
 424
 425 #ifdef USE_VTX_CONTROL
 426     [TASK_VTXCTRL] = DEFINE_TASK("VTXCTRL", NULL, NULL, vtxUpdate, TASK_PERIOD_HZ(5), TASK_PRIORITY_LOWEST),
 427 #endif
 428
 429 #ifdef USE_RCDEVICE
 430     [TASK_RCDEVICE] = DEFINE_TASK("RCDEVICE", NULL, NULL, rcdeviceUpdate, TASK_PERIOD_HZ(20), TASK_PRIORITY_MEDIUM),
 431 #endif
 432
 433 #ifdef USE_CAMERA_CONTROL
 434     [TASK_CAMCTRL] = DEFINE_TASK("CAMCTRL", NULL, NULL, taskCameraControl, TASK_PERIOD_HZ(5), TASK_PRIORITY_LOW),
 435 #endif
 436
 437 #ifdef USE_ADC_INTERNAL
 438     [TASK_ADC_INTERNAL] = DEFINE_TASK("ADCINTERNAL", NULL, NULL, adcInternalProcess, TASK_PERIOD_HZ(1), TASK_PRIORITY_LOWEST),
 439 #endif
 440
 441 #ifdef USE_PINIOBOX
 442     [TASK_PINIOBOX] = DEFINE_TASK("PINIOBOX", NULL, NULL, pinioBoxUpdate, TASK_PERIOD_HZ(20), TASK_PRIORITY_LOWEST),
 443 #endif
 444
 445 #ifdef USE_RANGEFINDER
 446     [TASK_RANGEFINDER] = DEFINE_TASK("RANGEFINDER", NULL, NULL, taskUpdateRangefinder, TASK_PERIOD_HZ(10), TASK_PRIORITY_LOWEST),
 447 #endif
 448
 449 #ifdef USE_CRSF_V3
 450     [TASK_SPEED_NEGOTIATION] = DEFINE_TASK("SPEED_NEGOTIATION", NULL, NULL, speedNegotiationProcess, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW),
 451 #endif
 452
 453 #ifdef USE_RC_STATS
 454     [TASK_RC_STATS] = DEFINE_TASK("RC_STATS", NULL, NULL, rcStatsUpdate, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW),
 455 #endif
 456
 457 };
 458
 459 task_t *getTask(unsigned taskId)
 460 {
 461     return &tasks[taskId];
 462 }
 463
 464 // Has to be done before tasksInit() in order to initialize any task data which may be uninitialized at boot
 465 void tasksInitData(void)
 466 {
 467     for (int i = 0; i < TASK_COUNT; i++) {
 468         tasks[i].attribute = &task_attributes[i];
 469     }
 470 }
 471
 472 void tasksInit(void)
 473 {
 474     schedulerInit();
 475
 476     setTaskEnabled(TASK_MAIN, true);
 477
 478     setTaskEnabled(TASK_SERIAL, true);
 479     rescheduleTask(TASK_SERIAL, TASK_PERIOD_HZ(serialConfig()->serial_update_rate_hz));
 480
 481     const bool useBatteryVoltage = batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE;
 482     setTaskEnabled(TASK_BATTERY_VOLTAGE, useBatteryVoltage);
 483
 484 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
 485     // If vbat motor output compensation is used, use fast vbat samplingTime
 486     if (isSagCompensationConfigured()) {
 487         rescheduleTask(TASK_BATTERY_VOLTAGE, TASK_PERIOD_HZ(FAST_VOLTAGE_TASK_FREQ_HZ));
 488     }
 489 #endif
 490
 491     const bool useBatteryCurrent = batteryConfig()->currentMeterSource != CURRENT_METER_NONE;
 492     setTaskEnabled(TASK_BATTERY_CURRENT, useBatteryCurrent);
 493     const bool useBatteryAlerts = batteryConfig()->useVBatAlerts || batteryConfig()->useConsumptionAlerts || featureIsEnabled(FEATURE_OSD);
 494     setTaskEnabled(TASK_BATTERY_ALERTS, (useBatteryVoltage || useBatteryCurrent) && useBatteryAlerts);
 495
 496 #ifdef USE_STACK_CHECK
 497     setTaskEnabled(TASK_STACK_CHECK, true);
 498 #endif
 499
 500     if (sensors(SENSOR_GYRO)) {
 501         rescheduleTask(TASK_GYRO, gyro.sampleLooptime);
 502         rescheduleTask(TASK_FILTER, gyro.targetLooptime);
 503         rescheduleTask(TASK_PID, gyro.targetLooptime);
 504         setTaskEnabled(TASK_GYRO, true);
 505         setTaskEnabled(TASK_FILTER, true);
 506         setTaskEnabled(TASK_PID, true);
 507         schedulerEnableGyro();
 508     }
 509
 510 #if defined(USE_ACC)
 511     if (sensors(SENSOR_ACC) && acc.sampleRateHz) {
 512         setTaskEnabled(TASK_ACCEL, true);
 513         rescheduleTask(TASK_ACCEL, TASK_PERIOD_HZ(acc.sampleRateHz));
 514         setTaskEnabled(TASK_ATTITUDE, true);
 515     }
 516 #endif
 517
 518 #ifdef USE_RANGEFINDER
 519     if (sensors(SENSOR_RANGEFINDER)) {
 520         setTaskEnabled(TASK_RANGEFINDER, featureIsEnabled(FEATURE_RANGEFINDER));
 521     }
 522 #endif
 523
 524     setTaskEnabled(TASK_RX, true);
 525
 526     setTaskEnabled(TASK_DISPATCH, dispatchIsEnabled());
 527
 528 #ifdef USE_BEEPER
 529     setTaskEnabled(TASK_BEEPER, true);
 530 #endif
 531
 532 #ifdef USE_GPS
 533     setTaskEnabled(TASK_GPS, featureIsEnabled(FEATURE_GPS));
 534 #endif
 535
 536 #ifdef USE_GPS_RESCUE
 537     setTaskEnabled(TASK_GPS_RESCUE, featureIsEnabled(FEATURE_GPS));
 538 #endif
 539
 540 #ifdef USE_MAG
 541     setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
 542 #endif
 543
 544 #ifdef USE_BARO
 545     setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
 546 #endif
 547
 548 #if defined(USE_BARO) || defined(USE_GPS)
 549     setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || featureIsEnabled(FEATURE_GPS));
 550 #endif
 551
 552 #ifdef USE_DASHBOARD
 553     setTaskEnabled(TASK_DASHBOARD, featureIsEnabled(FEATURE_DASHBOARD));
 554 #endif
 555
 556 #ifdef USE_TELEMETRY
 557     if (featureIsEnabled(FEATURE_TELEMETRY)) {
 558         setTaskEnabled(TASK_TELEMETRY, true);
 559         if (rxRuntimeState.serialrxProvider == SERIALRX_JETIEXBUS) {
 560             // Reschedule telemetry to 500hz for Jeti Exbus
 561             rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500));
 562         } else if (rxRuntimeState.serialrxProvider == SERIALRX_CRSF) {
 563             // Reschedule telemetry to 500hz, 2ms for CRSF
 564             rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500));
 565         }
 566     }
 567 #endif
 568
 569 #ifdef USE_LED_STRIP
 570     setTaskEnabled(TASK_LEDSTRIP, featureIsEnabled(FEATURE_LED_STRIP));
 571 #endif
 572
 573 #ifdef USE_TRANSPONDER
 574     setTaskEnabled(TASK_TRANSPONDER, featureIsEnabled(FEATURE_TRANSPONDER));
 575 #endif
 576
 577 #ifdef USE_OSD
 578     rescheduleTask(TASK_OSD, TASK_PERIOD_HZ(osdConfig()->framerate_hz));
 579     setTaskEnabled(TASK_OSD, featureIsEnabled(FEATURE_OSD) && osdGetDisplayPort(NULL));
 580 #endif
 581
 582 #ifdef USE_BST
 583     setTaskEnabled(TASK_BST_MASTER_PROCESS, true);
 584 #endif
 585
 586 #ifdef USE_ESC_SENSOR
 587     setTaskEnabled(TASK_ESC_SENSOR, featureIsEnabled(FEATURE_ESC_SENSOR));
 588 #endif
 589
 590 #ifdef USE_ADC_INTERNAL
 591     setTaskEnabled(TASK_ADC_INTERNAL, true);
 592 #endif
 593
 594 #ifdef USE_PINIOBOX
 595     pinioBoxTaskControl();
 596 #endif
 597
 598 #ifdef USE_CMS
 599 #ifdef USE_MSP_DISPLAYPORT
 600     setTaskEnabled(TASK_CMS, true);
 601 #else
 602     setTaskEnabled(TASK_CMS, featureIsEnabled(FEATURE_OSD) || featureIsEnabled(FEATURE_DASHBOARD));
 603 #endif
 604 #endif
 605
 606 #ifdef USE_VTX_CONTROL
 607 #if defined(USE_VTX_RTC6705) || defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP)
 608     setTaskEnabled(TASK_VTXCTRL, true);
 609 #endif
 610 #endif
 611
 612 #ifdef USE_CAMERA_CONTROL
 613     setTaskEnabled(TASK_CAMCTRL, true);
 614 #endif
 615
 616 #ifdef USE_RCDEVICE
 617     setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled());
 618 #endif
 619
 620 #ifdef USE_CRSF_V3
 621     const bool useCRSF = rxRuntimeState.serialrxProvider == SERIALRX_CRSF;
 622     setTaskEnabled(TASK_SPEED_NEGOTIATION, useCRSF);
 623 #endif
 624
 625 #ifdef SIMULATOR_MULTITHREAD
 626     rescheduleTask(TASK_RX, 1);
 627 #endif
 628
 629 #ifdef USE_RC_STATS
 630     setTaskEnabled(TASK_RC_STATS, true);
 631 #endif
 632 }