src/main/fc/fc_init.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 <stdint.h>
  20 #include <string.h>
  21
  22 #include "platform.h"
  23
  24 #include "blackbox/blackbox.h"
  25 #include "blackbox/blackbox_io.h"
  26
  27 #include "build/assert.h"
  28 #include "build/atomic.h"
  29 #include "build/build_config.h"
  30 #include "build/debug.h"
  31
  32 #include "common/axis.h"
  33 #include "common/color.h"
  34 #include "common/log.h"
  35 #include "common/maths.h"
  36 #include "common/memory.h"
  37 #include "common/printf.h"
  38 #include "programming/global_variables.h"
  39
  40 #include "config/config_eeprom.h"
  41 #include "config/feature.h"
  42 #include "config/parameter_group.h"
  43 #include "config/parameter_group_ids.h"
  44
  45 #include "cms/cms.h"
  46
  47 #include "drivers/1-wire.h"
  48 #include "drivers/accgyro/accgyro.h"
  49 #include "drivers/adc.h"
  50 #include "drivers/compass/compass.h"
  51 #include "drivers/bus.h"
  52 #include "drivers/dma.h"
  53 #include "drivers/exti.h"
  54 #include "drivers/io.h"
  55 #include "drivers/flash.h"
  56 #include "drivers/gimbal_common.h"
  57 #include "drivers/headtracker_common.h"
  58 #include "drivers/light_led.h"
  59 #include "drivers/nvic.h"
  60 #include "drivers/osd.h"
  61 #include "drivers/persistent.h"
  62 #include "drivers/pwm_esc_detect.h"
  63 #include "drivers/pwm_mapping.h"
  64 #include "drivers/pwm_output.h"
  65 #include "drivers/sensor.h"
  66 #include "drivers/serial.h"
  67 #include "drivers/serial_softserial.h"
  68 #include "drivers/serial_uart.h"
  69 #include "drivers/serial_usb_vcp.h"
  70 #include "drivers/sound_beeper.h"
  71 #include "drivers/system.h"
  72 #include "drivers/time.h"
  73 #include "drivers/timer.h"
  74 #include "drivers/uart_inverter.h"
  75 #include "drivers/io.h"
  76 #include "drivers/vtx_common.h"
  77 #ifdef USE_USB_MSC
  78 #include "drivers/usb_msc.h"
  79 #include "msc/emfat_file.h"
  80 #endif
  81 #include "drivers/sdcard/sdcard.h"
  82 #include "drivers/sdio.h"
  83 #include "drivers/io_port_expander.h"
  84
  85 #include "fc/cli.h"
  86 #include "fc/config.h"
  87 #include "fc/fc_msp.h"
  88 #include "fc/fc_tasks.h"
  89 #include "fc/rc_controls.h"
  90 #include "fc/runtime_config.h"
  91 #include "fc/firmware_update.h"
  92
  93 #include "flight/failsafe.h"
  94 #include "flight/imu.h"
  95 #include "flight/mixer.h"
  96 #include "flight/pid.h"
  97 #include "flight/power_limits.h"
  98 #include "flight/rpm_filter.h"
  99 #include "flight/servos.h"
 100 #include "flight/ez_tune.h"
 101
 102 #include "io/asyncfatfs/asyncfatfs.h"
 103 #include "io/beeper.h"
 104 #include "io/lights.h"
 105 #include "io/dashboard.h"
 106 #include "io/displayport_frsky_osd.h"
 107 #include "io/displayport_msp.h"
 108 #include "io/displayport_max7456.h"
 109 #include "io/displayport_msp_osd.h"
 110 #include "io/displayport_srxl.h"
 111 #include "io/flashfs.h"
 112 #include "io/gimbal_serial.h"
 113 #include "io/headtracker_msp.h"
 114 #include "io/gps.h"
 115 #include "io/ledstrip.h"
 116 #include "io/osd.h"
 117 #include "io/osd_dji_hd.h"
 118 #include "io/rcdevice_cam.h"
 119 #include "io/serial.h"
 120 #include "io/displayport_msp.h"
 121 #include "io/smartport_master.h"
 122 #include "io/vtx.h"
 123 #include "io/vtx_control.h"
 124 #include "io/vtx_smartaudio.h"
 125 #include "io/vtx_tramp.h"
 126 #include "io/vtx_msp.h"
 127 #include "io/vtx_ffpv24g.h"
 128 #include "io/piniobox.h"
 129
 130 #include "msp/msp_serial.h"
 131
 132 #include "navigation/navigation.h"
 133
 134 #include "rx/rx.h"
 135 #include "rx/spektrum.h"
 136
 137 #include "sensors/acceleration.h"
 138 #include "sensors/barometer.h"
 139 #include "sensors/battery.h"
 140 #include "sensors/boardalignment.h"
 141 #include "sensors/compass.h"
 142 #include "sensors/gyro.h"
 143 #include "sensors/initialisation.h"
 144 #include "sensors/pitotmeter.h"
 145 #include "sensors/rangefinder.h"
 146 #include "sensors/sensors.h"
 147 #include "sensors/esc_sensor.h"
 148
 149 #include "scheduler/scheduler.h"
 150
 151 #include "telemetry/telemetry.h"
 152
 153 #if defined(SITL_BUILD)
 154 #include "target/SITL/serial_proxy.h"
 155 #endif
 156
 157 #ifdef USE_HARDWARE_REVISION_DETECTION
 158 #include "hardware_revision.h"
 159 #endif
 160
 161 #ifdef USE_HARDWARE_PREBOOT_SETUP
 162 extern void initialisePreBootHardware(void);
 163 #endif
 164
 165 extern uint8_t motorControlEnable;
 166
 167 typedef enum {
 168     SYSTEM_STATE_INITIALISING   = 0,
 169     SYSTEM_STATE_CONFIG_LOADED  = (1 << 0),
 170     SYSTEM_STATE_SENSORS_READY  = (1 << 1),
 171     SYSTEM_STATE_MOTORS_READY   = (1 << 2),
 172     SYSTEM_STATE_TRANSPONDER_ENABLED = (1 << 3),
 173     SYSTEM_STATE_READY          = (1 << 7)
 174 } systemState_e;
 175
 176 uint8_t systemState = SYSTEM_STATE_INITIALISING;
 177
 178 void flashLedsAndBeep(void)
 179 {
 180     LED1_ON;
 181     LED0_OFF;
 182     for (uint8_t i = 0; i < 10; i++) {
 183         LED1_TOGGLE;
 184         LED0_TOGGLE;
 185         delay(25);
 186         if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1))))
 187             BEEP_ON;
 188         delay(25);
 189         BEEP_OFF;
 190     }
 191     LED0_OFF;
 192     LED1_OFF;
 193 }
 194
 195 void init(void)
 196 {
 197 #if defined(USE_FLASHFS)
 198     bool flashDeviceInitialized = false;
 199 #endif
 200
 201 #ifdef USE_HAL_DRIVER
 202     HAL_Init();
 203 #endif
 204
 205     systemState = SYSTEM_STATE_INITIALISING;
 206     printfSupportInit();
 207
 208     // Initialize system and CPU clocks to their initial values
 209     systemInit();
 210
 211 #if !defined(SITL_BUILD)
 212     __enable_irq();
 213 #endif
 214
 215     // initialize IO (needed for all IO operations)
 216     IOInitGlobal();
 217
 218 #ifdef USE_HARDWARE_REVISION_DETECTION
 219     detectHardwareRevision();
 220 #endif
 221
 222 #ifdef USE_BRUSHED_ESC_AUTODETECT
 223     detectBrushedESC();
 224 #endif
 225
 226 #ifdef CONFIG_IN_EXTERNAL_FLASH
 227     // Reset config to defaults. Note: Default flash config must be functional for config in external flash to work.
 228     pgResetAll(0);
 229
 230     flashDeviceInitialized = flashInit();
 231 #endif
 232
 233 #if defined(SITL_BUILD)
 234     serialProxyInit();
 235 #endif
 236
 237     initEEPROM();
 238     ensureEEPROMContainsValidData();
 239     suspendRxSignal();
 240     readEEPROM();
 241     resumeRxSignal();
 242
 243 #ifdef USE_I2C
 244     i2cSetSpeed(systemConfig()->i2c_speed);
 245 #endif
 246
 247 #ifdef USE_HARDWARE_PREBOOT_SETUP
 248     initialisePreBootHardware();
 249 #endif
 250
 251     systemState |= SYSTEM_STATE_CONFIG_LOADED;
 252
 253     debugMode = systemConfig()->debug_mode;
 254
 255     // Latch active features to be used for feature() in the remainder of init().
 256     latchActiveFeatures();
 257
 258     ledInit(false);
 259 #if !defined(SITL_BUILD)
 260     EXTIInit();
 261 #endif
 262
 263 #if defined(USE_SPEKTRUM_BIND) && defined(USE_SERIALRX_SPEKTRUM)
 264     if (rxConfig()->receiverType == RX_TYPE_SERIAL) {
 265         switch (rxConfig()->serialrx_provider) {
 266             case SERIALRX_SPEKTRUM1024:
 267             case SERIALRX_SPEKTRUM2048:
 268                 // Spektrum satellite binding if enabled on startup.
 269                 // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
 270                 // The rest of Spektrum initialization will happen later - via spektrumInit()
 271                 spektrumBind(rxConfigMutable());
 272                 break;
 273         }
 274     }
 275 #endif
 276
 277 #ifdef USE_VCP
 278     // Early initialize USB hardware
 279     usbVcpInitHardware();
 280 #endif
 281
 282     timerInit();  // timer must be initialized before any channel is allocated
 283
 284     serialInit(feature(FEATURE_SOFTSERIAL));
 285
 286     // Initialize MSP serial ports here so LOG can share a port with MSP.
 287     // XXX: Don't call mspFcInit() yet, since it initializes the boxes and needs
 288     // to run after the sensors have been detected.
 289     mspSerialInit();
 290
 291 #if defined(USE_DJI_HD_OSD)
 292     // DJI OSD uses a special flavour of MSP (subset of Betaflight 4.1.1 MSP) - process as part of serial task
 293     djiOsdSerialInit();
 294 #endif
 295
 296 #if defined(USE_SMARTPORT_MASTER)
 297     smartportMasterInit();
 298 #endif
 299
 300 #if defined(USE_LOG)
 301     // LOG might use serial output, so we only can init it after serial port is ready
 302     // From this point on we can use LOG_*() to produce real-time debugging information
 303     logInit();
 304 #endif
 305
 306 #ifdef USE_PROGRAMMING_FRAMEWORK
 307     gvInit();
 308 #endif
 309
 310     // Initialize servo and motor mixers
 311     // This needs to be called early to set up platform type correctly and count required motors & servos
 312     mixerConfigInit();
 313
 314     // Some sanity checking
 315     if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) {
 316         featureClear(FEATURE_REVERSIBLE_MOTORS);
 317     }
 318     if (!STATE(ALTITUDE_CONTROL)) {
 319         featureClear(FEATURE_AIRMODE);
 320     }
 321 #if !defined(SITL_BUILD)
 322     // Initialize motor and servo outpus
 323     if (pwmMotorAndServoInit()) {
 324         DISABLE_ARMING_FLAG(ARMING_DISABLED_PWM_OUTPUT_ERROR);
 325     }
 326     else {
 327         ENABLE_ARMING_FLAG(ARMING_DISABLED_PWM_OUTPUT_ERROR);
 328     }
 329 #else
 330     DISABLE_ARMING_FLAG(ARMING_DISABLED_PWM_OUTPUT_ERROR);
 331 #endif
 332     systemState |= SYSTEM_STATE_MOTORS_READY;
 333
 334 #ifdef USE_ESC_SENSOR
 335     // DSHOT supports a dedicated wire ESC telemetry. Kick off the ESC-sensor receiver initialization
 336     // We may, however, do listen_only, so need to init this anyway
 337     // Initialize escSensor after having done it with outputs
 338     escSensorInitialize();
 339 #endif
 340
 341 #ifdef BEEPER
 342     beeperDevConfig_t beeperDevConfig = {
 343         .ioTag = IO_TAG(BEEPER),
 344 #ifdef BEEPER_INVERTED
 345         .isOD = false,
 346         .isInverted = true
 347 #else
 348         .isOD = true,
 349         .isInverted = false
 350 #endif
 351     };
 352
 353     beeperInit(&beeperDevConfig);
 354 #endif
 355 #ifdef USE_LIGHTS
 356     lightsInit();
 357 #endif
 358
 359 #ifdef USE_UART_INVERTER
 360     uartInverterInit();
 361 #endif
 362
 363     // Initialize buses
 364     busInit();
 365
 366 #ifdef CONFIG_IN_EXTERNAL_FLASH
 367     // busInit re-configures the SPI pins. Init flash again so it is ready to write settings
 368     flashDeviceInitialized = flashInit();
 369 #endif
 370
 371 #ifdef USE_HARDWARE_REVISION_DETECTION
 372     updateHardwareRevision();
 373 #endif
 374
 375 #if defined(USE_SDCARD_SDIO) && (defined(STM32H7) || defined(STM32F7))
 376     sdioPinConfigure();
 377 #endif
 378
 379 #ifdef USE_USB_MSC
 380     /* MSC mode will start after init, but will not allow scheduler to run,
 381      * so there is no bottleneck in reading and writing data
 382      */
 383     mscInit();
 384 #if defined(USE_FLASHFS)
 385         // If the blackbox device is onboard flash, then initialize and scan
 386         // it to identify the log files *before* starting the USB device to
 387         // prevent timeouts of the mass storage device.
 388         if (blackboxConfig()->device == BLACKBOX_DEVICE_FLASH) {
 389             // Must initialise the device to read _anything_
 390             if (!flashDeviceInitialized) {
 391                 flashDeviceInitialized = flashInit();
 392             }
 393             emfat_init_files();
 394         }
 395 #endif
 396
 397     if (mscCheckBoot() || mscCheckButton()) {
 398         if (mscStart() == 0) {
 399              mscWaitForButton();
 400         } else {
 401              NVIC_SystemReset();
 402         }
 403     }
 404 #endif
 405
 406 #ifdef USE_I2C
 407 #ifdef USE_I2C_DEVICE_1
 408     #ifdef I2C_DEVICE_1_SHARES_UART3
 409         if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
 410             i2cInit(I2CDEV_1);
 411         }
 412     #else
 413             i2cInit(I2CDEV_1);
 414     #endif
 415 #endif
 416
 417 #ifdef USE_I2C_DEVICE_2
 418     #ifdef I2C_DEVICE_2_SHARES_UART3
 419         if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
 420             i2cInit(I2CDEV_2);
 421         }
 422     #else
 423             i2cInit(I2CDEV_2);
 424     #endif
 425 #endif
 426
 427 #ifdef USE_I2C_DEVICE_3
 428     i2cInit(I2CDEV_3);
 429 #endif
 430
 431 #ifdef USE_I2C_DEVICE_4
 432     i2cInit(I2CDEV_4);
 433 #endif
 434
 435 #ifdef USE_I2C_DEVICE_EMULATED
 436     #ifdef I2C_DEVICE_EMULATED_SHARES_UART3
 437         if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
 438             i2cInit(I2CDEV_EMULATED);
 439         }
 440     #else
 441             i2cInit(I2CDEV_EMULATED);
 442     #endif
 443 #endif
 444 #endif
 445
 446 #ifdef USE_ADC
 447     drv_adc_config_t adc_params;
 448     memset(&adc_params, 0, sizeof(adc_params));
 449
 450     // Allocate and initialize ADC channels if features are configured - can't rely on sensor detection here, it's done later
 451     if (feature(FEATURE_VBAT)) {
 452         adc_params.adcFunctionChannel[ADC_BATTERY] = adcChannelConfig()->adcFunctionChannel[ADC_BATTERY];
 453     }
 454
 455     if (feature(FEATURE_RSSI_ADC)) {
 456         adc_params.adcFunctionChannel[ADC_RSSI] = adcChannelConfig()->adcFunctionChannel[ADC_RSSI];
 457     }
 458
 459     if (feature(FEATURE_CURRENT_METER) && batteryMetersConfig()->current.type == CURRENT_SENSOR_ADC) {
 460         adc_params.adcFunctionChannel[ADC_CURRENT] =  adcChannelConfig()->adcFunctionChannel[ADC_CURRENT];
 461     }
 462
 463 #if defined(USE_PITOT) && defined(USE_ADC) && defined(USE_PITOT_ADC)
 464     if (pitotmeterConfig()->pitot_hardware == PITOT_ADC || pitotmeterConfig()->pitot_hardware == PITOT_AUTODETECT) {
 465         adc_params.adcFunctionChannel[ADC_AIRSPEED] = adcChannelConfig()->adcFunctionChannel[ADC_AIRSPEED];
 466     }
 467 #endif
 468
 469     adcInit(&adc_params);
 470 #endif
 471
 472 #ifdef USE_PINIO
 473     pinioInit();
 474 #endif
 475
 476 #ifdef USE_PINIOBOX
 477     pinioBoxInit();
 478 #endif
 479
 480 #if defined(USE_GPS) || defined(USE_MAG)
 481     delay(500);
 482
 483     /* Extra 500ms delay prior to initialising hardware if board is cold-booting */
 484     if (!isMPUSoftReset()) {
 485         LED1_ON;
 486         LED0_OFF;
 487
 488         for (int i = 0; i < 5; i++) {
 489             LED1_TOGGLE;
 490             LED0_TOGGLE;
 491             delay(100);
 492         }
 493
 494         LED0_OFF;
 495         LED1_OFF;
 496     }
 497 #endif
 498
 499     initBoardAlignment();
 500
 501 #ifdef USE_CMS
 502     cmsInit();
 503 #endif
 504
 505 #ifdef USE_DASHBOARD
 506     if (feature(FEATURE_DASHBOARD)) {
 507         dashboardInit();
 508     }
 509 #endif
 510
 511 #ifdef USE_GPS
 512     if (feature(FEATURE_GPS)) {
 513         gpsPreInit();
 514     }
 515 #endif
 516
 517     // 1-Wire IF chip
 518 #ifdef USE_1WIRE
 519     owInit();
 520 #endif
 521
 522 #ifdef USE_EZ_TUNE
 523     ezTuneUpdate();
 524 #endif
 525
 526 #ifndef USE_GEOZONE
 527     featureClear(FEATURE_GEOZONE);
 528 #endif
 529
 530     if (!sensorsAutodetect()) {
 531         // if gyro was not detected due to whatever reason, we give up now.
 532         failureMode(FAILURE_MISSING_ACC);
 533     }
 534
 535     systemState |= SYSTEM_STATE_SENSORS_READY;
 536
 537     flashLedsAndBeep();
 538
 539     pidInitFilters();
 540
 541     imuInit();
 542
 543     // Sensors have now been detected, mspFcInit() can now be called
 544     // to set the boxes up
 545     mspFcInit();
 546
 547     cliInit(serialConfig());
 548
 549     failsafeInit();
 550
 551     rxInit();
 552
 553 #if defined(USE_OSD)
 554     displayPort_t *osdDisplayPort = NULL;
 555 #endif
 556
 557 #ifdef USE_OSD
 558     if (feature(FEATURE_OSD)) {
 559 #if defined(USE_FRSKYOSD)
 560         if (!osdDisplayPort) {
 561             osdDisplayPort = frskyOSDDisplayPortInit(osdConfig()->video_system);
 562         }
 563 #endif
 564 #ifdef USE_MSP_OSD
 565         if (!osdDisplayPort) {
 566             osdDisplayPort = mspOsdDisplayPortInit(osdConfig()->video_system);
 567         }
 568 #endif
 569 #if defined(USE_MAX7456)
 570         // If there is a max7456 chip for the OSD and we have no
 571         // external OSD initialized, use it.
 572         if (!osdDisplayPort) {
 573             osdDisplayPort = max7456DisplayPortInit(osdConfig()->video_system);
 574         }
 575 #elif defined(USE_OSD_OVER_MSP_DISPLAYPORT) // OSD over MSP; not supported (yet)
 576         if (!osdDisplayPort) {
 577             osdDisplayPort = displayPortMspInit();
 578         }
 579 #endif
 580         // osdInit  will register with CMS by itself.
 581         osdInit(osdDisplayPort);
 582     }
 583 #endif
 584
 585 #if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
 586     // Register the srxl Textgen telemetry sensor as a displayport device
 587     cmsDisplayPortRegister(displayPortSrxlInit());
 588 #endif
 589
 590 #ifdef USE_GPS
 591     if (feature(FEATURE_GPS)) {
 592         gpsInit();
 593     }
 594 #endif
 595
 596
 597     navigationInit();
 598
 599 #ifdef USE_LED_STRIP
 600     ledStripInit();
 601
 602     if (feature(FEATURE_LED_STRIP)) {
 603         ledStripEnable();
 604     }
 605 #endif
 606
 607 #ifdef USE_TELEMETRY
 608     if (feature(FEATURE_TELEMETRY)) {
 609         telemetryInit();
 610     }
 611 #endif
 612
 613 #ifdef USE_BLACKBOX
 614
 615     //Do not allow blackbox to be run faster that 1kHz. It can cause UAV to drop dead when digital ESC protocol is used
 616     const uint32_t blackboxLooptime =  getLooptime() * blackboxConfig()->rate_denom / blackboxConfig()->rate_num;
 617
 618     if (blackboxLooptime < 1000) {
 619         blackboxConfigMutable()->rate_num = 1;
 620         blackboxConfigMutable()->rate_denom = ceil(1000 / getLooptime());
 621     }
 622
 623     // SDCARD and FLASHFS are used only for blackbox
 624     // Make sure we only init what's necessary for blackbox
 625     switch (blackboxConfig()->device) {
 626 #ifdef USE_FLASHFS
 627         case BLACKBOX_DEVICE_FLASH:
 628             if (!flashDeviceInitialized) {
 629                 flashDeviceInitialized = flashInit();
 630             }
 631             if (flashDeviceInitialized) {
 632                 // do not initialize flashfs if no flash was found
 633                 flashfsInit();
 634             }
 635             break;
 636 #endif
 637
 638 #ifdef USE_SDCARD
 639         case BLACKBOX_DEVICE_SDCARD:
 640             sdcardInsertionDetectInit();
 641             sdcard_init();
 642             afatfs_init();
 643             break;
 644 #endif
 645         default:
 646             break;
 647     }
 648
 649     blackboxInit();
 650 #endif
 651
 652     gyroStartCalibration();
 653
 654 #ifdef USE_BARO
 655     baroStartCalibration();
 656 #endif
 657
 658 #ifdef USE_PITOT
 659     pitotStartCalibration();
 660 #endif
 661
 662 #if defined(USE_VTX_CONTROL)
 663     vtxControlInit();
 664     vtxCommonInit();
 665     vtxInit();
 666
 667 #ifdef USE_VTX_SMARTAUDIO
 668     vtxSmartAudioInit();
 669 #endif
 670
 671 #ifdef USE_VTX_TRAMP
 672     vtxTrampInit();
 673 #endif
 674
 675 #ifdef USE_VTX_FFPV
 676     vtxFuriousFPVInit();
 677 #endif
 678
 679 #ifdef USE_VTX_MSP
 680     vtxMspInit();
 681 #endif
 682
 683 #endif // USE_VTX_CONTROL
 684
 685     // Now that everything has powered up the voltage and cell count be determined.
 686     if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
 687         batteryInit();
 688
 689 #ifdef USE_RCDEVICE
 690     rcdeviceInit();
 691 #endif // USE_RCDEVICE
 692
 693 #ifdef USE_DSHOT
 694     initDShotCommands();
 695 #endif
 696
 697 #ifdef USE_SERIAL_GIMBAL
 698     gimbalCommonInit();
 699     // Needs to be called before gimbalSerialHeadTrackerInit
 700     gimbalSerialInit();
 701 #endif
 702
 703 #ifdef USE_HEADTRACKER
 704     headTrackerCommonInit();
 705 #ifdef USE_HEADTRACKER_SERIAL
 706     // Needs to be called after gimbalSerialInit
 707     gimbalSerialHeadTrackerInit();
 708 #endif
 709 #ifdef USE_HEADTRACKER_MSP
 710     mspHeadTrackerInit();
 711 #endif
 712 #endif
 713
 714     // Latch active features AGAIN since some may be modified by init().
 715     latchActiveFeatures();
 716     motorControlEnable = true;
 717
 718     fcTasksInit();
 719
 720 #ifdef USE_OSD
 721     if (feature(FEATURE_OSD) && (osdDisplayPort != NULL)) {
 722         setTaskEnabled(TASK_OSD, feature(FEATURE_OSD));
 723     }
 724 #endif
 725
 726 #ifdef USE_RPM_FILTER
 727     disableRpmFilters();
 728     if (STATE(ESC_SENSOR_ENABLED) && (rpmFilterConfig()->gyro_filter_enabled || rpmFilterConfig()->dterm_filter_enabled)) {
 729         rpmFiltersInit();
 730         setTaskEnabled(TASK_RPM_FILTER, true);
 731     }
 732 #endif
 733
 734 #ifdef USE_I2C_IO_EXPANDER
 735     ioPortExpanderInit();
 736 #endif
 737
 738 #ifdef USE_POWER_LIMITS
 739     powerLimiterInit();
 740 #endif
 741
 742 #if !defined(SITL_BUILD)
 743     // Considering that the persistent reset reason is only used during init
 744     persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE);
 745 #endif
 746
 747     systemState |= SYSTEM_STATE_READY;
 748 }