src/main/fc/fc_tasks.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
  24 #include "cms/cms.h"
  25
  26 #include "common/axis.h"
  27 #include "common/color.h"
  28 #include "common/utils.h"
  29 #include "programming/programming_task.h"
  30
  31 #include "drivers/accgyro/accgyro.h"
  32 #include "drivers/compass/compass.h"
  33 #include "drivers/sensor.h"
  34 #include "drivers/serial.h"
  35 #include "drivers/stack_check.h"
  36 #include "drivers/pwm_mapping.h"
  37
  38 #include "fc/cli.h"
  39 #include "fc/config.h"
  40 #include "fc/fc_core.h"
  41 #include "fc/fc_msp.h"
  42 #include "fc/fc_tasks.h"
  43 #include "fc/rc_controls.h"
  44 #include "fc/runtime_config.h"
  45
  46 #include "flight/imu.h"
  47 #include "flight/mixer.h"
  48 #include "flight/pid.h"
  49 #include "flight/wind_estimator.h"
  50 #include "flight/rpm_filter.h"
  51 #include "flight/servos.h"
  52
  53 #include "navigation/navigation.h"
  54
  55 #include "io/beeper.h"
  56 #include "io/lights.h"
  57 #include "io/dashboard.h"
  58 #include "io/gps.h"
  59 #include "io/ledstrip.h"
  60 #include "io/osd.h"
  61 #include "io/pwmdriver_i2c.h"
  62 #include "io/serial.h"
  63 #include "io/rcdevice_cam.h"
  64 #include "io/vtx.h"
  65 #include "io/osd_dji_hd.h"
  66 #include "io/servo_sbus.h"
  67
  68 #include "msp/msp_serial.h"
  69
  70 #include "rx/rx.h"
  71 #include "rx/eleres.h"
  72 #include "rx/rx_spi.h"
  73
  74 #include "scheduler/scheduler.h"
  75
  76 #include "sensors/sensors.h"
  77 #include "sensors/acceleration.h"
  78 #include "sensors/temperature.h"
  79 #include "sensors/barometer.h"
  80 #include "sensors/battery.h"
  81 #include "sensors/compass.h"
  82 #include "sensors/gyro.h"
  83 #include "sensors/irlock.h"
  84 #include "sensors/pitotmeter.h"
  85 #include "sensors/rangefinder.h"
  86 #include "sensors/opflow.h"
  87
  88 #include "telemetry/telemetry.h"
  89
  90 #include "config/feature.h"
  91
  92 #include "uav_interconnect/uav_interconnect.h"
  93
  94 void taskHandleSerial(timeUs_t currentTimeUs)
  95 {
  96     UNUSED(currentTimeUs);
  97     // in cli mode, all serial stuff goes to here. enter cli mode by sending #
  98     if (cliMode) {
  99         cliProcess();
 100     }
 101
 102     // Allow MSP processing even if in CLI mode
 103     mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand);
 104
 105 #if defined(USE_DJI_HD_OSD)
 106     // DJI OSD uses a special flavour of MSP (subset of Betaflight 4.1.1 MSP) - process as part of serial task
 107     djiOsdSerialProcess();
 108 #endif
 109 }
 110
 111 void taskUpdateBattery(timeUs_t currentTimeUs)
 112 {
 113     static timeUs_t batMonitoringLastServiced = 0;
 114     timeUs_t BatMonitoringTimeSinceLastServiced = cmpTimeUs(currentTimeUs, batMonitoringLastServiced);
 115
 116     if (isAmperageConfigured())
 117         currentMeterUpdate(BatMonitoringTimeSinceLastServiced);
 118 #ifdef USE_ADC
 119     if (feature(FEATURE_VBAT))
 120         batteryUpdate(BatMonitoringTimeSinceLastServiced);
 121     if (feature(FEATURE_VBAT) && isAmperageConfigured()) {
 122         powerMeterUpdate(BatMonitoringTimeSinceLastServiced);
 123         sagCompensatedVBatUpdate(currentTimeUs, BatMonitoringTimeSinceLastServiced);
 124     }
 125 #endif
 126     batMonitoringLastServiced = currentTimeUs;
 127 }
 128
 129 void taskUpdateTemperature(timeUs_t currentTimeUs)
 130 {
 131     UNUSED(currentTimeUs);
 132     temperatureUpdate();
 133 }
 134
 135 #ifdef USE_GPS
 136 void taskProcessGPS(timeUs_t currentTimeUs)
 137 {
 138     // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
 139     // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
 140     // change this based on available hardware
 141     if (feature(FEATURE_GPS)) {
 142         if (gpsUpdate()) {
 143 #ifdef USE_WIND_ESTIMATOR
 144             updateWindEstimator(currentTimeUs);
 145 #endif
 146         }
 147     }
 148
 149     if (sensors(SENSOR_GPS)) {
 150         updateGpsIndicator(currentTimeUs);
 151     }
 152 }
 153 #endif
 154
 155 #ifdef USE_MAG
 156 void taskUpdateCompass(timeUs_t currentTimeUs)
 157 {
 158     if (sensors(SENSOR_MAG)) {
 159         compassUpdate(currentTimeUs);
 160     }
 161 }
 162 #endif
 163
 164 #ifdef USE_BARO
 165 void taskUpdateBaro(timeUs_t currentTimeUs)
 166 {
 167     if (!sensors(SENSOR_BARO)) {
 168         return;
 169     }
 170
 171     const uint32_t newDeadline = baroUpdate();
 172     if (newDeadline != 0) {
 173         rescheduleTask(TASK_SELF, newDeadline);
 174     }
 175
 176     updatePositionEstimator_BaroTopic(currentTimeUs);
 177 }
 178 #endif
 179
 180 #ifdef USE_PITOT
 181 void taskUpdatePitot(timeUs_t currentTimeUs)
 182 {
 183     if (!sensors(SENSOR_PITOT)) {
 184         return;
 185     }
 186
 187     pitotUpdate();
 188     updatePositionEstimator_PitotTopic(currentTimeUs);
 189 }
 190 #endif
 191
 192 #ifdef USE_RANGEFINDER
 193 void taskUpdateRangefinder(timeUs_t currentTimeUs)
 194 {
 195     UNUSED(currentTimeUs);
 196
 197     if (!sensors(SENSOR_RANGEFINDER))
 198         return;
 199
 200     // Update and adjust task to update at required rate
 201     const uint32_t newDeadline = rangefinderUpdate();
 202     if (newDeadline != 0) {
 203         rescheduleTask(TASK_SELF, newDeadline);
 204     }
 205
 206     /*
 207      * Process raw rangefinder readout
 208      */
 209     if (rangefinderProcess(calculateCosTiltAngle())) {
 210         updatePositionEstimator_SurfaceTopic(currentTimeUs, rangefinderGetLatestAltitude());
 211     }
 212 }
 213 #endif
 214
 215 #if defined(USE_NAV) && defined(USE_IRLOCK)
 216 void taskUpdateIrlock(timeUs_t currentTimeUs)
 217 {
 218     UNUSED(currentTimeUs);
 219     irlockUpdate();
 220 }
 221 #endif
 222
 223 #ifdef USE_OPFLOW
 224 void taskUpdateOpticalFlow(timeUs_t currentTimeUs)
 225 {
 226     if (!sensors(SENSOR_OPFLOW))
 227         return;
 228
 229     opflowUpdate(currentTimeUs);
 230     updatePositionEstimator_OpticalFlowTopic(currentTimeUs);
 231 }
 232 #endif
 233
 234 #ifdef USE_DASHBOARD
 235 void taskDashboardUpdate(timeUs_t currentTimeUs)
 236 {
 237     if (feature(FEATURE_DASHBOARD)) {
 238         dashboardUpdate(currentTimeUs);
 239     }
 240 }
 241 #endif
 242
 243 #ifdef USE_TELEMETRY
 244 void taskTelemetry(timeUs_t currentTimeUs)
 245 {
 246     telemetryCheckState();
 247
 248     if (!cliMode && feature(FEATURE_TELEMETRY)) {
 249         telemetryProcess(currentTimeUs);
 250     }
 251 }
 252 #endif
 253
 254 #ifdef USE_LED_STRIP
 255 void taskLedStrip(timeUs_t currentTimeUs)
 256 {
 257     if (feature(FEATURE_LED_STRIP)) {
 258         ledStripUpdate(currentTimeUs);
 259     }
 260 }
 261 #endif
 262
 263 void taskSyncServoDriver(timeUs_t currentTimeUs)
 264 {
 265     UNUSED(currentTimeUs);
 266
 267 #if defined(USE_SERVO_SBUS)
 268     sbusServoSendUpdate();
 269 #endif
 270
 271 #ifdef USE_PWM_SERVO_DRIVER
 272     pwmDriverSync();
 273 #endif
 274 }
 275
 276 #ifdef USE_OSD
 277 void taskUpdateOsd(timeUs_t currentTimeUs)
 278 {
 279     if (feature(FEATURE_OSD)) {
 280         osdUpdate(currentTimeUs);
 281     }
 282 }
 283 #endif
 284
 285 void fcTasksInit(void)
 286 {
 287     schedulerInit();
 288
 289     rescheduleTask(TASK_GYROPID, getLooptime());
 290     setTaskEnabled(TASK_GYROPID, true);
 291
 292     setTaskEnabled(TASK_SERIAL, true);
 293 #ifdef BEEPER
 294     setTaskEnabled(TASK_BEEPER, true);
 295 #endif
 296 #ifdef USE_LIGHTS
 297     setTaskEnabled(TASK_LIGHTS, true);
 298 #endif
 299     setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || isAmperageConfigured());
 300     setTaskEnabled(TASK_TEMPERATURE, true);
 301     setTaskEnabled(TASK_RX, true);
 302 #ifdef USE_GPS
 303     setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
 304 #endif
 305 #ifdef USE_MAG
 306     setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
 307 #if defined(USE_MAG_MPU9250)
 308     // fixme temporary solution for AK6983 via slave I2C on MPU9250
 309     rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40));
 310 #endif
 311 #endif
 312 #ifdef USE_BARO
 313     setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
 314 #endif
 315 #ifdef USE_PITOT
 316     setTaskEnabled(TASK_PITOT, sensors(SENSOR_PITOT));
 317 #endif
 318 #ifdef USE_RANGEFINDER
 319     setTaskEnabled(TASK_RANGEFINDER, sensors(SENSOR_RANGEFINDER));
 320 #endif
 321 #ifdef USE_DASHBOARD
 322     setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD));
 323 #endif
 324 #ifdef USE_TELEMETRY
 325     setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
 326 #endif
 327 #ifdef USE_LED_STRIP
 328     setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
 329 #endif
 330 #ifdef STACK_CHECK
 331     setTaskEnabled(TASK_STACK_CHECK, true);
 332 #endif
 333 #if defined(USE_PWM_SERVO_DRIVER) || defined(USE_SERVO_SBUS)
 334     setTaskEnabled(TASK_PWMDRIVER, (servoConfig()->servo_protocol == SERVO_TYPE_SERVO_DRIVER) || (servoConfig()->servo_protocol == SERVO_TYPE_SBUS));
 335 #endif
 336 #ifdef USE_CMS
 337 #ifdef USE_MSP_DISPLAYPORT
 338     setTaskEnabled(TASK_CMS, true);
 339 #else
 340     setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD));
 341 #endif
 342 #endif
 343 #ifdef USE_OPFLOW
 344     setTaskEnabled(TASK_OPFLOW, sensors(SENSOR_OPFLOW));
 345 #endif
 346 #ifdef USE_VTX_CONTROL
 347 #if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
 348     setTaskEnabled(TASK_VTXCTRL, true);
 349 #endif
 350 #endif
 351 #ifdef USE_UAV_INTERCONNECT
 352     setTaskEnabled(TASK_UAV_INTERCONNECT, uavInterconnectBusIsInitialized());
 353 #endif
 354 #ifdef USE_RCDEVICE
 355     setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled());
 356 #endif
 357 #ifdef USE_PROGRAMMING_FRAMEWORK
 358     setTaskEnabled(TASK_PROGRAMMING_FRAMEWORK, true);
 359 #endif
 360 #ifdef USE_IRLOCK
 361     setTaskEnabled(TASK_IRLOCK, irlockHasBeenDetected());
 362 #endif
 363 }
 364
 365 cfTask_t cfTasks[TASK_COUNT] = {
 366     [TASK_SYSTEM] = {
 367         .taskName = "SYSTEM",
 368         .taskFunc = taskSystem,
 369         .desiredPeriod = TASK_PERIOD_HZ(10),              // run every 100 ms, 10Hz
 370         .staticPriority = TASK_PRIORITY_HIGH,
 371     },
 372     [TASK_GYROPID] = {
 373         .taskName = "GYRO/PID",
 374         .taskFunc = taskMainPidLoop,
 375         .desiredPeriod = TASK_PERIOD_US(1000),
 376         .staticPriority = TASK_PRIORITY_REALTIME,
 377     },
 378     [TASK_SERIAL] = {
 379         .taskName = "SERIAL",
 380         .taskFunc = taskHandleSerial,
 381         .desiredPeriod = TASK_PERIOD_HZ(100),     // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud
 382         .staticPriority = TASK_PRIORITY_LOW,
 383     },
 384
 385 #ifdef BEEPER
 386     [TASK_BEEPER] = {
 387         .taskName = "BEEPER",
 388         .taskFunc = beeperUpdate,
 389         .desiredPeriod = TASK_PERIOD_HZ(100),     // 100 Hz
 390         .staticPriority = TASK_PRIORITY_MEDIUM,
 391     },
 392 #endif
 393
 394 #ifdef USE_LIGHTS
 395     [TASK_LIGHTS] = {
 396         .taskName = "LIGHTS",
 397         .taskFunc = lightsUpdate,
 398         .desiredPeriod = TASK_PERIOD_HZ(100),     // 100 Hz
 399         .staticPriority = TASK_PRIORITY_LOW,
 400     },
 401 #endif
 402
 403     [TASK_BATTERY] = {
 404         .taskName = "BATTERY",
 405         .taskFunc = taskUpdateBattery,
 406         .desiredPeriod = TASK_PERIOD_HZ(50),      // 50 Hz
 407         .staticPriority = TASK_PRIORITY_MEDIUM,
 408     },
 409
 410     [TASK_TEMPERATURE] = {
 411         .taskName = "TEMPERATURE",
 412         .taskFunc = taskUpdateTemperature,
 413         .desiredPeriod = TASK_PERIOD_HZ(100),     // 100 Hz
 414         .staticPriority = TASK_PRIORITY_LOW,
 415     },
 416
 417     [TASK_RX] = {
 418         .taskName = "RX",
 419         .checkFunc = taskUpdateRxCheck,
 420         .taskFunc = taskUpdateRxMain,
 421         .desiredPeriod = TASK_PERIOD_HZ(50),      // If event-based scheduling doesn't work, fallback to periodic scheduling
 422         .staticPriority = TASK_PRIORITY_HIGH,
 423     },
 424
 425 #ifdef USE_GPS
 426     [TASK_GPS] = {
 427         .taskName = "GPS",
 428         .taskFunc = taskProcessGPS,
 429         .desiredPeriod = TASK_PERIOD_HZ(50),      // GPS usually don't go raster than 10Hz
 430         .staticPriority = TASK_PRIORITY_MEDIUM,
 431     },
 432 #endif
 433
 434 #ifdef USE_MAG
 435     [TASK_COMPASS] = {
 436         .taskName = "COMPASS",
 437         .taskFunc = taskUpdateCompass,
 438         .desiredPeriod = TASK_PERIOD_HZ(10),      // Compass is updated at 10 Hz
 439         .staticPriority = TASK_PRIORITY_MEDIUM,
 440     },
 441 #endif
 442
 443 #ifdef USE_BARO
 444     [TASK_BARO] = {
 445         .taskName = "BARO",
 446         .taskFunc = taskUpdateBaro,
 447         .desiredPeriod = TASK_PERIOD_HZ(20),
 448         .staticPriority = TASK_PRIORITY_MEDIUM,
 449     },
 450 #endif
 451
 452 #ifdef USE_PITOT
 453     [TASK_PITOT] = {
 454         .taskName = "PITOT",
 455         .taskFunc = taskUpdatePitot,
 456         .desiredPeriod = TASK_PERIOD_HZ(100),
 457         .staticPriority = TASK_PRIORITY_MEDIUM,
 458     },
 459 #endif
 460
 461 #ifdef USE_RANGEFINDER
 462     [TASK_RANGEFINDER] = {
 463         .taskName = "RANGEFINDER",
 464         .taskFunc = taskUpdateRangefinder,
 465         .desiredPeriod = TASK_PERIOD_MS(70),
 466         .staticPriority = TASK_PRIORITY_MEDIUM,
 467     },
 468 #endif
 469
 470 #ifdef USE_IRLOCK
 471     [TASK_IRLOCK] = {
 472         .taskName = "IRLOCK",
 473         .taskFunc = taskUpdateIrlock,
 474         .desiredPeriod = TASK_PERIOD_HZ(100),
 475         .staticPriority = TASK_PRIORITY_MEDIUM,
 476     },
 477 #endif
 478
 479 #ifdef USE_DASHBOARD
 480     [TASK_DASHBOARD] = {
 481         .taskName = "DASHBOARD",
 482         .taskFunc = taskDashboardUpdate,
 483         .desiredPeriod = TASK_PERIOD_HZ(10),
 484         .staticPriority = TASK_PRIORITY_LOW,
 485     },
 486 #endif
 487
 488 #ifdef USE_TELEMETRY
 489     [TASK_TELEMETRY] = {
 490         .taskName = "TELEMETRY",
 491         .taskFunc = taskTelemetry,
 492         .desiredPeriod = TASK_PERIOD_HZ(500),         // 500 Hz
 493         .staticPriority = TASK_PRIORITY_IDLE,
 494     },
 495 #endif
 496
 497 #ifdef USE_LED_STRIP
 498     [TASK_LEDSTRIP] = {
 499         .taskName = "LEDSTRIP",
 500         .taskFunc = taskLedStrip,
 501         .desiredPeriod = TASK_PERIOD_HZ(100),         // 100 Hz
 502         .staticPriority = TASK_PRIORITY_IDLE,
 503     },
 504 #endif
 505
 506 #if defined(USE_PWM_SERVO_DRIVER) || defined(USE_SERVO_SBUS)
 507     [TASK_PWMDRIVER] = {
 508         .taskName = "SERVOS",
 509         .taskFunc = taskSyncServoDriver,
 510         .desiredPeriod = TASK_PERIOD_HZ(200),         // 200 Hz
 511         .staticPriority = TASK_PRIORITY_HIGH,
 512     },
 513 #endif
 514
 515 #ifdef STACK_CHECK
 516     [TASK_STACK_CHECK] = {
 517         .taskName = "STACKCHECK",
 518         .taskFunc = taskStackCheck,
 519         .desiredPeriod = TASK_PERIOD_HZ(10),          // 10 Hz
 520         .staticPriority = TASK_PRIORITY_IDLE,
 521     },
 522 #endif
 523
 524 #ifdef USE_OSD
 525     [TASK_OSD] = {
 526         .taskName = "OSD",
 527         .taskFunc = taskUpdateOsd,
 528         .desiredPeriod = TASK_PERIOD_HZ(250),
 529         .staticPriority = TASK_PRIORITY_LOW,
 530     },
 531 #endif
 532
 533 #ifdef USE_CMS
 534     [TASK_CMS] = {
 535         .taskName = "CMS",
 536         .taskFunc = cmsHandler,
 537         .desiredPeriod = TASK_PERIOD_HZ(50),
 538         .staticPriority = TASK_PRIORITY_LOW,
 539     },
 540 #endif
 541
 542 #ifdef USE_OPFLOW
 543     [TASK_OPFLOW] = {
 544         .taskName = "OPFLOW",
 545         .taskFunc = taskUpdateOpticalFlow,
 546         .desiredPeriod = TASK_PERIOD_HZ(100),   // I2C/SPI sensor will work at higher rate and accumulate, UIB/UART sensor will work at lower rate w/o accumulation
 547         .staticPriority = TASK_PRIORITY_MEDIUM,
 548     },
 549 #endif
 550
 551 #ifdef USE_UAV_INTERCONNECT
 552     [TASK_UAV_INTERCONNECT] = {
 553         .taskName = "UIB",
 554         .taskFunc = uavInterconnectBusTask,
 555         .desiredPeriod = 1000000 / 500,          // 500 Hz
 556         .staticPriority = TASK_PRIORITY_MEDIUM,
 557     },
 558 #endif
 559
 560 #ifdef USE_RCDEVICE
 561     [TASK_RCDEVICE] = {
 562         .taskName = "RCDEVICE",
 563         .taskFunc = rcdeviceUpdate,
 564         .desiredPeriod = TASK_PERIOD_HZ(10),        // 10 Hz, 100ms
 565         .staticPriority = TASK_PRIORITY_MEDIUM,
 566     },
 567 #endif
 568
 569 #if defined(USE_VTX_CONTROL)
 570     [TASK_VTXCTRL] = {
 571         .taskName = "VTXCTRL",
 572         .taskFunc = vtxUpdate,
 573         .desiredPeriod = TASK_PERIOD_HZ(5),          // 5Hz @200msec
 574         .staticPriority = TASK_PRIORITY_IDLE,
 575     },
 576 #endif
 577 #ifdef USE_PROGRAMMING_FRAMEWORK
 578     [TASK_PROGRAMMING_FRAMEWORK] = {
 579         .taskName = "PROGRAMMING",
 580         .taskFunc = programmingFrameworkUpdateTask,
 581         .desiredPeriod = TASK_PERIOD_HZ(10),          // 10Hz @100msec
 582         .staticPriority = TASK_PRIORITY_IDLE,
 583     },
 584 #endif
 585 #ifdef USE_RPM_FILTER
 586     [TASK_RPM_FILTER] = {
 587         .taskName = "RPM",
 588         .taskFunc = rpmFilterUpdateTask,
 589         .desiredPeriod = TASK_PERIOD_HZ(RPM_FILTER_UPDATE_RATE_HZ),          // 300Hz @3,33ms
 590         .staticPriority = TASK_PRIORITY_LOW,
 591     },
 592 #endif
 593 };