src/main/fc/init.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 <stdint.h>
  23 #include <string.h>
  24 #include <math.h>
  25
  26 #include "platform.h"
  27
  28 #include "blackbox/blackbox.h"
  29
  30 #include "cli/cli.h"
  31
  32 #include "common/axis.h"
  33 #include "common/color.h"
  34 #include "common/maths.h"
  35 #include "common/printf.h"
  36
  37 #include "config/config_eeprom.h"
  38 #include "config/feature.h"
  39
  40 #include "cms/cms.h"
  41 #include "cms/cms_types.h"
  42
  43 #include "drivers/accgyro/accgyro.h"
  44 #include "drivers/camera_control.h"
  45 #include "drivers/compass/compass.h"
  46 #include "drivers/pwm_esc_detect.h"
  47 #include "drivers/pwm_output.h"
  48 #include "drivers/adc.h"
  49 #include "drivers/bus.h"
  50 #include "drivers/bus_i2c.h"
  51 #include "drivers/bus_spi.h"
  52 #include "drivers/buttons.h"
  53 #include "drivers/camera_control.h"
  54 #include "drivers/compass/compass.h"
  55 #include "drivers/dma.h"
  56 #include "drivers/exti.h"
  57 #include "drivers/flash.h"
  58 #include "drivers/inverter.h"
  59 #include "drivers/io.h"
  60 #include "drivers/light_led.h"
  61 #include "drivers/mco.h"
  62 #include "drivers/nvic.h"
  63 #include "drivers/persistent.h"
  64 #include "drivers/pwm_esc_detect.h"
  65 #include "drivers/pwm_output.h"
  66 #include "drivers/rx/rx_pwm.h"
  67 #include "drivers/sensor.h"
  68 #include "drivers/serial.h"
  69 #include "drivers/serial_softserial.h"
  70 #include "drivers/serial_uart.h"
  71 #include "drivers/sdcard.h"
  72 #include "drivers/sound_beeper.h"
  73 #include "drivers/system.h"
  74 #include "drivers/time.h"
  75 #include "drivers/timer.h"
  76 #include "drivers/transponder_ir.h"
  77 #include "drivers/exti.h"
  78 #include "drivers/usb_io.h"
  79 #include "drivers/vtx_rtc6705.h"
  80 #include "drivers/vtx_common.h"
  81 #include "drivers/vtx_table.h"
  82 #ifdef USE_USB_MSC
  83 #include "drivers/usb_msc.h"
  84 #endif
  85
  86 #include "fc/board_info.h"
  87 #include "fc/config.h"
  88 #include "fc/init.h"
  89 #include "fc/tasks.h"
  90 #include "fc/rc_controls.h"
  91 #include "fc/runtime_config.h"
  92 #include "fc/dispatch.h"
  93
  94 #ifdef USE_PERSISTENT_MSC_RTC
  95 #include "msc/usbd_storage.h"
  96 #endif
  97
  98 #include "msp/msp.h"
  99 #include "msp/msp_serial.h"
 100
 101 #include "pg/adc.h"
 102 #include "pg/beeper.h"
 103 #include "pg/beeper_dev.h"
 104 #include "pg/bus_i2c.h"
 105 #include "pg/bus_spi.h"
 106 #include "pg/flash.h"
 107 #include "pg/mco.h"
 108 #include "pg/pinio.h"
 109 #include "pg/piniobox.h"
 110 #include "pg/pg.h"
 111 #include "pg/rx.h"
 112 #include "pg/rx_spi.h"
 113 #include "pg/rx_pwm.h"
 114 #include "pg/sdcard.h"
 115 #include "pg/vcd.h"
 116
 117 #include "rx/rx.h"
 118 #include "rx/rx_spi.h"
 119 #include "rx/spektrum.h"
 120
 121 #include "io/beeper.h"
 122 #include "io/displayport_max7456.h"
 123 #include "io/displayport_srxl.h"
 124 #include "io/serial.h"
 125 #include "io/flashfs.h"
 126 #include "io/gps.h"
 127 #include "io/motors.h"
 128 #include "io/servos.h"
 129 #include "io/gimbal.h"
 130 #include "io/ledstrip.h"
 131 #include "io/dashboard.h"
 132 #include "io/asyncfatfs/asyncfatfs.h"
 133 #include "io/transponder_ir.h"
 134 #include "io/pidaudio.h"
 135 #include "io/piniobox.h"
 136 #include "io/displayport_msp.h"
 137 #include "io/vtx.h"
 138 #include "io/vtx_rtc6705.h"
 139 #include "io/vtx_control.h"
 140 #include "io/vtx_smartaudio.h"
 141 #include "io/vtx_tramp.h"
 142
 143 #include "osd/osd.h"
 144
 145 #include "scheduler/scheduler.h"
 146
 147 #include "sensors/acceleration.h"
 148 #include "sensors/barometer.h"
 149 #include "sensors/battery.h"
 150 #include "sensors/boardalignment.h"
 151 #include "sensors/compass.h"
 152 #include "sensors/esc_sensor.h"
 153 #include "sensors/gyro.h"
 154 #include "sensors/initialisation.h"
 155 #include "sensors/rpm_filter.h"
 156 #include "sensors/sensors.h"
 157
 158 #include "telemetry/telemetry.h"
 159
 160 #include "flight/failsafe.h"
 161 #include "flight/imu.h"
 162 #include "flight/mixer.h"
 163 #include "flight/pid.h"
 164 #include "flight/servos.h"
 165
 166 #include "io/rcdevice_cam.h"
 167
 168 #ifdef USE_HARDWARE_REVISION_DETECTION
 169 #include "hardware_revision.h"
 170 #endif
 171
 172 #include "build/build_config.h"
 173 #include "build/debug.h"
 174
 175 #ifdef TARGET_PREINIT
 176 void targetPreInit(void);
 177 #endif
 178
 179 #ifdef SOFTSERIAL_LOOPBACK
 180 serialPort_t *loopbackPort;
 181 #endif
 182
 183 uint8_t systemState = SYSTEM_STATE_INITIALISING;
 184
 185 void processLoopback(void)
 186 {
 187 #ifdef SOFTSERIAL_LOOPBACK
 188     if (loopbackPort) {
 189         uint8_t bytesWaiting;
 190         while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) {
 191             uint8_t b = serialRead(loopbackPort);
 192             serialWrite(loopbackPort, b);
 193         };
 194     }
 195 #endif
 196 }
 197
 198 #ifdef BUS_SWITCH_PIN
 199 void busSwitchInit(void)
 200 {
 201 static IO_t busSwitchResetPin        = IO_NONE;
 202
 203     busSwitchResetPin = IOGetByTag(IO_TAG(BUS_SWITCH_PIN));
 204     IOInit(busSwitchResetPin, OWNER_SYSTEM, 0);
 205     IOConfigGPIO(busSwitchResetPin, IOCFG_OUT_PP);
 206
 207     // ENABLE
 208     IOLo(busSwitchResetPin);
 209 }
 210 #endif
 211
 212 #ifdef USE_DSHOT_TELEMETRY
 213 void activateDshotTelemetry(struct dispatchEntry_s* self)
 214 {
 215     if (!ARMING_FLAG(ARMED) && !isDshotTelemetryActive()) {
 216         pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY, false);
 217         dispatchAdd(self, 1e6); // check again in 1 second
 218     }
 219 }
 220
 221 dispatchEntry_t activateDshotTelemetryEntry =
 222 {
 223     activateDshotTelemetry, 0, NULL, false
 224 };
 225 #endif
 226
 227 void init(void)
 228 {
 229 #ifdef USE_ITCM_RAM
 230     /* Load functions into ITCM RAM */
 231     extern uint8_t tcm_code_start;
 232     extern uint8_t tcm_code_end;
 233     extern uint8_t tcm_code;
 234     memcpy(&tcm_code_start, &tcm_code, (size_t) (&tcm_code_end - &tcm_code_start));
 235 #endif
 236
 237 #ifdef USE_FAST_RAM
 238     /* Load FAST_RAM variable intializers into DTCM RAM */
 239     extern uint8_t _sfastram_data;
 240     extern uint8_t _efastram_data;
 241     extern uint8_t _sfastram_idata;
 242     memcpy(&_sfastram_data, &_sfastram_idata, (size_t) (&_efastram_data - &_sfastram_data));
 243 #endif
 244
 245 #ifdef USE_HAL_DRIVER
 246     HAL_Init();
 247 #endif
 248
 249 #if defined(STM32F7)
 250     /* Enable I-Cache */
 251     if (INSTRUCTION_CACHE_ENABLE) {
 252         SCB_EnableICache();
 253     }
 254
 255     /* Enable D-Cache */
 256     if (DATA_CACHE_ENABLE) {
 257         SCB_EnableDCache();
 258     }
 259
 260     if (PREFETCH_ENABLE) {
 261         LL_FLASH_EnablePrefetch();
 262     }
 263 #endif
 264
 265     printfSupportInit();
 266
 267     systemInit();
 268
 269     // initialize IO (needed for all IO operations)
 270     IOInitGlobal();
 271
 272 #ifdef USE_HARDWARE_REVISION_DETECTION
 273     detectHardwareRevision();
 274 #endif
 275
 276 #ifdef USE_BRUSHED_ESC_AUTODETECT
 277     // Opportunistically use the first motor pin of the default configuration for detection.
 278     // We are doing this as with some boards, timing seems to be important, and the later detection will fail.
 279     ioTag_t motorIoTag = timerioTagGetByUsage(TIM_USE_MOTOR, 0);
 280
 281     if (motorIoTag) {
 282         detectBrushedESC(motorIoTag);
 283     }
 284 #endif
 285
 286     initEEPROM();
 287
 288     ensureEEPROMStructureIsValid();
 289
 290     bool readSuccess = readEEPROM();
 291
 292 #if defined(USE_BOARD_INFO)
 293     initBoardInformation();
 294 #endif
 295
 296     if (!readSuccess || !isEEPROMVersionValid() || strncasecmp(systemConfig()->boardIdentifier, TARGET_BOARD_IDENTIFIER, sizeof(TARGET_BOARD_IDENTIFIER))) {
 297         resetEEPROM();
 298     }
 299
 300     systemState |= SYSTEM_STATE_CONFIG_LOADED;
 301
 302 #ifdef USE_BRUSHED_ESC_AUTODETECT
 303     // Now detect again with the actually configured pin for motor 1, if it is not the default pin.
 304     ioTag_t configuredMotorIoTag = motorConfig()->dev.ioTags[0];
 305
 306     if (configuredMotorIoTag && configuredMotorIoTag != motorIoTag) {
 307         detectBrushedESC(configuredMotorIoTag);
 308     }
 309 #endif
 310
 311     //i2cSetOverclock(masterConfig.i2c_overclock);
 312
 313     debugMode = systemConfig()->debug_mode;
 314
 315 #ifdef TARGET_PREINIT
 316     targetPreInit();
 317 #endif
 318
 319 #if !defined(USE_FAKE_LED)
 320     ledInit(statusLedConfig());
 321 #endif
 322     LED2_ON;
 323
 324 #ifdef USE_EXTI
 325     EXTIInit();
 326 #endif
 327
 328 #if defined(USE_BUTTONS)
 329
 330     buttonsInit();
 331
 332     delayMicroseconds(10);  // allow configuration to settle // XXX Could be removed, too?
 333
 334     if (!isMPUSoftReset()) {
 335 #if defined(BUTTON_A_PIN) && defined(BUTTON_B_PIN)
 336         // two buttons required
 337         uint8_t secondsRemaining = 5;
 338         bool bothButtonsHeld;
 339         do {
 340             bothButtonsHeld = buttonAPressed() && buttonBPressed();
 341             if (bothButtonsHeld) {
 342                 if (--secondsRemaining == 0) {
 343                     resetEEPROM();
 344                     systemReset();
 345                 }
 346                 delay(1000);
 347                 LED0_TOGGLE;
 348             }
 349         } while (bothButtonsHeld);
 350 #endif
 351     }
 352 #endif
 353
 354     // Note that spektrumBind checks if a call is immediately after
 355     // hard reset (including power cycle), so it should be called before
 356     // systemClockSetHSEValue and OverclockRebootIfNecessary, as these
 357     // may cause soft reset which will prevent spektrumBind not to execute
 358     // the bind procedure.
 359
 360 #if defined(USE_SPEKTRUM_BIND)
 361     if (featureIsEnabled(FEATURE_RX_SERIAL)) {
 362         switch (rxConfig()->serialrx_provider) {
 363         case SERIALRX_SPEKTRUM1024:
 364         case SERIALRX_SPEKTRUM2048:
 365         case SERIALRX_SRXL:
 366             // Spektrum satellite binding if enabled on startup.
 367             // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
 368             // The rest of Spektrum initialization will happen later - via spektrumInit()
 369             spektrumBind(rxConfigMutable());
 370             break;
 371         }
 372     }
 373 #endif
 374
 375 #ifdef STM32F4
 376     // Only F4 has non-8MHz boards
 377     systemClockSetHSEValue(systemConfig()->hseMhz * 1000000U);
 378 #endif
 379
 380 #ifdef USE_OVERCLOCK
 381     OverclockRebootIfNecessary(systemConfig()->cpu_overclock);
 382 #endif
 383
 384     // Configure MCO output after config is stable
 385 #ifdef USE_MCO
 386     mcoInit(mcoConfig());
 387 #endif
 388
 389     timerInit();  // timer must be initialized before any channel is allocated
 390
 391 #ifdef BUS_SWITCH_PIN
 392     busSwitchInit();
 393 #endif
 394
 395 #if defined(USE_UART)
 396     uartPinConfigure(serialPinConfig());
 397 #endif
 398
 399 #if defined(AVOID_UART1_FOR_PWM_PPM)
 400     serialInit(featureIsEnabled(FEATURE_SOFTSERIAL),
 401             featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
 402 #elif defined(AVOID_UART2_FOR_PWM_PPM)
 403     serialInit(featureIsEnabled(FEATURE_SOFTSERIAL),
 404             featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
 405 #elif defined(AVOID_UART3_FOR_PWM_PPM)
 406     serialInit(featureIsEnabled(FEATURE_SOFTSERIAL),
 407             featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
 408 #else
 409     serialInit(featureIsEnabled(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
 410 #endif
 411
 412     mixerInit(mixerConfig()->mixerMode);
 413     mixerConfigureOutput();
 414
 415     uint16_t idlePulse = motorConfig()->mincommand;
 416     if (featureIsEnabled(FEATURE_3D)) {
 417         idlePulse = flight3DConfig()->neutral3d;
 418     }
 419     if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
 420         idlePulse = 0; // brushed motors
 421     }
 422     /* Motors needs to be initialized soon as posible because hardware initialization
 423      * may send spurious pulses to esc's causing their early initialization. Also ppm
 424      * receiver may share timer with motors so motors MUST be initialized here. */
 425     motorDevInit(&motorConfig()->dev, idlePulse, getMotorCount());
 426     systemState |= SYSTEM_STATE_MOTORS_READY;
 427
 428     if (0) {}
 429 #if defined(USE_PPM)
 430     else if (featureIsEnabled(FEATURE_RX_PPM)) {
 431         ppmRxInit(ppmConfig());
 432     }
 433 #endif
 434 #if defined(USE_PWM)
 435     else if (featureIsEnabled(FEATURE_RX_PARALLEL_PWM)) {
 436         pwmRxInit(pwmConfig());
 437     }
 438 #endif
 439
 440 #ifdef USE_BEEPER
 441     beeperInit(beeperDevConfig());
 442 #endif
 443 /* temp until PGs are implemented. */
 444 #if defined(USE_INVERTER) && !defined(SIMULATOR_BUILD)
 445     initInverters(serialPinConfig());
 446 #endif
 447
 448 #ifdef TARGET_BUS_INIT
 449     targetBusInit();
 450
 451 #else
 452
 453 #ifdef USE_SPI
 454     spiPinConfigure(spiPinConfig(0));
 455 #endif
 456
 457     sensorsPreInit();
 458
 459 #ifdef USE_SPI
 460     spiPreinit();
 461
 462 #ifdef USE_SPI_DEVICE_1
 463     spiInit(SPIDEV_1);
 464 #endif
 465 #ifdef USE_SPI_DEVICE_2
 466     spiInit(SPIDEV_2);
 467 #endif
 468 #ifdef USE_SPI_DEVICE_3
 469     spiInit(SPIDEV_3);
 470 #endif
 471 #ifdef USE_SPI_DEVICE_4
 472     spiInit(SPIDEV_4);
 473 #endif
 474 #endif // USE_SPI
 475
 476 #ifdef USE_USB_MSC
 477 /* MSC mode will start after init, but will not allow scheduler to run,
 478  *  so there is no bottleneck in reading and writing data */
 479     mscInit();
 480     if (mscCheckBoot() || mscCheckButton()) {
 481         if (mscStart() == 0) {
 482              mscWaitForButton();
 483         } else {
 484             systemResetFromMsc();
 485         }
 486     }
 487 #endif
 488
 489 #ifdef USE_PERSISTENT_MSC_RTC
 490     // if we didn't enter MSC mode then clear the persistent RTC value
 491     persistentObjectWrite(PERSISTENT_OBJECT_RTC_HIGH, 0);
 492     persistentObjectWrite(PERSISTENT_OBJECT_RTC_LOW, 0);
 493 #endif
 494
 495 #ifdef USE_I2C
 496     i2cHardwareConfigure(i2cConfig(0));
 497
 498     // Note: Unlike UARTs which are configured when client is present,
 499     // I2C buses are initialized unconditionally if they are configured.
 500
 501 #ifdef USE_I2C_DEVICE_1
 502     i2cInit(I2CDEV_1);
 503 #endif
 504 #ifdef USE_I2C_DEVICE_2
 505     i2cInit(I2CDEV_2);
 506 #endif
 507 #ifdef USE_I2C_DEVICE_3
 508     i2cInit(I2CDEV_3);
 509 #endif
 510 #ifdef USE_I2C_DEVICE_4
 511     i2cInit(I2CDEV_4);
 512 #endif
 513 #endif // USE_I2C
 514
 515 #endif // TARGET_BUS_INIT
 516
 517 #ifdef USE_HARDWARE_REVISION_DETECTION
 518     updateHardwareRevision();
 519 #endif
 520
 521 #ifdef USE_VTX_RTC6705
 522     rtc6705IOInit();
 523 #endif
 524
 525 #ifdef USE_CAMERA_CONTROL
 526     cameraControlInit();
 527 #endif
 528
 529 // XXX These kind of code should goto target/config.c?
 530 // XXX And these no longer work properly as FEATURE_RANGEFINDER does control HCSR04 runtime configuration.
 531 #if defined(RANGEFINDER_HCSR04_SOFTSERIAL2_EXCLUSIVE) && defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL2)
 532     if (featureIsEnabled(FEATURE_RANGEFINDER) && featureIsEnabled(FEATURE_SOFTSERIAL)) {
 533         serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
 534     }
 535 #endif
 536
 537 #if defined(RANGEFINDER_HCSR04_SOFTSERIAL1_EXCLUSIVE) && defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL1)
 538     if (featureIsEnabled(FEATURE_RANGEFINDER) && featureIsEnabled(FEATURE_SOFTSERIAL)) {
 539         serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
 540     }
 541 #endif
 542
 543 #ifdef USE_ADC
 544     adcConfigMutable()->vbat.enabled = (batteryConfig()->voltageMeterSource == VOLTAGE_METER_ADC);
 545     adcConfigMutable()->current.enabled = (batteryConfig()->currentMeterSource == CURRENT_METER_ADC);
 546
 547     // The FrSky D SPI RX sends RSSI_ADC_PIN (if configured) as A2
 548     adcConfigMutable()->rssi.enabled = featureIsEnabled(FEATURE_RSSI_ADC);
 549 #ifdef USE_RX_SPI
 550     adcConfigMutable()->rssi.enabled |= (featureIsEnabled(FEATURE_RX_SPI) && rxSpiConfig()->rx_spi_protocol == RX_SPI_FRSKY_D);
 551 #endif
 552     adcInit(adcConfig());
 553 #endif
 554
 555     initBoardAlignment(boardAlignment());
 556
 557     if (!sensorsAutodetect()) {
 558         // if gyro was not detected due to whatever reason, notify and don't arm.
 559         if (isSystemConfigured()) {
 560             indicateFailure(FAILURE_MISSING_ACC, 2);
 561         }
 562         setArmingDisabled(ARMING_DISABLED_NO_GYRO);
 563     }
 564
 565     systemState |= SYSTEM_STATE_SENSORS_READY;
 566
 567     // gyro.targetLooptime set in sensorsAutodetect(),
 568     // so we are ready to call validateAndFixGyroConfig(), pidInit(), and setAccelerationFilter()
 569     validateAndFixGyroConfig();
 570     pidInit(currentPidProfile);
 571 #ifdef USE_ACC
 572     accInitFilters();
 573 #endif
 574
 575 #ifdef USE_PID_AUDIO
 576     pidAudioInit();
 577 #endif
 578
 579 #ifdef USE_SERVOS
 580     servosInit();
 581     servoConfigureOutput();
 582     if (isMixerUsingServos()) {
 583         //pwm_params.useChannelForwarding = featureIsEnabled(FEATURE_CHANNEL_FORWARDING);
 584         servoDevInit(&servoConfig()->dev);
 585     }
 586     servosFilterInit();
 587 #endif
 588
 589 #ifdef USE_PINIO
 590     pinioInit(pinioConfig());
 591 #endif
 592
 593 #ifdef USE_PINIOBOX
 594     pinioBoxInit(pinioBoxConfig());
 595 #endif
 596
 597     LED1_ON;
 598     LED0_OFF;
 599     LED2_OFF;
 600
 601     for (int i = 0; i < 10; i++) {
 602         LED1_TOGGLE;
 603         LED0_TOGGLE;
 604 #if defined(USE_BEEPER)
 605         delay(25);
 606         if (!(beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(BEEPER_SYSTEM_INIT))) {
 607             BEEP_ON;
 608         }
 609         delay(25);
 610         BEEP_OFF;
 611 #else
 612         delay(50);
 613 #endif
 614     }
 615     LED0_OFF;
 616     LED1_OFF;
 617
 618     imuInit();
 619
 620     mspInit();
 621     mspSerialInit();
 622
 623 #ifdef USE_CLI
 624     cliInit(serialConfig());
 625 #endif
 626
 627     failsafeInit();
 628
 629     rxInit();
 630
 631 /*
 632  * CMS, display devices and OSD
 633  */
 634 #ifdef USE_CMS
 635     cmsInit();
 636 #endif
 637
 638 #if (defined(USE_OSD) || (defined(USE_MSP_DISPLAYPORT) && defined(USE_CMS)))
 639     displayPort_t *osdDisplayPort = NULL;
 640 #endif
 641
 642 #if defined(USE_OSD)
 643     //The OSD need to be initialised after GYRO to avoid GYRO initialisation failure on some targets
 644
 645     if (featureIsEnabled(FEATURE_OSD)) {
 646 #if defined(USE_MAX7456)
 647         // If there is a max7456 chip for the OSD then use it
 648         osdDisplayPort = max7456DisplayPortInit(vcdProfile());
 649 #elif defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT) && defined(USE_OSD_OVER_MSP_DISPLAYPORT) // OSD over MSP; not supported (yet)
 650         osdDisplayPort = displayPortMspInit();
 651 #endif
 652         // osdInit  will register with CMS by itself.
 653         osdInit(osdDisplayPort);
 654     }
 655 #endif
 656
 657 #if defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT)
 658     // If BFOSD is not active, then register MSP_DISPLAYPORT as a CMS device.
 659     if (!osdDisplayPort)
 660         cmsDisplayPortRegister(displayPortMspInit());
 661 #endif
 662
 663 #ifdef USE_DASHBOARD
 664     // Dashbord will register with CMS by itself.
 665     if (featureIsEnabled(FEATURE_DASHBOARD)) {
 666         dashboardInit();
 667     }
 668 #endif
 669
 670 #if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
 671     // Register the srxl Textgen telemetry sensor as a displayport device
 672     cmsDisplayPortRegister(displayPortSrxlInit());
 673 #endif
 674
 675 #ifdef USE_GPS
 676     if (featureIsEnabled(FEATURE_GPS)) {
 677         gpsInit();
 678     }
 679 #endif
 680
 681 #ifdef USE_LED_STRIP
 682     ledStripInit();
 683
 684     if (featureIsEnabled(FEATURE_LED_STRIP)) {
 685         ledStripEnable();
 686     }
 687 #endif
 688
 689 #ifdef USE_TELEMETRY
 690     if (featureIsEnabled(FEATURE_TELEMETRY)) {
 691         telemetryInit();
 692     }
 693 #endif
 694
 695 #ifdef USE_ESC_SENSOR
 696     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
 697         escSensorInit();
 698     }
 699 #endif
 700
 701 #ifdef USE_USB_DETECT
 702     usbCableDetectInit();
 703 #endif
 704
 705 #ifdef USE_TRANSPONDER
 706     if (featureIsEnabled(FEATURE_TRANSPONDER)) {
 707         transponderInit();
 708         transponderStartRepeating();
 709         systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
 710     }
 711 #endif
 712
 713 #ifdef USE_FLASHFS
 714 #if defined(USE_FLASH_CHIP)
 715     flashInit(flashConfig());
 716 #endif
 717     flashfsInit();
 718 #endif
 719
 720 #ifdef USE_BLACKBOX
 721 #ifdef USE_SDCARD
 722     if (blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD) {
 723         if (sdcardConfig()->mode) {
 724             sdcardInsertionDetectInit();
 725             sdcard_init(sdcardConfig());
 726             afatfs_init();
 727         } else {
 728             blackboxConfigMutable()->device = BLACKBOX_DEVICE_NONE;
 729         }
 730     }
 731 #endif
 732     blackboxInit();
 733 #endif
 734
 735 #ifdef USE_ACC
 736     if (mixerConfig()->mixerMode == MIXER_GIMBAL) {
 737         accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
 738     }
 739 #endif
 740     gyroStartCalibration(false);
 741 #ifdef USE_BARO
 742     baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
 743 #endif
 744
 745 #ifdef USE_VTX_TABLE
 746     vtxTableInit();
 747 #endif
 748
 749 #ifdef USE_VTX_CONTROL
 750     vtxControlInit();
 751
 752 #if defined(USE_VTX_COMMON)
 753     vtxCommonInit();
 754 #endif
 755
 756 #ifdef USE_VTX_SMARTAUDIO
 757     vtxSmartAudioInit();
 758 #endif
 759
 760 #ifdef USE_VTX_TRAMP
 761     vtxTrampInit();
 762 #endif
 763
 764 #ifdef USE_VTX_RTC6705
 765 #ifdef VTX_RTC6705_OPTIONAL
 766     if (!vtxCommonDevice()) // external VTX takes precedence when configured.
 767 #endif
 768     {
 769         vtxRTC6705Init();
 770     }
 771 #endif
 772
 773 #endif // VTX_CONTROL
 774
 775     // start all timers
 776     // TODO - not implemented yet
 777     timerStart();
 778
 779     ENABLE_STATE(SMALL_ANGLE);
 780
 781 #ifdef SOFTSERIAL_LOOPBACK
 782     // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
 783     loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
 784     if (!loopbackPort->vTable) {
 785         loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
 786     }
 787     serialPrint(loopbackPort, "LOOPBACK\r\n");
 788 #endif
 789
 790     batteryInit(); // always needs doing, regardless of features.
 791
 792 #ifdef USE_DASHBOARD
 793     if (featureIsEnabled(FEATURE_DASHBOARD)) {
 794 #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
 795         dashboardShowFixedPage(PAGE_GPS);
 796 #else
 797         dashboardResetPageCycling();
 798         dashboardEnablePageCycling();
 799 #endif
 800     }
 801 #endif
 802
 803 #ifdef USE_RCDEVICE
 804     rcdeviceInit();
 805 #endif // USE_RCDEVICE
 806
 807     pwmEnableMotors();
 808
 809     setArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME);
 810
 811 #ifdef USE_DSHOT_TELEMETRY
 812     if (motorConfig()->dev.useDshotTelemetry) {
 813         dispatchEnable();
 814         dispatchAdd(&activateDshotTelemetryEntry, 5000000);
 815     }
 816 #endif
 817
 818     fcTasksInit();
 819
 820     systemState |= SYSTEM_STATE_READY;
 821
 822 }