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 #include "build/debug_pin.h"
  33
  34 #include "cms/cms.h"
  35 #include "cms/cms_types.h"
  36
  37 #include "common/axis.h"
  38 #include "common/color.h"
  39 #include "common/maths.h"
  40 #include "common/printf_serial.h"
  41
  42 #include "config/config.h"
  43 #include "config/config_eeprom.h"
  44 #include "config/feature.h"
  45
  46 #include "drivers/accgyro/accgyro.h"
  47 #include "drivers/adc.h"
  48 #include "drivers/bus.h"
  49 #include "drivers/bus_i2c.h"
  50 #include "drivers/bus_octospi.h"
  51 #include "drivers/bus_quadspi.h"
  52 #include "drivers/bus_spi.h"
  53 #include "drivers/buttons.h"
  54 #include "drivers/camera_control_impl.h"
  55 #include "drivers/compass/compass.h"
  56 #include "drivers/dma.h"
  57 #include "drivers/dshot.h"
  58 #include "drivers/exti.h"
  59 #include "drivers/flash.h"
  60 #include "drivers/inverter.h"
  61 #include "drivers/io.h"
  62 #include "drivers/light_led.h"
  63 #include "drivers/mco.h"
  64 #include "drivers/nvic.h"
  65 #include "drivers/persistent.h"
  66 #include "drivers/pin_pull_up_down.h"
  67 #include "drivers/pwm_output.h"
  68 #include "drivers/rx/rx_pwm.h"
  69 #include "drivers/sensor.h"
  70 #include "drivers/serial.h"
  71 #include "drivers/serial_softserial.h"
  72 #include "drivers/serial_uart.h"
  73 #include "drivers/sdcard.h"
  74 #include "drivers/sdio.h"
  75 #include "drivers/sound_beeper.h"
  76 #include "drivers/system.h"
  77 #include "drivers/time.h"
  78 #include "drivers/timer.h"
  79 #include "drivers/transponder_ir.h"
  80 #include "drivers/usb_io.h"
  81 #ifdef USE_USB_MSC
  82 #include "drivers/usb_msc.h"
  83 #endif
  84 #include "drivers/vtx_common.h"
  85 #include "drivers/vtx_rtc6705.h"
  86 #include "drivers/vtx_table.h"
  87
  88 #include "fc/board_info.h"
  89 #include "fc/dispatch.h"
  90 #include "fc/gps_lap_timer.h"
  91 #include "fc/init.h"
  92 #include "fc/rc_controls.h"
  93 #include "fc/runtime_config.h"
  94 #include "fc/stats.h"
  95 #include "fc/tasks.h"
  96
  97 #include "flight/failsafe.h"
  98 #include "flight/imu.h"
  99 #include "flight/mixer.h"
 100 #include "flight/gps_rescue.h"
 101 #include "flight/pid.h"
 102 #include "flight/pid_init.h"
 103 #include "flight/position.h"
 104 #include "flight/servos.h"
 105
 106 #include "io/asyncfatfs/asyncfatfs.h"
 107 #include "io/beeper.h"
 108 #include "io/dashboard.h"
 109 #include "io/displayport_frsky_osd.h"
 110 #include "io/displayport_max7456.h"
 111 #include "io/displayport_msp.h"
 112 #include "io/flashfs.h"
 113 #include "io/gimbal.h"
 114 #include "io/gps.h"
 115 #include "io/ledstrip.h"
 116 #include "io/pidaudio.h"
 117 #include "io/piniobox.h"
 118 #include "io/rcdevice_cam.h"
 119 #include "io/serial.h"
 120 #include "io/servos.h"
 121 #include "io/transponder_ir.h"
 122 #include "io/vtx.h"
 123 #include "io/vtx_control.h"
 124 #include "io/vtx_msp.h"
 125 #include "io/vtx_rtc6705.h"
 126 #include "io/vtx_smartaudio.h"
 127 #include "io/vtx_tramp.h"
 128
 129 #include "msc/emfat_file.h"
 130 #ifdef USE_PERSISTENT_MSC_RTC
 131 #include "msc/usbd_storage.h"
 132 #endif
 133
 134 #include "msp/msp.h"
 135 #include "msp/msp_serial.h"
 136
 137 #include "osd/osd.h"
 138
 139 #include "pg/adc.h"
 140 #include "pg/beeper.h"
 141 #include "pg/beeper_dev.h"
 142 #include "pg/bus_i2c.h"
 143 #include "pg/bus_spi.h"
 144 #include "pg/bus_quadspi.h"
 145 #include "pg/flash.h"
 146 #include "pg/mco.h"
 147 #include "pg/motor.h"
 148 #include "pg/pinio.h"
 149 #include "pg/piniobox.h"
 150 #include "pg/pin_pull_up_down.h"
 151 #include "pg/pg.h"
 152 #include "pg/rx.h"
 153 #include "pg/rx_pwm.h"
 154 #include "pg/rx_spi.h"
 155 #include "pg/sdcard.h"
 156 #include "pg/vcd.h"
 157 #include "pg/vtx_io.h"
 158
 159 #include "rx/rx.h"
 160 #include "rx/spektrum.h"
 161
 162 #include "scheduler/scheduler.h"
 163
 164 #include "sensors/acceleration.h"
 165 #include "sensors/barometer.h"
 166 #include "sensors/battery.h"
 167 #include "sensors/boardalignment.h"
 168 #include "sensors/compass.h"
 169 #include "sensors/esc_sensor.h"
 170 #include "sensors/gyro.h"
 171 #include "sensors/gyro_init.h"
 172 #include "sensors/initialisation.h"
 173
 174 #include "telemetry/telemetry.h"
 175
 176 #ifdef USE_HARDWARE_REVISION_DETECTION
 177 #include "hardware_revision.h"
 178 #endif
 179
 180 #ifdef TARGET_PREINIT
 181 void targetPreInit(void);
 182 #endif
 183
 184 uint8_t systemState = SYSTEM_STATE_INITIALISING;
 185
 186 #ifdef BUS_SWITCH_PIN
 187 void busSwitchInit(void)
 188 {
 189     IO_t busSwitchResetPin = IO_NONE;
 190
 191     busSwitchResetPin = IOGetByTag(IO_TAG(BUS_SWITCH_PIN));
 192     IOInit(busSwitchResetPin, OWNER_SYSTEM, 0);
 193     IOConfigGPIO(busSwitchResetPin, IOCFG_OUT_PP);
 194
 195     // ENABLE
 196     IOLo(busSwitchResetPin);
 197 }
 198 #endif
 199
 200 static void configureSPIBusses(void)
 201 {
 202 #ifdef USE_SPI
 203     spiPinConfigure(spiPinConfig(0));
 204 #endif
 205
 206     sensorsPreInit();
 207
 208 #ifdef USE_SPI
 209     spiPreinit();
 210
 211 #ifdef USE_SPI_DEVICE_1
 212     spiInit(SPIDEV_1);
 213 #endif
 214 #ifdef USE_SPI_DEVICE_2
 215     spiInit(SPIDEV_2);
 216 #endif
 217 #ifdef USE_SPI_DEVICE_3
 218     spiInit(SPIDEV_3);
 219 #endif
 220 #ifdef USE_SPI_DEVICE_4
 221     spiInit(SPIDEV_4);
 222 #endif
 223 #ifdef USE_SPI_DEVICE_5
 224     spiInit(SPIDEV_5);
 225 #endif
 226 #ifdef USE_SPI_DEVICE_6
 227     spiInit(SPIDEV_6);
 228 #endif
 229 #endif // USE_SPI
 230 }
 231
 232 static void configureQuadSPIBusses(void)
 233 {
 234 #ifdef USE_QUADSPI
 235     quadSpiPinConfigure(quadSpiConfig(0));
 236
 237 #ifdef USE_QUADSPI_DEVICE_1
 238     quadSpiInit(QUADSPIDEV_1);
 239 #endif
 240 #endif // USE_QUAD_SPI
 241 }
 242
 243 static void configureOctoSPIBusses(void)
 244 {
 245 #ifdef USE_OCTOSPI
 246 #ifdef USE_OCTOSPI_DEVICE_1
 247     octoSpiInit(OCTOSPIDEV_1);
 248 #endif
 249 #endif
 250 }
 251
 252 #ifdef USE_SDCARD
 253 static void sdCardAndFSInit(void)
 254 {
 255     sdcard_init(sdcardConfig());
 256     afatfs_init();
 257 }
 258 #endif
 259
 260 void init(void)
 261 {
 262 #ifdef SERIAL_PORT_COUNT
 263     printfSerialInit();
 264 #endif
 265
 266     systemInit();
 267
 268     // Initialize task data as soon as possible. Has to be done before tasksInit(),
 269     // and any init code that may try to modify task behaviour before tasksInit().
 270     tasksInitData();
 271
 272     // initialize IO (needed for all IO operations)
 273     IOInitGlobal();
 274
 275 #ifdef USE_HARDWARE_REVISION_DETECTION
 276     detectHardwareRevision();
 277 #endif
 278
 279 #if defined(USE_TARGET_CONFIG)
 280     // Call once before the config is loaded for any target specific configuration required to support loading the config
 281     targetConfiguration();
 282 #endif
 283
 284     enum {
 285         FLASH_INIT_ATTEMPTED                = (1 << 0),
 286         SD_INIT_ATTEMPTED                   = (1 << 1),
 287         SPI_BUSSES_INIT_ATTEMPTED           = (1 << 2),
 288         QUAD_OCTO_SPI_BUSSES_INIT_ATTEMPTED = (1 << 3),
 289     };
 290     uint8_t initFlags = 0;
 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 For now, users must NOT change flash/pin configs needed for the system to boot and/or to save the config.
 299     // One possible solution is to lock the pins for the flash chip so they cannot be modified post-boot.
 300     //
 301     // note that target specific SDCARD/SDIO/SPI/QUADSPI/OCTOSPI configs are
 302     // also not supported in USE_TARGET_CONFIG/targetConfigure() when using CONFIG_IN_SDCARD.
 303     //
 304
 305     //
 306     // IMPORTANT: all default flash and pin configurations must be valid for the target after pgResetAll() is called.
 307     // Target designers must ensure other devices connected the same SPI/QUADSPI/OCTOSPI interface as the flash chip do not
 308     // cause communication issues with the flash chip.  e.g. use external pullups on SPI/QUADSPI/OCTOSPI CS lines.
 309     //
 310
 311 #ifdef TARGET_BUS_INIT
 312 #error "CONFIG_IN_SDCARD and TARGET_BUS_INIT are mutually exclusive"
 313 #endif
 314
 315     pgResetAll();
 316
 317 #ifdef USE_SDCARD_SPI
 318     configureSPIBusses();
 319     initFlags |= SPI_BUSSES_INIT_ATTEMPTED;
 320 #endif
 321
 322     sdCardAndFSInit();
 323     initFlags |= SD_INIT_ATTEMPTED;
 324
 325     if (!sdcard_isInserted()) {
 326         failureMode(FAILURE_SDCARD_REQUIRED);
 327     }
 328
 329     while (afatfs_getFilesystemState() != AFATFS_FILESYSTEM_STATE_READY) {
 330         afatfs_poll();
 331
 332         if (afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_FATAL) {
 333             failureMode(FAILURE_SDCARD_INITIALISATION_FAILED);
 334         }
 335     }
 336
 337 #endif // CONFIG_IN_SDCARD
 338
 339 #if defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH)
 340     //
 341     // Config on external flash presents an issue with pin configuration since the pin and flash configs for the
 342     // external flash are in the config which is on a chip which we can't read yet!
 343     //
 344     // FIXME For now, users must NOT change flash/pin configs needed for the system to boot and/or to save the config.
 345     // One possible solution is to lock the pins for the flash chip so they cannot be modified post-boot.
 346     //
 347     // note that target specific FLASH/SPI/QUADSPI/OCTOSPI configs are
 348     // also not supported in USE_TARGET_CONFIG/targetConfigure() when using CONFIG_IN_EXTERNAL_FLASH/CONFIG_IN_MEMORY_MAPPED_FLASH.
 349     //
 350
 351     //
 352     // IMPORTANT: all default flash and pin configurations must be valid for the target after pgResetAll() is called.
 353     // Target designers must ensure other devices connected the same SPI/QUADSPI/OCTOSPI interface as the flash chip do not
 354     // cause communication issues with the flash chip.  e.g. use external pullups on SPI/QUADSPI/OCTOSPI CS lines.
 355     //
 356     pgResetAll();
 357
 358 #ifdef TARGET_BUS_INIT
 359 #error "CONFIG_IN_EXTERNAL_FLASH/CONFIG_IN_MEMORY_MAPPED_FLASH and TARGET_BUS_INIT are mutually exclusive"
 360 #endif
 361
 362 #if defined(CONFIG_IN_EXTERNAL_FLASH)
 363     configureSPIBusses();
 364     initFlags |= SPI_BUSSES_INIT_ATTEMPTED;
 365 #endif
 366
 367 #if defined(CONFIG_IN_MEMORY_MAPPED_FLASH) || defined(CONFIG_IN_EXTERNAL_FLASH)
 368     configureQuadSPIBusses();
 369     configureOctoSPIBusses();
 370     initFlags |= QUAD_OCTO_SPI_BUSSES_INIT_ATTEMPTED;
 371 #endif
 372
 373 #ifndef USE_FLASH_CHIP
 374 #error "CONFIG_IN_EXTERNAL_FLASH/CONFIG_IN_MEMORY_MAPPED_FLASH requires USE_FLASH_CHIP to be defined."
 375 #endif
 376
 377     bool haveFlash = flashInit(flashConfig());
 378
 379     if (!haveFlash) {
 380         failureMode(FAILURE_EXTERNAL_FLASH_INIT_FAILED);
 381     }
 382     initFlags |= FLASH_INIT_ATTEMPTED;
 383
 384 #endif // CONFIG_IN_EXTERNAL_FLASH || CONFIG_IN_MEMORY_MAPPED_FLASH
 385
 386
 387     initEEPROM();
 388
 389     ensureEEPROMStructureIsValid();
 390
 391     bool readSuccess = readEEPROM();
 392
 393 #if defined(USE_BOARD_INFO)
 394     initBoardInformation();
 395 #endif
 396
 397     if (!readSuccess || !isEEPROMVersionValid() || strncasecmp(systemConfig()->boardIdentifier, TARGET_BOARD_IDENTIFIER, sizeof(TARGET_BOARD_IDENTIFIER))) {
 398         resetEEPROM();
 399     }
 400
 401     systemState |= SYSTEM_STATE_CONFIG_LOADED;
 402
 403 #ifdef USE_DEBUG_PIN
 404     dbgPinInit();
 405 #endif
 406
 407     debugMode = systemConfig()->debug_mode;
 408
 409 #ifdef TARGET_PREINIT
 410     targetPreInit();
 411 #endif
 412
 413 #if !defined(USE_VIRTUAL_LED)
 414     ledInit(statusLedConfig());
 415 #endif
 416     LED2_ON;
 417
 418 #if !defined(SIMULATOR_BUILD)
 419     EXTIInit();
 420 #endif
 421
 422 #if defined(USE_BUTTONS)
 423
 424     buttonsInit();
 425
 426     delayMicroseconds(10);  // allow configuration to settle // XXX Could be removed, too?
 427
 428     // Allow EEPROM reset with two-button-press without power cycling in DEBUG build
 429 #ifdef DEBUG
 430 #define EEPROM_RESET_PRECONDITION true
 431 #else
 432 #define EEPROM_RESET_PRECONDITION (!isMPUSoftReset())
 433 #endif
 434
 435     if (EEPROM_RESET_PRECONDITION) {
 436 #if defined(BUTTON_A_PIN) && defined(BUTTON_B_PIN)
 437         // two buttons required
 438         uint8_t secondsRemaining = 5;
 439         bool bothButtonsHeld;
 440         do {
 441             bothButtonsHeld = buttonAPressed() && buttonBPressed();
 442             if (bothButtonsHeld) {
 443                 if (--secondsRemaining == 0) {
 444                     resetEEPROM();
 445 #ifdef USE_PERSISTENT_OBJECTS
 446                     persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE);
 447 #endif
 448                     systemReset();
 449                 }
 450                 delay(1000);
 451                 LED0_TOGGLE;
 452             }
 453         } while (bothButtonsHeld);
 454 #endif
 455     }
 456
 457 #undef EEPROM_RESET_PRECONDITION
 458
 459 #endif // USE_BUTTONS
 460
 461     // Note that spektrumBind checks if a call is immediately after
 462     // hard reset (including power cycle), so it should be called before
 463     // systemClockSetHSEValue and OverclockRebootIfNecessary, as these
 464     // may cause soft reset which will prevent spektrumBind not to execute
 465     // the bind procedure.
 466
 467 #if defined(USE_SPEKTRUM_BIND)
 468     if (featureIsEnabled(FEATURE_RX_SERIAL)) {
 469         switch (rxConfig()->serialrx_provider) {
 470         case SERIALRX_SPEKTRUM1024:
 471         case SERIALRX_SPEKTRUM2048:
 472         case SERIALRX_SRXL:
 473             // Spektrum satellite binding if enabled on startup.
 474             // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
 475             // The rest of Spektrum initialization will happen later - via spektrumInit()
 476             spektrumBind(rxConfigMutable());
 477             break;
 478         }
 479     }
 480 #endif
 481
 482 #if defined(STM32F4) || defined(STM32G4)
 483     // F4 has non-8MHz boards
 484     // G4 for Betaflight allow 24 or 27MHz oscillator
 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     // Note that mcoConfigure must be augmented with an additional argument to
 495     // indicate which device instance to configure when MCO and MCO2 are both supported
 496
 497 #if defined(STM32F4) || defined(STM32F7)
 498     // F4 and F7 support MCO on PA8 and MCO2 on PC9, but only MCO2 is supported for now
 499     mcoConfigure(MCODEV_2, mcoConfig(MCODEV_2));
 500 #elif defined(STM32G4)
 501     // G4 only supports one MCO on PA8
 502     mcoConfigure(MCODEV_1, mcoConfig(MCODEV_1));
 503 #else
 504 #error Unsupported MCU
 505 #endif
 506 #endif // USE_MCO
 507
 508 #ifdef USE_TIMER
 509     timerInit();  // timer must be initialized before any channel is allocated
 510 #endif
 511
 512 #ifdef BUS_SWITCH_PIN
 513     busSwitchInit();
 514 #endif
 515
 516 #if defined(USE_UART) && !defined(SIMULATOR_BUILD)
 517     uartPinConfigure(serialPinConfig());
 518 #endif
 519
 520 #if defined(AVOID_UART1_FOR_PWM_PPM)
 521     serialInit(featureIsEnabled(FEATURE_SOFTSERIAL),
 522             featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
 523 #elif defined(AVOID_UART2_FOR_PWM_PPM)
 524     serialInit(featureIsEnabled(FEATURE_SOFTSERIAL),
 525             featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
 526 #elif defined(AVOID_UART3_FOR_PWM_PPM)
 527     serialInit(featureIsEnabled(FEATURE_SOFTSERIAL),
 528             featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
 529 #else
 530     serialInit(featureIsEnabled(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
 531 #endif
 532
 533     mixerInit(mixerConfig()->mixerMode);
 534
 535     uint16_t idlePulse = motorConfig()->mincommand;
 536     if (featureIsEnabled(FEATURE_3D)) {
 537         idlePulse = flight3DConfig()->neutral3d;
 538     }
 539     if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
 540         idlePulse = 0; // brushed motors
 541     }
 542 #ifdef USE_MOTOR
 543     /* Motors needs to be initialized soon as posible because hardware initialization
 544      * may send spurious pulses to esc's causing their early initialization. Also ppm
 545      * receiver may share timer with motors so motors MUST be initialized here. */
 546     motorDevInit(&motorConfig()->dev, idlePulse, getMotorCount());
 547     systemState |= SYSTEM_STATE_MOTORS_READY;
 548 #else
 549     UNUSED(idlePulse);
 550 #endif
 551
 552     if (0) {}
 553 #if defined(USE_RX_PPM)
 554     else if (featureIsEnabled(FEATURE_RX_PPM)) {
 555         ppmRxInit(ppmConfig());
 556     }
 557 #endif
 558 #if defined(USE_RX_PWM)
 559     else if (featureIsEnabled(FEATURE_RX_PARALLEL_PWM)) {
 560         pwmRxInit(pwmConfig());
 561     }
 562 #endif
 563
 564 #ifdef USE_BEEPER
 565     beeperInit(beeperDevConfig());
 566 #endif
 567 /* temp until PGs are implemented. */
 568 #if defined(USE_INVERTER) && !defined(SIMULATOR_BUILD)
 569     initInverters(serialPinConfig());
 570 #endif
 571
 572
 573 #ifdef TARGET_BUS_INIT
 574     targetBusInit();
 575
 576 #else
 577
 578     // Depending on compilation options SPI/QSPI/OSPI initialisation may already be done.
 579     if (!(initFlags & SPI_BUSSES_INIT_ATTEMPTED)) {
 580         configureSPIBusses();
 581         initFlags |= SPI_BUSSES_INIT_ATTEMPTED;
 582     }
 583
 584     if (!(initFlags & QUAD_OCTO_SPI_BUSSES_INIT_ATTEMPTED)) {
 585         configureQuadSPIBusses();
 586         configureOctoSPIBusses();
 587         initFlags |= QUAD_OCTO_SPI_BUSSES_INIT_ATTEMPTED;
 588     }
 589
 590 #if defined(USE_SDCARD_SDIO) && !defined(CONFIG_IN_SDCARD) && defined(STM32H7)
 591     sdioPinConfigure();
 592     SDIO_GPIO_Init();
 593 #endif
 594
 595 #ifdef USE_USB_MSC
 596 /* MSC mode will start after init, but will not allow scheduler to run,
 597  *  so there is no bottleneck in reading and writing data */
 598     mscInit();
 599     if (mscCheckBootAndReset() || mscCheckButton()) {
 600         ledInit(statusLedConfig());
 601
 602 #ifdef USE_SDCARD
 603         if (blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD) {
 604             if (sdcardConfig()->mode) {
 605                 if (!(initFlags & SD_INIT_ATTEMPTED)) {
 606                     sdCardAndFSInit();
 607                     initFlags |= SD_INIT_ATTEMPTED;
 608                 }
 609             }
 610         }
 611 #endif
 612
 613 #if defined(USE_FLASHFS)
 614         // If the blackbox device is onboard flash, then initialize and scan
 615         // it to identify the log files *before* starting the USB device to
 616         // prevent timeouts of the mass storage device.
 617         if (blackboxConfig()->device == BLACKBOX_DEVICE_FLASH) {
 618             emfat_init_files();
 619         }
 620 #endif
 621         // There's no more initialisation to be done, so enable DMA where possible for SPI
 622 #ifdef USE_SPI
 623         spiInitBusDMA();
 624 #endif
 625         if (mscStart() == 0) {
 626              mscWaitForButton();
 627         } else {
 628             systemResetFromMsc();
 629         }
 630     }
 631 #endif
 632
 633 #ifdef USE_PERSISTENT_MSC_RTC
 634     // if we didn't enter MSC mode then clear the persistent RTC value
 635     persistentObjectWrite(PERSISTENT_OBJECT_RTC_HIGH, 0);
 636     persistentObjectWrite(PERSISTENT_OBJECT_RTC_LOW, 0);
 637 #endif
 638
 639 #ifdef USE_I2C
 640     i2cHardwareConfigure(i2cConfig(0));
 641
 642     // Note: Unlike UARTs which are configured when client is present,
 643     // I2C buses are initialized unconditionally if they are configured.
 644
 645 #ifdef USE_I2C_DEVICE_1
 646     i2cInit(I2CDEV_1);
 647 #endif
 648 #ifdef USE_I2C_DEVICE_2
 649     i2cInit(I2CDEV_2);
 650 #endif
 651 #ifdef USE_I2C_DEVICE_3
 652     i2cInit(I2CDEV_3);
 653 #endif
 654 #ifdef USE_I2C_DEVICE_4
 655     i2cInit(I2CDEV_4);
 656 #endif
 657 #endif // USE_I2C
 658
 659 #endif // TARGET_BUS_INIT
 660
 661 #ifdef USE_HARDWARE_REVISION_DETECTION
 662     updateHardwareRevision();
 663 #endif
 664
 665 #ifdef USE_VTX_RTC6705
 666     bool useRTC6705 = rtc6705IOInit(vtxIOConfig());
 667 #endif
 668
 669 #ifdef USE_CAMERA_CONTROL
 670     cameraControlInit();
 671 #endif
 672
 673 #ifdef USE_ADC
 674     adcInit(adcConfig());
 675 #endif
 676
 677     initBoardAlignment(boardAlignment());
 678
 679     if (!sensorsAutodetect()) {
 680         // if gyro was not detected due to whatever reason, notify and don't arm.
 681         if (isSystemConfigured()) {
 682             indicateFailure(FAILURE_MISSING_ACC, 2);
 683         }
 684         setArmingDisabled(ARMING_DISABLED_NO_GYRO);
 685     }
 686
 687     systemState |= SYSTEM_STATE_SENSORS_READY;
 688
 689     // Set the targetLooptime based on the detected gyro sampleRateHz and pid_process_denom
 690     gyroSetTargetLooptime(pidConfig()->pid_process_denom);
 691
 692     // Validate and correct the gyro config or PID loop time if needed
 693     validateAndFixGyroConfig();
 694
 695     // Now reset the targetLooptime as it's possible for the validation to change the pid_process_denom
 696     gyroSetTargetLooptime(pidConfig()->pid_process_denom);
 697
 698 #ifdef USE_DSHOT_TELEMETRY
 699     // Initialize the motor frequency filter now that we have a target looptime
 700     initDshotTelemetry(gyro.targetLooptime);
 701 #endif
 702
 703     // Finally initialize the gyro filtering
 704     gyroInitFilters();
 705
 706     pidInit(currentPidProfile);
 707
 708     mixerInitProfile();
 709
 710 #ifdef USE_PID_AUDIO
 711     pidAudioInit();
 712 #endif
 713
 714 #ifdef USE_SERVOS
 715     servosInit();
 716     if (isMixerUsingServos()) {
 717         //pwm_params.useChannelForwarding = featureIsEnabled(FEATURE_CHANNEL_FORWARDING);
 718         servoDevInit(&servoConfig()->dev);
 719     }
 720     servosFilterInit();
 721 #endif
 722
 723 #ifdef USE_PINIO
 724     pinioInit(pinioConfig());
 725 #endif
 726
 727 #ifdef USE_PIN_PULL_UP_DOWN
 728     pinPullupPulldownInit();
 729 #endif
 730
 731 #ifdef USE_PINIOBOX
 732     pinioBoxInit(pinioBoxConfig());
 733 #endif
 734
 735     LED1_ON;
 736     LED0_OFF;
 737     LED2_OFF;
 738
 739     for (int i = 0; i < 10; i++) {
 740         LED1_TOGGLE;
 741         LED0_TOGGLE;
 742 #if defined(USE_BEEPER)
 743         delay(25);
 744         if (!(beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(BEEPER_SYSTEM_INIT))) {
 745             BEEP_ON;
 746         }
 747         delay(25);
 748         BEEP_OFF;
 749 #else
 750         delay(50);
 751 #endif
 752     }
 753     LED0_OFF;
 754     LED1_OFF;
 755
 756     imuInit();
 757
 758     failsafeInit();
 759
 760     rxInit();
 761
 762 #ifdef USE_GPS
 763     if (featureIsEnabled(FEATURE_GPS)) {
 764         gpsInit();
 765 #ifdef USE_GPS_RESCUE
 766         gpsRescueInit();
 767 #endif
 768 #ifdef USE_GPS_LAP_TIMER
 769         gpsLapTimerInit();
 770 #endif // USE_GPS_LAP_TIMER
 771     }
 772 #endif
 773
 774 #ifdef USE_LED_STRIP
 775     ledStripInit();
 776
 777     if (featureIsEnabled(FEATURE_LED_STRIP)) {
 778         ledStripEnable();
 779     }
 780 #endif
 781
 782 #ifdef USE_ESC_SENSOR
 783     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
 784         escSensorInit();
 785     }
 786 #endif
 787
 788 #ifdef USE_USB_DETECT
 789     usbCableDetectInit();
 790 #endif
 791
 792 #ifdef USE_TRANSPONDER
 793     if (featureIsEnabled(FEATURE_TRANSPONDER)) {
 794         transponderInit();
 795         transponderStartRepeating();
 796         systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
 797     }
 798 #endif
 799
 800 #ifdef USE_FLASH_CHIP
 801     if (!(initFlags & FLASH_INIT_ATTEMPTED)) {
 802         flashInit(flashConfig());
 803         initFlags |= FLASH_INIT_ATTEMPTED;
 804     }
 805 #endif
 806 #ifdef USE_FLASHFS
 807     flashfsInit();
 808 #endif
 809
 810 #ifdef USE_SDCARD
 811     if (sdcardConfig()->mode) {
 812         if (!(initFlags & SD_INIT_ATTEMPTED)) {
 813             sdCardAndFSInit();
 814             initFlags |= SD_INIT_ATTEMPTED;
 815         }
 816     }
 817 #endif
 818 #ifdef USE_BLACKBOX
 819     blackboxInit();
 820 #endif
 821
 822 #ifdef USE_ACC
 823     if (mixerConfig()->mixerMode == MIXER_GIMBAL) {
 824         accStartCalibration();
 825     }
 826 #endif
 827     gyroStartCalibration(false);
 828 #ifdef USE_BARO
 829     baroStartCalibration();
 830 #endif
 831     positionInit();
 832
 833 #if defined(USE_VTX_COMMON) || defined(USE_VTX_CONTROL)
 834     vtxTableInit();
 835 #endif
 836
 837 #ifdef USE_VTX_CONTROL
 838     vtxControlInit();
 839
 840 #if defined(USE_VTX_COMMON)
 841     vtxCommonInit();
 842 #endif
 843
 844 #ifdef USE_VTX_MSP
 845     vtxMspInit();
 846 #endif
 847
 848 #ifdef USE_VTX_SMARTAUDIO
 849     vtxSmartAudioInit();
 850 #endif
 851
 852 #ifdef USE_VTX_TRAMP
 853     vtxTrampInit();
 854 #endif
 855
 856 #ifdef USE_VTX_RTC6705
 857     if (!vtxCommonDevice() && useRTC6705) { // external VTX takes precedence when configured.
 858         vtxRTC6705Init();
 859     }
 860 #endif
 861
 862 #endif // VTX_CONTROL
 863
 864     batteryInit(); // always needs doing, regardless of features.
 865
 866 #ifdef USE_RCDEVICE
 867     rcdeviceInit();
 868 #endif // USE_RCDEVICE
 869
 870 #ifdef USE_PERSISTENT_STATS
 871     statsInit();
 872 #endif
 873
 874     // Initialize MSP
 875     mspInit();
 876     mspSerialInit();
 877
 878 /*
 879  * CMS, display devices and OSD
 880  */
 881 #ifdef USE_CMS
 882     cmsInit();
 883 #endif
 884
 885 #if (defined(USE_OSD) || (defined(USE_MSP_DISPLAYPORT) && defined(USE_CMS)))
 886     displayPort_t *osdDisplayPort = NULL;
 887 #endif
 888
 889 #if defined(USE_OSD)
 890     osdDisplayPortDevice_e osdDisplayPortDevice = OSD_DISPLAYPORT_DEVICE_NONE;
 891
 892     //The OSD need to be initialised after GYRO to avoid GYRO initialisation failure on some targets
 893
 894     if (featureIsEnabled(FEATURE_OSD)) {
 895         osdDisplayPortDevice_e device = osdConfig()->displayPortDevice;
 896
 897         switch(device) {
 898
 899         case OSD_DISPLAYPORT_DEVICE_AUTO:
 900             FALLTHROUGH;
 901
 902 #if defined(USE_FRSKYOSD)
 903         // Test OSD_DISPLAYPORT_DEVICE_FRSKYOSD first, since an FC could
 904         // have a builtin MAX7456 but also an FRSKYOSD connected to an
 905         // uart.
 906         case OSD_DISPLAYPORT_DEVICE_FRSKYOSD:
 907             osdDisplayPort = frskyOsdDisplayPortInit(vcdProfile()->video_system);
 908             if (osdDisplayPort || device == OSD_DISPLAYPORT_DEVICE_FRSKYOSD) {
 909                 osdDisplayPortDevice = OSD_DISPLAYPORT_DEVICE_FRSKYOSD;
 910                 break;
 911             }
 912             FALLTHROUGH;
 913 #endif
 914
 915 #if defined(USE_MAX7456)
 916         case OSD_DISPLAYPORT_DEVICE_MAX7456:
 917             // If there is a max7456 chip for the OSD configured and detected then use it.
 918             if (max7456DisplayPortInit(vcdProfile(), &osdDisplayPort) || device == OSD_DISPLAYPORT_DEVICE_MAX7456) {
 919                 osdDisplayPortDevice = OSD_DISPLAYPORT_DEVICE_MAX7456;
 920                 break;
 921             }
 922             FALLTHROUGH;
 923 #endif
 924
 925 #if defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT) && defined(USE_OSD_OVER_MSP_DISPLAYPORT)
 926         case OSD_DISPLAYPORT_DEVICE_MSP:
 927             osdDisplayPort = displayPortMspInit();
 928             if (osdDisplayPort || device == OSD_DISPLAYPORT_DEVICE_MSP) {
 929                 osdDisplayPortDevice = OSD_DISPLAYPORT_DEVICE_MSP;
 930                 break;
 931             }
 932             FALLTHROUGH;
 933 #endif
 934
 935         // Other device cases can be added here
 936
 937         case OSD_DISPLAYPORT_DEVICE_NONE:
 938         default:
 939             break;
 940         }
 941
 942         // osdInit will register with CMS by itself.
 943         osdInit(osdDisplayPort, osdDisplayPortDevice);
 944
 945         if (osdDisplayPortDevice == OSD_DISPLAYPORT_DEVICE_NONE) {
 946             featureDisableImmediate(FEATURE_OSD);
 947         }
 948     }
 949 #endif // USE_OSD
 950
 951 #if defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT)
 952     // If BFOSD is not active, then register MSP_DISPLAYPORT as a CMS device.
 953     if (!osdDisplayPort) {
 954         cmsDisplayPortRegister(displayPortMspInit());
 955     }
 956 #endif
 957
 958 #ifdef USE_DASHBOARD
 959     // Dashbord will register with CMS by itself.
 960     if (featureIsEnabled(FEATURE_DASHBOARD)) {
 961         dashboardInit();
 962 #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
 963         dashboardShowFixedPage(PAGE_GPS);
 964 #else
 965         dashboardResetPageCycling();
 966         dashboardEnablePageCycling();
 967 #endif
 968     }
 969 #endif
 970
 971 #ifdef USE_TELEMETRY
 972     // Telemetry will initialise displayport and register with CMS by itself.
 973     if (featureIsEnabled(FEATURE_TELEMETRY)) {
 974         telemetryInit();
 975     }
 976 #endif
 977
 978     setArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME);
 979
 980 // allocate SPI DMA streams before motor timers
 981 #if defined(USE_SPI) && defined(USE_SPI_DMA_ENABLE_EARLY)
 982     // Attempt to enable DMA on all SPI busses
 983     spiInitBusDMA();
 984 #endif
 985
 986 #ifdef USE_MOTOR
 987     motorPostInit();
 988     motorEnable();
 989 #endif
 990
 991 // allocate SPI DMA streams after motor timers as SPI DMA allocate will always be possible
 992 #if defined(USE_SPI) && defined(USE_SPI_DMA_ENABLE_LATE) && !defined(USE_SPI_DMA_ENABLE_EARLY)
 993     // Attempt to enable DMA on all SPI busses
 994     spiInitBusDMA();
 995 #endif
 996
 997     debugInit();
 998
 999     unusedPinsInit();
1000
1001     tasksInit();
1002
1003     systemState |= SYSTEM_STATE_READY;
1004 }