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)
 376     sdioPinConfigure();
 377     SDIO_GPIO_Init();
 378 #endif
 379
 380 #ifdef USE_USB_MSC
 381     /* MSC mode will start after init, but will not allow scheduler to run,
 382      * so there is no bottleneck in reading and writing data
 383      */
 384     mscInit();
 385 #if defined(USE_FLASHFS)
 386         // If the blackbox device is onboard flash, then initialize and scan
 387         // it to identify the log files *before* starting the USB device to
 388         // prevent timeouts of the mass storage device.
 389         if (blackboxConfig()->device == BLACKBOX_DEVICE_FLASH) {
 390             // Must initialise the device to read _anything_
 391             if (!flashDeviceInitialized) {
 392                 flashDeviceInitialized = flashInit();
 393             }
 394             emfat_init_files();
 395         }
 396 #endif
 397
 398     if (mscCheckBoot() || mscCheckButton()) {
 399         if (mscStart() == 0) {
 400              mscWaitForButton();
 401         } else {
 402              NVIC_SystemReset();
 403         }
 404     }
 405 #endif
 406
 407 #ifdef USE_I2C
 408 #ifdef USE_I2C_DEVICE_1
 409     #ifdef I2C_DEVICE_1_SHARES_UART3
 410         if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
 411             i2cInit(I2CDEV_1);
 412         }
 413     #else
 414             i2cInit(I2CDEV_1);
 415     #endif
 416 #endif
 417
 418 #ifdef USE_I2C_DEVICE_2
 419     #ifdef I2C_DEVICE_2_SHARES_UART3
 420         if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
 421             i2cInit(I2CDEV_2);
 422         }
 423     #else
 424             i2cInit(I2CDEV_2);
 425     #endif
 426 #endif
 427
 428 #ifdef USE_I2C_DEVICE_3
 429     i2cInit(I2CDEV_3);
 430 #endif
 431
 432 #ifdef USE_I2C_DEVICE_4
 433     i2cInit(I2CDEV_4);
 434 #endif
 435
 436 #ifdef USE_I2C_DEVICE_EMULATED
 437     #ifdef I2C_DEVICE_EMULATED_SHARES_UART3
 438         if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
 439             i2cInit(I2CDEV_EMULATED);
 440         }
 441     #else
 442             i2cInit(I2CDEV_EMULATED);
 443     #endif
 444 #endif
 445 #endif
 446
 447 #ifdef USE_ADC
 448     drv_adc_config_t adc_params;
 449     memset(&adc_params, 0, sizeof(adc_params));
 450
 451     // Allocate and initialize ADC channels if features are configured - can't rely on sensor detection here, it's done later
 452     if (feature(FEATURE_VBAT)) {
 453         adc_params.adcFunctionChannel[ADC_BATTERY] = adcChannelConfig()->adcFunctionChannel[ADC_BATTERY];
 454     }
 455
 456     if (feature(FEATURE_RSSI_ADC)) {
 457         adc_params.adcFunctionChannel[ADC_RSSI] = adcChannelConfig()->adcFunctionChannel[ADC_RSSI];
 458     }
 459
 460     if (feature(FEATURE_CURRENT_METER) && batteryMetersConfig()->current.type == CURRENT_SENSOR_ADC) {
 461         adc_params.adcFunctionChannel[ADC_CURRENT] =  adcChannelConfig()->adcFunctionChannel[ADC_CURRENT];
 462     }
 463
 464 #if defined(USE_PITOT) && defined(USE_ADC) && defined(USE_PITOT_ADC)
 465     if (pitotmeterConfig()->pitot_hardware == PITOT_ADC || pitotmeterConfig()->pitot_hardware == PITOT_AUTODETECT) {
 466         adc_params.adcFunctionChannel[ADC_AIRSPEED] = adcChannelConfig()->adcFunctionChannel[ADC_AIRSPEED];
 467     }
 468 #endif
 469
 470     adcInit(&adc_params);
 471 #endif
 472
 473 #ifdef USE_PINIO
 474     pinioInit();
 475 #endif
 476
 477 #ifdef USE_PINIOBOX
 478     pinioBoxInit();
 479 #endif
 480
 481 #if defined(USE_GPS) || defined(USE_MAG)
 482     delay(500);
 483
 484     /* Extra 500ms delay prior to initialising hardware if board is cold-booting */
 485     if (!isMPUSoftReset()) {
 486         LED1_ON;
 487         LED0_OFF;
 488
 489         for (int i = 0; i < 5; i++) {
 490             LED1_TOGGLE;
 491             LED0_TOGGLE;
 492             delay(100);
 493         }
 494
 495         LED0_OFF;
 496         LED1_OFF;
 497     }
 498 #endif
 499
 500     initBoardAlignment();
 501
 502 #ifdef USE_CMS
 503     cmsInit();
 504 #endif
 505
 506 #ifdef USE_DASHBOARD
 507     if (feature(FEATURE_DASHBOARD)) {
 508         dashboardInit();
 509     }
 510 #endif
 511
 512 #ifdef USE_GPS
 513     if (feature(FEATURE_GPS)) {
 514         gpsPreInit();
 515     }
 516 #endif
 517
 518     // 1-Wire IF chip
 519 #ifdef USE_1WIRE
 520     owInit();
 521 #endif
 522
 523 #ifdef USE_EZ_TUNE
 524     ezTuneUpdate();
 525 #endif
 526
 527     if (!sensorsAutodetect()) {
 528         // if gyro was not detected due to whatever reason, we give up now.
 529         failureMode(FAILURE_MISSING_ACC);
 530     }
 531
 532     systemState |= SYSTEM_STATE_SENSORS_READY;
 533
 534     flashLedsAndBeep();
 535
 536     pidInitFilters();
 537
 538     imuInit();
 539
 540     // Sensors have now been detected, mspFcInit() can now be called
 541     // to set the boxes up
 542     mspFcInit();
 543
 544     cliInit(serialConfig());
 545
 546     failsafeInit();
 547
 548     rxInit();
 549
 550 #if defined(USE_OSD)
 551     displayPort_t *osdDisplayPort = NULL;
 552 #endif
 553
 554 #ifdef USE_OSD
 555     if (feature(FEATURE_OSD)) {
 556 #if defined(USE_FRSKYOSD)
 557         if (!osdDisplayPort) {
 558             osdDisplayPort = frskyOSDDisplayPortInit(osdConfig()->video_system);
 559         }
 560 #endif
 561 #ifdef USE_MSP_OSD
 562         if (!osdDisplayPort) {
 563             osdDisplayPort = mspOsdDisplayPortInit(osdConfig()->video_system);
 564         }
 565 #endif
 566 #if defined(USE_MAX7456)
 567         // If there is a max7456 chip for the OSD and we have no
 568         // external OSD initialized, use it.
 569         if (!osdDisplayPort) {
 570             osdDisplayPort = max7456DisplayPortInit(osdConfig()->video_system);
 571         }
 572 #elif defined(USE_OSD_OVER_MSP_DISPLAYPORT) // OSD over MSP; not supported (yet)
 573         if (!osdDisplayPort) {
 574             osdDisplayPort = displayPortMspInit();
 575         }
 576 #endif
 577         // osdInit  will register with CMS by itself.
 578         osdInit(osdDisplayPort);
 579     }
 580 #endif
 581
 582 #if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
 583     // Register the srxl Textgen telemetry sensor as a displayport device
 584     cmsDisplayPortRegister(displayPortSrxlInit());
 585 #endif
 586
 587 #ifdef USE_GPS
 588     if (feature(FEATURE_GPS)) {
 589         gpsInit();
 590     }
 591 #endif
 592
 593
 594     navigationInit();
 595
 596 #ifdef USE_LED_STRIP
 597     ledStripInit();
 598
 599     if (feature(FEATURE_LED_STRIP)) {
 600         ledStripEnable();
 601     }
 602 #endif
 603
 604 #ifdef USE_TELEMETRY
 605     if (feature(FEATURE_TELEMETRY)) {
 606         telemetryInit();
 607     }
 608 #endif
 609
 610 #ifdef USE_BLACKBOX
 611
 612     //Do not allow blackbox to be run faster that 1kHz. It can cause UAV to drop dead when digital ESC protocol is used
 613     const uint32_t blackboxLooptime =  getLooptime() * blackboxConfig()->rate_denom / blackboxConfig()->rate_num;
 614
 615     if (blackboxLooptime < 1000) {
 616         blackboxConfigMutable()->rate_num = 1;
 617         blackboxConfigMutable()->rate_denom = ceil(1000 / getLooptime());
 618     }
 619
 620     // SDCARD and FLASHFS are used only for blackbox
 621     // Make sure we only init what's necessary for blackbox
 622     switch (blackboxConfig()->device) {
 623 #ifdef USE_FLASHFS
 624         case BLACKBOX_DEVICE_FLASH:
 625             if (!flashDeviceInitialized) {
 626                 flashDeviceInitialized = flashInit();
 627             }
 628             if (flashDeviceInitialized) {
 629                 // do not initialize flashfs if no flash was found
 630                 flashfsInit();
 631             }
 632             break;
 633 #endif
 634
 635 #ifdef USE_SDCARD
 636         case BLACKBOX_DEVICE_SDCARD:
 637             sdcardInsertionDetectInit();
 638             sdcard_init();
 639             afatfs_init();
 640             break;
 641 #endif
 642         default:
 643             break;
 644     }
 645
 646     blackboxInit();
 647 #endif
 648
 649     gyroStartCalibration();
 650
 651 #ifdef USE_BARO
 652     baroStartCalibration();
 653 #endif
 654
 655 #ifdef USE_PITOT
 656     pitotStartCalibration();
 657 #endif
 658
 659 #if defined(USE_VTX_CONTROL)
 660     vtxControlInit();
 661     vtxCommonInit();
 662     vtxInit();
 663
 664 #ifdef USE_VTX_SMARTAUDIO
 665     vtxSmartAudioInit();
 666 #endif
 667
 668 #ifdef USE_VTX_TRAMP
 669     vtxTrampInit();
 670 #endif
 671
 672 #ifdef USE_VTX_FFPV
 673     vtxFuriousFPVInit();
 674 #endif
 675
 676 #ifdef USE_VTX_MSP
 677     vtxMspInit();
 678 #endif
 679
 680 #endif // USE_VTX_CONTROL
 681
 682     // Now that everything has powered up the voltage and cell count be determined.
 683     if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
 684         batteryInit();
 685
 686 #ifdef USE_RCDEVICE
 687     rcdeviceInit();
 688 #endif // USE_RCDEVICE
 689
 690 #ifdef USE_DSHOT
 691     initDShotCommands();
 692 #endif
 693
 694 #ifdef USE_SERIAL_GIMBAL
 695     gimbalCommonInit();
 696     // Needs to be called before gimbalSerialHeadTrackerInit
 697     gimbalSerialInit();
 698 #endif
 699
 700 #ifdef USE_HEADTRACKER
 701     headTrackerCommonInit();
 702 #ifdef USE_HEADTRACKER_SERIAL
 703     // Needs to be called after gimbalSerialInit
 704     gimbalSerialHeadTrackerInit();
 705 #endif
 706 #ifdef USE_HEADTRACKER_MSP
 707     mspHeadTrackerInit();
 708 #endif
 709 #endif
 710
 711     // Latch active features AGAIN since some may be modified by init().
 712     latchActiveFeatures();
 713     motorControlEnable = true;
 714
 715     fcTasksInit();
 716
 717 #ifdef USE_OSD
 718     if (feature(FEATURE_OSD) && (osdDisplayPort != NULL)) {
 719         setTaskEnabled(TASK_OSD, feature(FEATURE_OSD));
 720     }
 721 #endif
 722
 723 #ifdef USE_RPM_FILTER
 724     disableRpmFilters();
 725     if (STATE(ESC_SENSOR_ENABLED) && (rpmFilterConfig()->gyro_filter_enabled || rpmFilterConfig()->dterm_filter_enabled)) {
 726         rpmFiltersInit();
 727         setTaskEnabled(TASK_RPM_FILTER, true);
 728     }
 729 #endif
 730
 731 #ifdef USE_I2C_IO_EXPANDER
 732     ioPortExpanderInit();
 733 #endif
 734
 735 #ifdef USE_POWER_LIMITS
 736     powerLimiterInit();
 737 #endif
 738
 739 #if !defined(SITL_BUILD)
 740     // Considering that the persistent reset reason is only used during init
 741     persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE);
 742 #endif
 743
 744     systemState |= SYSTEM_STATE_READY;
 745 }