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