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 "build/build_config.h"
  31 #include "build/debug.h"
  32
  33 #include "cms/cms.h"
  34 #include "cms/cms_types.h"
  35
  36 #include "common/axis.h"
  37 #include "common/color.h"
  38 #include "common/maths.h"
  39 #include "common/printf_serial.h"
  40
  41 #include "config/config_eeprom.h"
  42 #include "config/feature.h"
  43
  44 #include "drivers/accgyro/accgyro.h"
  45 #include "drivers/adc.h"
  46 #include "drivers/bus.h"
  47 #include "drivers/bus_i2c.h"
  48 #include "drivers/bus_quadspi.h"
  49 #include "drivers/bus_spi.h"
  50 #include "drivers/buttons.h"
  51 #include "drivers/camera_control.h"
  52 #include "drivers/compass/compass.h"
  53 #include "drivers/dma.h"
  54 #include "drivers/exti.h"
  55 #include "drivers/flash.h"
  56 #include "drivers/inverter.h"
  57 #include "drivers/io.h"
  58 #include "drivers/light_led.h"
  59 #include "drivers/mco.h"
  60 #include "drivers/nvic.h"
  61 #include "drivers/persistent.h"
  62 #include "drivers/pin_pull_up_down.h"
  63 #include "drivers/pwm_esc_detect.h"
  64 #include "drivers/pwm_output.h"
  65 #include "drivers/rx/rx_pwm.h"
  66 #include "drivers/sensor.h"
  67 #include "drivers/serial.h"
  68 #include "drivers/serial_softserial.h"
  69 #include "drivers/serial_uart.h"
  70 #include "drivers/sdcard.h"
  71 #include "drivers/sdio.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/usb_io.h"
  78 #ifdef USE_USB_MSC
  79 #include "drivers/usb_msc.h"
  80 #endif
  81 #include "drivers/vtx_common.h"
  82 #include "drivers/vtx_rtc6705.h"
  83 #include "drivers/vtx_table.h"
  84
  85 #include "fc/board_info.h"
  86 #include "config/config.h"
  87 #include "fc/dispatch.h"
  88 #include "fc/init.h"
  89 #include "fc/rc_controls.h"
  90 #include "fc/runtime_config.h"
  91 #include "fc/stats.h"
  92 #include "fc/tasks.h"
  93
  94 #include "flight/failsafe.h"
  95 #include "flight/imu.h"
  96 #include "flight/mixer.h"
  97 #include "flight/pid.h"
  98 #include "flight/rpm_filter.h"
  99 #include "flight/servos.h"
 100
 101 #include "io/asyncfatfs/asyncfatfs.h"
 102 #include "io/beeper.h"
 103 #include "io/dashboard.h"
 104 #include "io/displayport_max7456.h"
 105 #include "io/displayport_msp.h"
 106 #include "io/displayport_srxl.h"
 107 #include "io/flashfs.h"
 108 #include "io/gimbal.h"
 109 #include "io/gps.h"
 110 #include "io/ledstrip.h"
 111 #include "io/motors.h"
 112 #include "io/pidaudio.h"
 113 #include "io/piniobox.h"
 114 #include "io/rcdevice_cam.h"
 115 #include "io/serial.h"
 116 #include "io/servos.h"
 117 #include "io/transponder_ir.h"
 118 #include "io/vtx.h"
 119 #include "io/vtx_control.h"
 120 #include "io/vtx_rtc6705.h"
 121 #include "io/vtx_smartaudio.h"
 122 #include "io/vtx_tramp.h"
 123
 124 #ifdef USE_PERSISTENT_MSC_RTC
 125 #include "msc/usbd_storage.h"
 126 #endif
 127
 128 #include "msp/msp.h"
 129 #include "msp/msp_serial.h"
 130
 131 #include "osd/osd.h"
 132
 133 #include "pg/adc.h"
 134 #include "pg/beeper.h"
 135 #include "pg/beeper_dev.h"
 136 #include "pg/bus_i2c.h"
 137 #include "pg/bus_spi.h"
 138 #include "pg/bus_quadspi.h"
 139 #include "pg/flash.h"
 140 #include "pg/mco.h"
 141 #include "pg/motor.h"
 142 #include "pg/pinio.h"
 143 #include "pg/piniobox.h"
 144 #include "pg/pin_pull_up_down.h"
 145 #include "pg/pg.h"
 146 #include "pg/rx.h"
 147 #include "pg/rx_spi.h"
 148 #include "pg/rx_pwm.h"
 149 #include "pg/sdcard.h"
 150 #include "pg/vcd.h"
 151 #include "pg/vtx_io.h"
 152
 153 #include "rx/rx.h"
 154 #include "rx/rx_spi.h"
 155 #include "rx/spektrum.h"
 156
 157 #include "scheduler/scheduler.h"
 158
 159 #include "sensors/acceleration.h"
 160 #include "sensors/barometer.h"
 161 #include "sensors/battery.h"
 162 #include "sensors/boardalignment.h"
 163 #include "sensors/compass.h"
 164 #include "sensors/esc_sensor.h"
 165 #include "sensors/gyro.h"
 166 #include "sensors/initialisation.h"
 167 #include "sensors/sensors.h"
 168
 169 #include "telemetry/telemetry.h"
 170
 171 #ifdef USE_HARDWARE_REVISION_DETECTION
 172 #include "hardware_revision.h"
 173 #endif
 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 static void configureSPIAndQuadSPI(void)
 213 {
 214 #ifdef USE_SPI
 215     spiPinConfigure(spiPinConfig(0));
 216 #endif
 217
 218     sensorsPreInit();
 219
 220 #ifdef USE_SPI
 221     spiPreinit();
 222
 223 #ifdef USE_SPI_DEVICE_1
 224     spiInit(SPIDEV_1);
 225 #endif
 226 #ifdef USE_SPI_DEVICE_2
 227     spiInit(SPIDEV_2);
 228 #endif
 229 #ifdef USE_SPI_DEVICE_3
 230     spiInit(SPIDEV_3);
 231 #endif
 232 #ifdef USE_SPI_DEVICE_4
 233     spiInit(SPIDEV_4);
 234 #endif
 235 #ifdef USE_SPI_DEVICE_5
 236     spiInit(SPIDEV_5);
 237 #endif
 238 #ifdef USE_SPI_DEVICE_6
 239     spiInit(SPIDEV_6);
 240 #endif
 241 #endif // USE_SPI
 242
 243 #ifdef USE_QUADSPI
 244     quadSpiPinConfigure(quadSpiConfig(0));
 245
 246 #ifdef USE_QUADSPI_DEVICE_1
 247     quadSpiInit(QUADSPIDEV_1);
 248 #endif
 249 #endif // USE_QUAD_SPI
 250 }
 251
 252 void sdCardAndFSInit()
 253 {
 254     sdcard_init(sdcardConfig());
 255     afatfs_init();
 256 }
 257
 258
 259 void init(void)
 260 {
 261 #ifdef SERIAL_PORT_COUNT
 262     printfSerialInit();
 263 #endif
 264
 265     systemInit();
 266
 267     // initialize IO (needed for all IO operations)
 268     IOInitGlobal();
 269
 270 #ifdef USE_HARDWARE_REVISION_DETECTION
 271     detectHardwareRevision();
 272 #endif
 273
 274 #ifdef USE_BRUSHED_ESC_AUTODETECT
 275     // Opportunistically use the first motor pin of the default configuration for detection.
 276     // We are doing this as with some boards, timing seems to be important, and the later detection will fail.
 277     ioTag_t motorIoTag = timerioTagGetByUsage(TIM_USE_MOTOR, 0);
 278
 279     if (motorIoTag) {
 280         detectBrushedESC(motorIoTag);
 281     }
 282 #endif
 283
 284     enum {
 285         FLASH_INIT_ATTEMPTED            = (1 << 0),
 286         SD_INIT_ATTEMPTED               = (1 << 1),
 287         SPI_AND_QSPI_INIT_ATTEMPTED      = (1 << 2),
 288     };
 289     uint8_t initFlags = 0;
 290
 291
 292 #ifdef CONFIG_IN_SDCARD
 293
 294     //
 295     // Config in sdcard presents an issue with pin configuration since the pin and sdcard configs for the
 296     // sdcard are in the config which is on the sdcard which we can't read yet!
 297     //
 298     // FIXME We need to add configuration somewhere, e.g. bootloader image or reserved flash area, that can be read by the firmware.
 299     // it's currently possible for the firmware resource allocation to be wrong after the config is loaded if the user changes the settings.
 300     // This would cause undefined behaviour once the config is loaded.  so for now, users must NOT change sdio/spi configs needed for
 301     // the system to boot and/or to save the config.
 302     //
 303     // note that target specific SDCARD/SDIO/SPI/QUADSPI configs are
 304     // also not supported in USE_TARGET_CONFIG/targetConfigure() when using CONFIG_IN_SDCARD.
 305     //
 306
 307     //
 308     // IMPORTANT: all default flash and pin configurations must be valid for the target after pgResetAll() is called.
 309     // Target designers must ensure other devices connected the same SPI/QUADSPI interface as the flash chip do not
 310     // cause communication issues with the flash chip.  e.g. use external pullups on SPI/QUADSPI CS lines.
 311     //
 312
 313 #ifdef TARGET_BUS_INIT
 314 #error "CONFIG_IN_SDCARD and TARGET_BUS_INIT are mutually exclusive"
 315 #endif
 316
 317     pgResetAll();
 318
 319 #if defined(STM32H7) && defined(USE_SDCARD_SDIO) // H7 only for now, likely should be applied to F4/F7 too
 320     sdioPinConfigure();
 321     SDIO_GPIO_Init();
 322 #endif
 323 #ifdef USE_SDCARD_SPI
 324     configureSPIAndQuadSPI();
 325     initFlags |= SPI_AND_QSPI_INIT_ATTEMPTED;
 326 #endif
 327
 328     sdCardAndFSInit();
 329     initFlags |= SD_INIT_ATTEMPTED;
 330
 331     while (afatfs_getFilesystemState() != AFATFS_FILESYSTEM_STATE_READY) {
 332         afatfs_poll();
 333
 334         if (afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_FATAL) {
 335             failureMode(FAILURE_SDCARD_INITIALISATION_FAILED);
 336         }
 337     }
 338
 339 #endif // CONFIG_IN_SDCARD
 340
 341 #ifdef CONFIG_IN_EXTERNAL_FLASH
 342     //
 343     // Config on external flash presents an issue with pin configuration since the pin and flash configs for the
 344     // external flash are in the config which is on a chip which we can't read yet!
 345     //
 346     // FIXME We need to add configuration somewhere, e.g. bootloader image or reserved flash area, that can be read by the firmware.
 347     // it's currently possible for the firmware resource allocation to be wrong after the config is loaded if the user changes the settings.
 348     // This would cause undefined behaviour once the config is loaded.  so for now, users must NOT change flash/pin configs needed for
 349     // the system to boot and/or to save the config.
 350     //
 351     // note that target specific FLASH/SPI/QUADSPI configs are
 352     // also not supported in USE_TARGET_CONFIG/targetConfigure() when using CONFIG_IN_EXTERNAL_FLASH.
 353     //
 354
 355     //
 356     // IMPORTANT: all default flash and pin configurations must be valid for the target after pgResetAll() is called.
 357     // Target designers must ensure other devices connected the same SPI/QUADSPI interface as the flash chip do not
 358     // cause communication issues with the flash chip.  e.g. use external pullups on SPI/QUADSPI CS lines.
 359     //
 360     pgResetAll();
 361
 362 #ifdef TARGET_BUS_INIT
 363 #error "CONFIG_IN_EXTERNAL_FLASH and TARGET_BUS_INIT are mutually exclusive"
 364 #endif
 365
 366     configureSPIAndQuadSPI();
 367     initFlags |= SPI_AND_QSPI_INIT_ATTEMPTED;
 368
 369
 370 #ifndef USE_FLASH_CHIP
 371 #error "CONFIG_IN_EXTERNAL_FLASH requires USE_FLASH_CHIP to be defined."
 372 #endif
 373
 374     bool haveFlash = flashInit(flashConfig());
 375
 376     if (!haveFlash) {
 377         failureMode(FAILURE_EXTERNAL_FLASH_INIT_FAILED);
 378     }
 379     initFlags |= FLASH_INIT_ATTEMPTED;
 380
 381 #endif // CONFIG_IN_EXTERNAL_FLASH
 382
 383     initEEPROM();
 384
 385     ensureEEPROMStructureIsValid();
 386
 387     bool readSuccess = readEEPROM();
 388
 389 #if defined(USE_BOARD_INFO)
 390     initBoardInformation();
 391 #endif
 392
 393     if (!readSuccess || !isEEPROMVersionValid() || strncasecmp(systemConfig()->boardIdentifier, TARGET_BOARD_IDENTIFIER, sizeof(TARGET_BOARD_IDENTIFIER))) {
 394         resetEEPROM(false);
 395     }
 396
 397     systemState |= SYSTEM_STATE_CONFIG_LOADED;
 398
 399 #ifdef USE_BRUSHED_ESC_AUTODETECT
 400     // Now detect again with the actually configured pin for motor 1, if it is not the default pin.
 401     ioTag_t configuredMotorIoTag = motorConfig()->dev.ioTags[0];
 402
 403     if (configuredMotorIoTag && configuredMotorIoTag != motorIoTag) {
 404         detectBrushedESC(configuredMotorIoTag);
 405     }
 406 #endif
 407
 408     debugMode = systemConfig()->debug_mode;
 409
 410 #ifdef TARGET_PREINIT
 411     targetPreInit();
 412 #endif
 413
 414 #if !defined(USE_FAKE_LED)
 415     ledInit(statusLedConfig());
 416 #endif
 417     LED2_ON;
 418
 419 #ifdef USE_EXTI
 420     EXTIInit();
 421 #endif
 422
 423 #if defined(USE_BUTTONS)
 424
 425     buttonsInit();
 426
 427     delayMicroseconds(10);  // allow configuration to settle // XXX Could be removed, too?
 428
 429     // Allow EEPROM reset with two-button-press without power cycling in DEBUG build
 430 #ifdef DEBUG
 431 #define EEPROM_RESET_PRECONDITION true
 432 #else
 433 #define EEPROM_RESET_PRECONDITION (!isMPUSoftReset())
 434 #endif
 435
 436     if (EEPROM_RESET_PRECONDITION) {
 437 #if defined(BUTTON_A_PIN) && defined(BUTTON_B_PIN)
 438         // two buttons required
 439         uint8_t secondsRemaining = 5;
 440         bool bothButtonsHeld;
 441         do {
 442             bothButtonsHeld = buttonAPressed() && buttonBPressed();
 443             if (bothButtonsHeld) {
 444                 if (--secondsRemaining == 0) {
 445                     resetEEPROM(false);
 446 #ifdef USE_PERSISTENT_OBJECTS
 447                     persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE);
 448 #endif
 449                     systemReset();
 450                 }
 451                 delay(1000);
 452                 LED0_TOGGLE;
 453             }
 454         } while (bothButtonsHeld);
 455 #endif
 456     }
 457
 458 #undef EEPROM_RESET_PRECONDITION
 459
 460 #endif // USE_BUTTONS
 461
 462     // Note that spektrumBind checks if a call is immediately after
 463     // hard reset (including power cycle), so it should be called before
 464     // systemClockSetHSEValue and OverclockRebootIfNecessary, as these
 465     // may cause soft reset which will prevent spektrumBind not to execute
 466     // the bind procedure.
 467
 468 #if defined(USE_SPEKTRUM_BIND)
 469     if (featureIsEnabled(FEATURE_RX_SERIAL)) {
 470         switch (rxConfig()->serialrx_provider) {
 471         case SERIALRX_SPEKTRUM1024:
 472         case SERIALRX_SPEKTRUM2048:
 473         case SERIALRX_SRXL:
 474             // Spektrum satellite binding if enabled on startup.
 475             // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
 476             // The rest of Spektrum initialization will happen later - via spektrumInit()
 477             spektrumBind(rxConfigMutable());
 478             break;
 479         }
 480     }
 481 #endif
 482
 483 #ifdef STM32F4
 484     // Only F4 has non-8MHz boards
 485     systemClockSetHSEValue(systemConfig()->hseMhz * 1000000U);
 486 #endif
 487
 488 #ifdef USE_OVERCLOCK
 489     OverclockRebootIfNecessary(systemConfig()->cpu_overclock);
 490 #endif
 491
 492     // Configure MCO output after config is stable
 493 #ifdef USE_MCO
 494     mcoInit(mcoConfig());
 495 #endif
 496
 497 #ifdef USE_TIMER
 498     timerInit();  // timer must be initialized before any channel is allocated
 499 #endif
 500
 501 #ifdef BUS_SWITCH_PIN
 502     busSwitchInit();
 503 #endif
 504
 505 #if defined(USE_UART)
 506     uartPinConfigure(serialPinConfig());
 507 #endif
 508
 509 #if defined(AVOID_UART1_FOR_PWM_PPM)
 510     serialInit(featureIsEnabled(FEATURE_SOFTSERIAL),
 511             featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
 512 #elif defined(AVOID_UART2_FOR_PWM_PPM)
 513     serialInit(featureIsEnabled(FEATURE_SOFTSERIAL),
 514             featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
 515 #elif defined(AVOID_UART3_FOR_PWM_PPM)
 516     serialInit(featureIsEnabled(FEATURE_SOFTSERIAL),
 517             featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
 518 #else
 519     serialInit(featureIsEnabled(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
 520 #endif
 521
 522     mixerInit(mixerConfig()->mixerMode);
 523     mixerConfigureOutput();
 524
 525     uint16_t idlePulse = motorConfig()->mincommand;
 526     if (featureIsEnabled(FEATURE_3D)) {
 527         idlePulse = flight3DConfig()->neutral3d;
 528     }
 529     if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
 530         idlePulse = 0; // brushed motors
 531     }
 532 #ifdef USE_MOTOR
 533     /* Motors needs to be initialized soon as posible because hardware initialization
 534      * may send spurious pulses to esc's causing their early initialization. Also ppm
 535      * receiver may share timer with motors so motors MUST be initialized here. */
 536     motorDevInit(&motorConfig()->dev, idlePulse, getMotorCount());
 537     systemState |= SYSTEM_STATE_MOTORS_READY;
 538 #else
 539     UNUSED(idlePulse);
 540 #endif
 541
 542     if (0) {}
 543 #if defined(USE_PPM)
 544     else if (featureIsEnabled(FEATURE_RX_PPM)) {
 545         ppmRxInit(ppmConfig());
 546     }
 547 #endif
 548 #if defined(USE_PWM)
 549     else if (featureIsEnabled(FEATURE_RX_PARALLEL_PWM)) {
 550         pwmRxInit(pwmConfig());
 551     }
 552 #endif
 553
 554 #ifdef USE_BEEPER
 555     beeperInit(beeperDevConfig());
 556 #endif
 557 /* temp until PGs are implemented. */
 558 #if defined(USE_INVERTER) && !defined(SIMULATOR_BUILD)
 559     initInverters(serialPinConfig());
 560 #endif
 561
 562
 563 #ifdef TARGET_BUS_INIT
 564     targetBusInit();
 565
 566 #else
 567
 568     // Depending on compilation options SPI/QSPI initialisation may already be done.
 569     if (!(initFlags & SPI_AND_QSPI_INIT_ATTEMPTED)) {
 570         configureSPIAndQuadSPI();
 571         initFlags |= SPI_AND_QSPI_INIT_ATTEMPTED;
 572     }
 573
 574 #ifdef USE_USB_MSC
 575 /* MSC mode will start after init, but will not allow scheduler to run,
 576  *  so there is no bottleneck in reading and writing data */
 577     mscInit();
 578     if (mscCheckBoot() || mscCheckButton()) {
 579         if (mscStart() == 0) {
 580              mscWaitForButton();
 581         } else {
 582             systemResetFromMsc();
 583         }
 584     }
 585 #endif
 586
 587 #ifdef USE_PERSISTENT_MSC_RTC
 588     // if we didn't enter MSC mode then clear the persistent RTC value
 589     persistentObjectWrite(PERSISTENT_OBJECT_RTC_HIGH, 0);
 590     persistentObjectWrite(PERSISTENT_OBJECT_RTC_LOW, 0);
 591 #endif
 592
 593 #ifdef USE_I2C
 594     i2cHardwareConfigure(i2cConfig(0));
 595
 596     // Note: Unlike UARTs which are configured when client is present,
 597     // I2C buses are initialized unconditionally if they are configured.
 598
 599 #ifdef USE_I2C_DEVICE_1
 600     i2cInit(I2CDEV_1);
 601 #endif
 602 #ifdef USE_I2C_DEVICE_2
 603     i2cInit(I2CDEV_2);
 604 #endif
 605 #ifdef USE_I2C_DEVICE_3
 606     i2cInit(I2CDEV_3);
 607 #endif
 608 #ifdef USE_I2C_DEVICE_4
 609     i2cInit(I2CDEV_4);
 610 #endif
 611 #endif // USE_I2C
 612
 613 #endif // TARGET_BUS_INIT
 614
 615 #ifdef USE_HARDWARE_REVISION_DETECTION
 616     updateHardwareRevision();
 617 #endif
 618
 619 #if defined(STM32H7) && defined(USE_SDCARD_SDIO) // H7 only for now, likely should be applied to F4/F7 too
 620     if (!(initFlags & SD_INIT_ATTEMPTED)) {
 621         sdioPinConfigure();
 622         SDIO_GPIO_Init();
 623     }
 624 #endif
 625
 626
 627 #ifdef USE_VTX_RTC6705
 628     bool useRTC6705 = rtc6705IOInit(vtxIOConfig());
 629 #endif
 630
 631 #ifdef USE_CAMERA_CONTROL
 632     cameraControlInit();
 633 #endif
 634
 635 // XXX These kind of code should goto target/config.c?
 636 // XXX And these no longer work properly as FEATURE_RANGEFINDER does control HCSR04 runtime configuration.
 637 #if defined(RANGEFINDER_HCSR04_SOFTSERIAL2_EXCLUSIVE) && defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL2)
 638     if (featureIsEnabled(FEATURE_RANGEFINDER) && featureIsEnabled(FEATURE_SOFTSERIAL)) {
 639         serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
 640     }
 641 #endif
 642
 643 #if defined(RANGEFINDER_HCSR04_SOFTSERIAL1_EXCLUSIVE) && defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL1)
 644     if (featureIsEnabled(FEATURE_RANGEFINDER) && featureIsEnabled(FEATURE_SOFTSERIAL)) {
 645         serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
 646     }
 647 #endif
 648
 649 #ifdef USE_ADC
 650     adcConfigMutable()->vbat.enabled = (batteryConfig()->voltageMeterSource == VOLTAGE_METER_ADC);
 651     adcConfigMutable()->current.enabled = (batteryConfig()->currentMeterSource == CURRENT_METER_ADC);
 652
 653     // The FrSky D SPI RX sends RSSI_ADC_PIN (if configured) as A2
 654     adcConfigMutable()->rssi.enabled = featureIsEnabled(FEATURE_RSSI_ADC);
 655 #ifdef USE_RX_SPI
 656     adcConfigMutable()->rssi.enabled |= (featureIsEnabled(FEATURE_RX_SPI) && rxSpiConfig()->rx_spi_protocol == RX_SPI_FRSKY_D);
 657 #endif
 658     adcInit(adcConfig());
 659 #endif
 660
 661     initBoardAlignment(boardAlignment());
 662
 663     if (!sensorsAutodetect()) {
 664         // if gyro was not detected due to whatever reason, notify and don't arm.
 665         if (true
 666 #if defined(USE_UNIFIED_TARGET)
 667             && isSystemConfigured()
 668 #endif
 669             ) {
 670             indicateFailure(FAILURE_MISSING_ACC, 2);
 671         }
 672         setArmingDisabled(ARMING_DISABLED_NO_GYRO);
 673     }
 674
 675     systemState |= SYSTEM_STATE_SENSORS_READY;
 676
 677     // gyro.targetLooptime set in sensorsAutodetect(),
 678     // so we are ready to call validateAndFixGyroConfig(), pidInit(), and setAccelerationFilter()
 679     validateAndFixGyroConfig();
 680     pidInit(currentPidProfile);
 681 #ifdef USE_ACC
 682     accInitFilters();
 683 #endif
 684
 685 #ifdef USE_PID_AUDIO
 686     pidAudioInit();
 687 #endif
 688
 689 #ifdef USE_SERVOS
 690     servosInit();
 691     servoConfigureOutput();
 692     if (isMixerUsingServos()) {
 693         //pwm_params.useChannelForwarding = featureIsEnabled(FEATURE_CHANNEL_FORWARDING);
 694         servoDevInit(&servoConfig()->dev);
 695     }
 696     servosFilterInit();
 697 #endif
 698
 699 #ifdef USE_PINIO
 700     pinioInit(pinioConfig());
 701 #endif
 702
 703 #ifdef USE_PIN_PULL_UP_DOWN
 704     pinPullupPulldownInit();
 705 #endif
 706
 707 #ifdef USE_PINIOBOX
 708     pinioBoxInit(pinioBoxConfig());
 709 #endif
 710
 711     LED1_ON;
 712     LED0_OFF;
 713     LED2_OFF;
 714
 715     for (int i = 0; i < 10; i++) {
 716         LED1_TOGGLE;
 717         LED0_TOGGLE;
 718 #if defined(USE_BEEPER)
 719         delay(25);
 720         if (!(beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(BEEPER_SYSTEM_INIT))) {
 721             BEEP_ON;
 722         }
 723         delay(25);
 724         BEEP_OFF;
 725 #else
 726         delay(50);
 727 #endif
 728     }
 729     LED0_OFF;
 730     LED1_OFF;
 731
 732     imuInit();
 733
 734     mspInit();
 735     mspSerialInit();
 736
 737     failsafeInit();
 738
 739     rxInit();
 740
 741 /*
 742  * CMS, display devices and OSD
 743  */
 744 #ifdef USE_CMS
 745     cmsInit();
 746 #endif
 747
 748 #if (defined(USE_OSD) || (defined(USE_MSP_DISPLAYPORT) && defined(USE_CMS)))
 749     displayPort_t *osdDisplayPort = NULL;
 750 #endif
 751
 752 #if defined(USE_OSD)
 753     //The OSD need to be initialised after GYRO to avoid GYRO initialisation failure on some targets
 754
 755     if (featureIsEnabled(FEATURE_OSD)) {
 756         osdDisplayPortDevice_e device = osdConfig()->displayPortDevice;
 757
 758         switch(device) {
 759
 760         case OSD_DISPLAYPORT_DEVICE_AUTO:
 761             FALLTHROUGH;
 762
 763 #if defined(USE_MAX7456)
 764         case OSD_DISPLAYPORT_DEVICE_MAX7456:
 765             // If there is a max7456 chip for the OSD configured and detectd then use it.
 766             osdDisplayPort = max7456DisplayPortInit(vcdProfile());
 767             if (osdDisplayPort || device == OSD_DISPLAYPORT_DEVICE_MAX7456) {
 768                 break;
 769             }
 770             FALLTHROUGH;
 771 #endif
 772
 773 #if defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT) && defined(USE_OSD_OVER_MSP_DISPLAYPORT)
 774         case OSD_DISPLAYPORT_DEVICE_MSP:
 775             osdDisplayPort = displayPortMspInit();
 776             if (osdDisplayPort || device == OSD_DISPLAYPORT_DEVICE_MSP) {
 777                 break;
 778             }
 779             FALLTHROUGH;
 780 #endif
 781
 782         // Other device cases can be added here
 783
 784         case OSD_DISPLAYPORT_DEVICE_NONE:
 785         default:
 786             break;
 787         }
 788
 789         // osdInit will register with CMS by itself.
 790         osdInit(osdDisplayPort);
 791     }
 792 #endif // USE_OSD
 793
 794 #if defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT)
 795     // If BFOSD is not active, then register MSP_DISPLAYPORT as a CMS device.
 796     if (!osdDisplayPort)
 797         cmsDisplayPortRegister(displayPortMspInit());
 798 #endif
 799
 800 #ifdef USE_DASHBOARD
 801     // Dashbord will register with CMS by itself.
 802     if (featureIsEnabled(FEATURE_DASHBOARD)) {
 803         dashboardInit();
 804     }
 805 #endif
 806
 807 #if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
 808     // Register the srxl Textgen telemetry sensor as a displayport device
 809     cmsDisplayPortRegister(displayPortSrxlInit());
 810 #endif
 811
 812 #ifdef USE_GPS
 813     if (featureIsEnabled(FEATURE_GPS)) {
 814         gpsInit();
 815     }
 816 #endif
 817
 818 #ifdef USE_LED_STRIP
 819     ledStripInit();
 820
 821     if (featureIsEnabled(FEATURE_LED_STRIP)) {
 822         ledStripEnable();
 823     }
 824 #endif
 825
 826 #ifdef USE_TELEMETRY
 827     if (featureIsEnabled(FEATURE_TELEMETRY)) {
 828         telemetryInit();
 829     }
 830 #endif
 831
 832 #ifdef USE_ESC_SENSOR
 833     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
 834         escSensorInit();
 835     }
 836 #endif
 837
 838 #ifdef USE_USB_DETECT
 839     usbCableDetectInit();
 840 #endif
 841
 842 #ifdef USE_TRANSPONDER
 843     if (featureIsEnabled(FEATURE_TRANSPONDER)) {
 844         transponderInit();
 845         transponderStartRepeating();
 846         systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
 847     }
 848 #endif
 849
 850 #ifdef USE_FLASH_CHIP
 851     if (!(initFlags & FLASH_INIT_ATTEMPTED)) {
 852         flashInit(flashConfig());
 853         initFlags |= FLASH_INIT_ATTEMPTED;
 854     }
 855 #endif
 856 #ifdef USE_FLASHFS
 857     flashfsInit();
 858 #endif
 859
 860 #ifdef USE_BLACKBOX
 861 #ifdef USE_SDCARD
 862     if (blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD) {
 863         if (sdcardConfig()->mode) {
 864             if (!(initFlags & SD_INIT_ATTEMPTED)) {
 865                 initFlags |= SD_INIT_ATTEMPTED;
 866                 sdCardAndFSInit();
 867             }
 868         } else {
 869             blackboxConfigMutable()->device = BLACKBOX_DEVICE_NONE;
 870         }
 871     }
 872 #endif
 873     blackboxInit();
 874 #endif
 875
 876 #ifdef USE_ACC
 877     if (mixerConfig()->mixerMode == MIXER_GIMBAL) {
 878         accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
 879     }
 880 #endif
 881     gyroStartCalibration(false);
 882 #ifdef USE_BARO
 883     baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
 884 #endif
 885
 886 #if defined(USE_VTX_COMMON) || defined(USE_VTX_CONTROL)
 887     vtxTableInit();
 888 #endif
 889
 890 #ifdef USE_VTX_CONTROL
 891     vtxControlInit();
 892
 893 #if defined(USE_VTX_COMMON)
 894     vtxCommonInit();
 895 #endif
 896
 897 #ifdef USE_VTX_SMARTAUDIO
 898     vtxSmartAudioInit();
 899 #endif
 900
 901 #ifdef USE_VTX_TRAMP
 902     vtxTrampInit();
 903 #endif
 904
 905 #ifdef USE_VTX_RTC6705
 906     if (!vtxCommonDevice() && useRTC6705) { // external VTX takes precedence when configured.
 907         vtxRTC6705Init();
 908     }
 909 #endif
 910
 911 #endif // VTX_CONTROL
 912
 913 #ifdef USE_TIMER
 914     // start all timers
 915     // TODO - not implemented yet
 916     timerStart();
 917 #endif
 918
 919     ENABLE_STATE(SMALL_ANGLE);
 920
 921 #ifdef SOFTSERIAL_LOOPBACK
 922     // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
 923     loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
 924     if (!loopbackPort->vTable) {
 925         loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
 926     }
 927     serialPrint(loopbackPort, "LOOPBACK\r\n");
 928 #endif
 929
 930     batteryInit(); // always needs doing, regardless of features.
 931
 932 #ifdef USE_DASHBOARD
 933     if (featureIsEnabled(FEATURE_DASHBOARD)) {
 934 #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
 935         dashboardShowFixedPage(PAGE_GPS);
 936 #else
 937         dashboardResetPageCycling();
 938         dashboardEnablePageCycling();
 939 #endif
 940     }
 941 #endif
 942
 943 #ifdef USE_RCDEVICE
 944     rcdeviceInit();
 945 #endif // USE_RCDEVICE
 946
 947 #ifdef USE_MOTOR
 948     motorPostInit();
 949     motorEnable();
 950 #endif
 951
 952 #ifdef USE_PERSISTENT_STATS
 953     statsInit();
 954 #endif
 955
 956     setArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME);
 957
 958     tasksInit();
 959
 960     systemState |= SYSTEM_STATE_READY;
 961 }