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/gimbal_control.h"
 117 #include "io/gps.h"
 118 #include "io/ledstrip.h"
 119 #include "io/pidaudio.h"
 120 #include "io/piniobox.h"
 121 #include "io/rcdevice_cam.h"
 122 #include "io/serial.h"
 123 #include "io/transponder_ir.h"
 124 #include "io/vtx.h"
 125 #include "io/vtx_control.h"
 126 #include "io/vtx_msp.h"
 127 #include "io/vtx_rtc6705.h"
 128 #include "io/vtx_smartaudio.h"
 129 #include "io/vtx_tramp.h"
 130
 131 #include "msc/emfat_file.h"
 132 #ifdef USE_PERSISTENT_MSC_RTC
 133 #include "msc/usbd_storage.h"
 134 #endif
 135
 136 #include "msp/msp.h"
 137 #include "msp/msp_serial.h"
 138
 139 #include "osd/osd.h"
 140
 141 #include "pg/adc.h"
 142 #include "pg/beeper.h"
 143 #include "pg/beeper_dev.h"
 144 #include "pg/bus_i2c.h"
 145 #include "pg/bus_spi.h"
 146 #include "pg/bus_quadspi.h"
 147 #include "pg/flash.h"
 148 #include "pg/mco.h"
 149 #include "pg/motor.h"
 150 #include "pg/pinio.h"
 151 #include "pg/piniobox.h"
 152 #include "pg/pin_pull_up_down.h"
 153 #include "pg/pg.h"
 154 #include "pg/rx.h"
 155 #include "pg/rx_pwm.h"
 156 #include "pg/rx_spi.h"
 157 #include "pg/sdcard.h"
 158 #include "pg/vcd.h"
 159 #include "pg/vtx_io.h"
 160
 161 #include "rx/rx.h"
 162 #include "rx/spektrum.h"
 163
 164 #include "scheduler/scheduler.h"
 165
 166 #include "sensors/acceleration.h"
 167 #include "sensors/barometer.h"
 168 #include "sensors/battery.h"
 169 #include "sensors/boardalignment.h"
 170 #include "sensors/compass.h"
 171 #include "sensors/esc_sensor.h"
 172 #include "sensors/gyro.h"
 173 #include "sensors/gyro_init.h"
 174 #include "sensors/initialisation.h"
 175
 176 #include "telemetry/telemetry.h"
 177
 178 #ifdef USE_HARDWARE_REVISION_DETECTION
 179 #include "hardware_revision.h"
 180 #endif
 181
 182 #ifdef TARGET_PREINIT
 183 void targetPreInit(void);
 184 #endif
 185
 186 uint8_t systemState = SYSTEM_STATE_INITIALISING;
 187
 188 #ifdef BUS_SWITCH_PIN
 189 void busSwitchInit(void)
 190 {
 191     IO_t busSwitchResetPin = IO_NONE;
 192
 193     busSwitchResetPin = IOGetByTag(IO_TAG(BUS_SWITCH_PIN));
 194     IOInit(busSwitchResetPin, OWNER_SYSTEM, 0);
 195     IOConfigGPIO(busSwitchResetPin, IOCFG_OUT_PP);
 196
 197     // ENABLE
 198     IOLo(busSwitchResetPin);
 199 }
 200 #endif
 201
 202 static void configureSPIBusses(void)
 203 {
 204 #ifdef USE_SPI
 205     spiPinConfigure(spiPinConfig(0));
 206 #endif
 207
 208     sensorsPreInit();
 209
 210 #ifdef USE_SPI
 211     spiPreinit();
 212
 213 #ifdef USE_SPI_DEVICE_1
 214     spiInit(SPIDEV_1);
 215 #endif
 216 #ifdef USE_SPI_DEVICE_2
 217     spiInit(SPIDEV_2);
 218 #endif
 219 #ifdef USE_SPI_DEVICE_3
 220     spiInit(SPIDEV_3);
 221 #endif
 222 #ifdef USE_SPI_DEVICE_4
 223     spiInit(SPIDEV_4);
 224 #endif
 225 #ifdef USE_SPI_DEVICE_5
 226     spiInit(SPIDEV_5);
 227 #endif
 228 #ifdef USE_SPI_DEVICE_6
 229     spiInit(SPIDEV_6);
 230 #endif
 231 #endif // USE_SPI
 232 }
 233
 234 static void configureQuadSPIBusses(void)
 235 {
 236 #ifdef USE_QUADSPI
 237     quadSpiPinConfigure(quadSpiConfig(0));
 238
 239 #ifdef USE_QUADSPI_DEVICE_1
 240     quadSpiInit(QUADSPIDEV_1);
 241 #endif
 242 #endif // USE_QUADSPI
 243 }
 244
 245 static void configureOctoSPIBusses(void)
 246 {
 247 #ifdef USE_OCTOSPI
 248 #ifdef USE_OCTOSPI_DEVICE_1
 249     octoSpiInit(OCTOSPIDEV_1);
 250 #endif
 251 #endif
 252 }
 253
 254 #ifdef USE_SDCARD
 255 static void sdCardAndFSInit(void)
 256 {
 257     sdcard_init(sdcardConfig());
 258     afatfs_init();
 259 }
 260 #endif
 261
 262 void init(void)
 263 {
 264 #if SERIAL_PORT_COUNT > 0
 265     printfSerialInit();
 266 #endif
 267
 268     systemInit();
 269
 270     // Initialize task data as soon as possible. Has to be done before tasksInit(),
 271     // and any init code that may try to modify task behaviour before tasksInit().
 272     tasksInitData();
 273
 274     // initialize IO (needed for all IO operations)
 275     IOInitGlobal();
 276
 277 #ifdef USE_HARDWARE_REVISION_DETECTION
 278     detectHardwareRevision();
 279 #endif
 280
 281 #if defined(USE_TARGET_CONFIG)
 282     // Call once before the config is loaded for any target specific configuration required to support loading the config
 283     targetConfiguration();
 284 #endif
 285
 286     enum {
 287         FLASH_INIT_ATTEMPTED                = (1 << 0),
 288         SD_INIT_ATTEMPTED                   = (1 << 1),
 289         SPI_BUSSES_INIT_ATTEMPTED           = (1 << 2),
 290         QUAD_OCTO_SPI_BUSSES_INIT_ATTEMPTED = (1 << 3),
 291     };
 292     uint8_t initFlags = 0;
 293
 294 #ifdef CONFIG_IN_SDCARD
 295
 296     //
 297     // Config in sdcard presents an issue with pin configuration since the pin and sdcard configs for the
 298     // sdcard are in the config which is on the sdcard which we can't read yet!
 299     //
 300     // FIXME For now, users must NOT change flash/pin configs needed for the system to boot and/or to save the config.
 301     // One possible solution is to lock the pins for the flash chip so they cannot be modified post-boot.
 302     //
 303     // note that target specific SDCARD/SDIO/SPI/QUADSPI/OCTOSPI 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/OCTOSPI interface as the flash chip do not
 310     // cause communication issues with the flash chip.  e.g. use external pullups on SPI/QUADSPI/OCTOSPI 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 #ifdef USE_SDCARD_SPI
 320     configureSPIBusses();
 321     initFlags |= SPI_BUSSES_INIT_ATTEMPTED;
 322 #endif
 323
 324     sdCardAndFSInit();
 325     initFlags |= SD_INIT_ATTEMPTED;
 326
 327     if (!sdcard_isInserted()) {
 328         failureMode(FAILURE_SDCARD_REQUIRED);
 329     }
 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 #if defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_MEMORY_MAPPED_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 For now, users must NOT change flash/pin configs needed for the system to boot and/or to save the config.
 347     // One possible solution is to lock the pins for the flash chip so they cannot be modified post-boot.
 348     //
 349     // note that target specific FLASH/SPI/QUADSPI/OCTOSPI configs are
 350     // also not supported in USE_TARGET_CONFIG/targetConfigure() when using CONFIG_IN_EXTERNAL_FLASH/CONFIG_IN_MEMORY_MAPPED_FLASH.
 351     //
 352
 353     //
 354     // IMPORTANT: all default flash and pin configurations must be valid for the target after pgResetAll() is called.
 355     // Target designers must ensure other devices connected the same SPI/QUADSPI/OCTOSPI interface as the flash chip do not
 356     // cause communication issues with the flash chip.  e.g. use external pullups on SPI/QUADSPI/OCTOSPI CS lines.
 357     //
 358     pgResetAll();
 359
 360 #ifdef TARGET_BUS_INIT
 361 #error "CONFIG_IN_EXTERNAL_FLASH/CONFIG_IN_MEMORY_MAPPED_FLASH and TARGET_BUS_INIT are mutually exclusive"
 362 #endif
 363
 364 #if defined(CONFIG_IN_EXTERNAL_FLASH)
 365     configureSPIBusses();
 366     initFlags |= SPI_BUSSES_INIT_ATTEMPTED;
 367 #endif
 368
 369 #if defined(CONFIG_IN_MEMORY_MAPPED_FLASH) || defined(CONFIG_IN_EXTERNAL_FLASH)
 370     configureQuadSPIBusses();
 371     configureOctoSPIBusses();
 372     initFlags |= QUAD_OCTO_SPI_BUSSES_INIT_ATTEMPTED;
 373 #endif
 374
 375 #ifndef USE_FLASH_CHIP
 376 #error "CONFIG_IN_EXTERNAL_FLASH/CONFIG_IN_MEMORY_MAPPED_FLASH requires USE_FLASH_CHIP to be defined."
 377 #endif
 378
 379     bool haveFlash = flashInit(flashConfig());
 380
 381     if (!haveFlash) {
 382         failureMode(FAILURE_EXTERNAL_FLASH_INIT_FAILED);
 383     }
 384     initFlags |= FLASH_INIT_ATTEMPTED;
 385
 386 #endif // CONFIG_IN_EXTERNAL_FLASH || CONFIG_IN_MEMORY_MAPPED_FLASH
 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 #ifdef TARGET_BUS_INIT
 578     targetBusInit();
 579
 580 #else
 581
 582     // Depending on compilation options SPI/QSPI/OSPI initialisation may already be done.
 583     if (!(initFlags & SPI_BUSSES_INIT_ATTEMPTED)) {
 584         configureSPIBusses();
 585         initFlags |= SPI_BUSSES_INIT_ATTEMPTED;
 586     }
 587
 588     if (!(initFlags & QUAD_OCTO_SPI_BUSSES_INIT_ATTEMPTED)) {
 589         configureQuadSPIBusses();
 590         configureOctoSPIBusses();
 591         initFlags |= QUAD_OCTO_SPI_BUSSES_INIT_ATTEMPTED;
 592     }
 593
 594 #if defined(USE_SDCARD_SDIO) && !defined(CONFIG_IN_SDCARD) && defined(STM32H7)
 595     sdioPinConfigure();
 596     SDIO_GPIO_Init();
 597 #endif
 598
 599 #ifdef USE_USB_MSC
 600 /* MSC mode will start after init, but will not allow scheduler to run,
 601  *  so there is no bottleneck in reading and writing data */
 602     mscInit();
 603     if (mscCheckBootAndReset() || mscCheckButton()) {
 604         ledInit(statusLedConfig());
 605
 606 #ifdef USE_SDCARD
 607         if (blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD) {
 608             if (sdcardConfig()->mode) {
 609                 if (!(initFlags & SD_INIT_ATTEMPTED)) {
 610                     sdCardAndFSInit();
 611                     initFlags |= SD_INIT_ATTEMPTED;
 612                 }
 613             }
 614         }
 615 #endif
 616
 617 #if defined(USE_FLASHFS)
 618         // If the blackbox device is onboard flash, then initialize and scan
 619         // it to identify the log files *before* starting the USB device to
 620         // prevent timeouts of the mass storage device.
 621         if (blackboxConfig()->device == BLACKBOX_DEVICE_FLASH) {
 622             emfat_init_files();
 623         }
 624 #endif
 625         // There's no more initialisation to be done, so enable DMA where possible for SPI
 626 #ifdef USE_SPI
 627         spiInitBusDMA();
 628 #endif
 629         if (mscStart() == 0) {
 630              mscWaitForButton();
 631         } else {
 632             systemResetFromMsc();
 633         }
 634     }
 635 #endif
 636
 637 #ifdef USE_PERSISTENT_MSC_RTC
 638     // if we didn't enter MSC mode then clear the persistent RTC value
 639     persistentObjectWrite(PERSISTENT_OBJECT_RTC_HIGH, 0);
 640     persistentObjectWrite(PERSISTENT_OBJECT_RTC_LOW, 0);
 641 #endif
 642
 643 #ifdef USE_I2C
 644     i2cHardwareConfigure(i2cConfig(0));
 645
 646     // Note: Unlike UARTs which are configured when client is present,
 647     // I2C buses are initialized unconditionally if they are configured.
 648
 649 #ifdef USE_I2C_DEVICE_1
 650     i2cInit(I2CDEV_1);
 651 #endif
 652 #ifdef USE_I2C_DEVICE_2
 653     i2cInit(I2CDEV_2);
 654 #endif
 655 #ifdef USE_I2C_DEVICE_3
 656     i2cInit(I2CDEV_3);
 657 #endif
 658 #ifdef USE_I2C_DEVICE_4
 659     i2cInit(I2CDEV_4);
 660 #endif
 661 #endif // USE_I2C
 662
 663 #endif // TARGET_BUS_INIT
 664
 665 #ifdef USE_HARDWARE_REVISION_DETECTION
 666     updateHardwareRevision();
 667 #endif
 668
 669 #ifdef USE_VTX_RTC6705
 670     bool useRTC6705 = rtc6705IOInit(vtxIOConfig());
 671 #endif
 672
 673 #ifdef USE_CAMERA_CONTROL
 674     cameraControlInit();
 675 #endif
 676
 677 #ifdef USE_ADC
 678     adcInit(adcConfig());
 679 #endif
 680
 681     initBoardAlignment(boardAlignment());
 682
 683     if (!sensorsAutodetect()) {
 684         // if gyro was not detected due to whatever reason, notify and don't arm.
 685         if (isSystemConfigured()) {
 686             indicateFailure(FAILURE_MISSING_ACC, 2);
 687         }
 688         setArmingDisabled(ARMING_DISABLED_NO_GYRO);
 689     }
 690
 691     systemState |= SYSTEM_STATE_SENSORS_READY;
 692
 693     // Set the targetLooptime based on the detected gyro sampleRateHz and pid_process_denom
 694     gyroSetTargetLooptime(pidConfig()->pid_process_denom);
 695
 696     // Validate and correct the gyro config or PID loop time if needed
 697     validateAndFixGyroConfig();
 698
 699     // Now reset the targetLooptime as it's possible for the validation to change the pid_process_denom
 700     gyroSetTargetLooptime(pidConfig()->pid_process_denom);
 701
 702 #if defined(USE_DSHOT_TELEMETRY) || defined(USE_ESC_SENSOR)
 703     // Initialize the motor frequency filter now that we have a target looptime
 704     initDshotTelemetry(gyro.targetLooptime);
 705 #endif
 706
 707     // Finally initialize the gyro filtering
 708     gyroInitFilters();
 709
 710     pidInit(currentPidProfile);
 711
 712     mixerInitProfile();
 713
 714 #ifdef USE_PID_AUDIO
 715     pidAudioInit();
 716 #endif
 717
 718 #ifdef USE_SERVOS
 719     servosInit();
 720     if (isMixerUsingServos()) {
 721         //pwm_params.useChannelForwarding = featureIsEnabled(FEATURE_CHANNEL_FORWARDING);
 722         servoDevInit(&servoConfig()->dev);
 723     }
 724     servosFilterInit();
 725 #endif
 726
 727 #ifdef USE_PINIO
 728     pinioInit(pinioConfig());
 729 #endif
 730
 731 #ifdef USE_PIN_PULL_UP_DOWN
 732     pinPullupPulldownInit();
 733 #endif
 734
 735 #ifdef USE_PINIOBOX
 736     pinioBoxInit(pinioBoxConfig());
 737 #endif
 738
 739     LED1_ON;
 740     LED0_OFF;
 741     LED2_OFF;
 742
 743     for (int i = 0; i < 10; i++) {
 744         LED1_TOGGLE;
 745         LED0_TOGGLE;
 746 #if defined(USE_BEEPER)
 747         delay(25);
 748         if (!(beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(BEEPER_SYSTEM_INIT))) {
 749             BEEP_ON;
 750         }
 751         delay(25);
 752         BEEP_OFF;
 753 #else
 754         delay(50);
 755 #endif
 756     }
 757     LED0_OFF;
 758     LED1_OFF;
 759
 760     imuInit();
 761
 762     failsafeInit();
 763
 764     rxInit();
 765
 766 #ifdef USE_GPS
 767     if (featureIsEnabled(FEATURE_GPS)) {
 768         gpsInit();
 769 #ifdef USE_GPS_LAP_TIMER
 770         gpsLapTimerInit();
 771 #endif // USE_GPS_LAP_TIMER
 772     }
 773 #endif
 774
 775 #ifdef USE_LED_STRIP
 776     ledStripInit();
 777
 778     if (featureIsEnabled(FEATURE_LED_STRIP)) {
 779         ledStripEnable();
 780     }
 781 #endif
 782
 783 #ifdef USE_ESC_SENSOR
 784     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
 785         escSensorInit();
 786     }
 787 #endif
 788
 789 #ifdef USE_USB_DETECT
 790     usbCableDetectInit();
 791 #endif
 792
 793 #ifdef USE_TRANSPONDER
 794     if (featureIsEnabled(FEATURE_TRANSPONDER)) {
 795         transponderInit();
 796         transponderStartRepeating();
 797         systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
 798     }
 799 #endif
 800
 801 #ifdef USE_FLASH_CHIP
 802     if (!(initFlags & FLASH_INIT_ATTEMPTED)) {
 803         flashInit(flashConfig());
 804         initFlags |= FLASH_INIT_ATTEMPTED;
 805     }
 806 #endif
 807 #ifdef USE_FLASHFS
 808     flashfsInit();
 809 #endif
 810
 811 #ifdef USE_SDCARD
 812     if (sdcardConfig()->mode) {
 813         if (!(initFlags & SD_INIT_ATTEMPTED)) {
 814             sdCardAndFSInit();
 815             initFlags |= SD_INIT_ATTEMPTED;
 816         }
 817     }
 818 #endif
 819 #ifdef USE_BLACKBOX
 820     blackboxInit();
 821 #endif
 822
 823 #ifdef USE_ACC
 824     if (mixerConfig()->mixerMode == MIXER_GIMBAL) {
 825         accStartCalibration();
 826     }
 827 #endif
 828     gyroStartCalibration(false);
 829 #ifdef USE_BARO
 830     baroStartCalibration();
 831 #endif
 832
 833     positionInit();
 834     autopilotInit(autopilotConfig());
 835
 836 #if defined(USE_VTX_COMMON) || defined(USE_VTX_CONTROL)
 837     vtxTableInit();
 838 #endif
 839
 840 #ifdef USE_VTX_CONTROL
 841     vtxControlInit();
 842
 843 #if defined(USE_VTX_COMMON)
 844     vtxCommonInit();
 845 #endif
 846
 847 #ifdef USE_VTX_MSP
 848     vtxMspInit();
 849 #endif
 850
 851 #ifdef USE_VTX_SMARTAUDIO
 852     vtxSmartAudioInit();
 853 #endif
 854
 855 #ifdef USE_VTX_TRAMP
 856     vtxTrampInit();
 857 #endif
 858
 859 #ifdef USE_VTX_RTC6705
 860     if (!vtxCommonDevice() && useRTC6705) { // external VTX takes precedence when configured.
 861         vtxRTC6705Init();
 862     }
 863 #endif
 864
 865 #endif // VTX_CONTROL
 866
 867 #ifdef USE_GIMBAL
 868     gimbalInit();
 869 #endif
 870
 871     batteryInit(); // always needs doing, regardless of features.
 872
 873 #ifdef USE_RCDEVICE
 874     rcdeviceInit();
 875 #endif // USE_RCDEVICE
 876
 877 #ifdef USE_PERSISTENT_STATS
 878     statsInit();
 879 #endif
 880
 881     // Initialize MSP
 882     mspInit();
 883     mspSerialInit();
 884
 885 /*
 886  * CMS, display devices and OSD
 887  */
 888 #ifdef USE_CMS
 889     cmsInit();
 890 #endif
 891
 892 #if (defined(USE_OSD) || (defined(USE_MSP_DISPLAYPORT) && defined(USE_CMS)))
 893     displayPort_t *osdDisplayPort = NULL;
 894 #endif
 895
 896 #if defined(USE_OSD)
 897     osdDisplayPortDevice_e osdDisplayPortDevice = OSD_DISPLAYPORT_DEVICE_NONE;
 898
 899     //The OSD need to be initialised after GYRO to avoid GYRO initialisation failure on some targets
 900
 901     if (featureIsEnabled(FEATURE_OSD)) {
 902         osdDisplayPortDevice_e device;
 903
 904         if (vcdProfile()->video_system == VIDEO_SYSTEM_HD) {
 905             device = OSD_DISPLAYPORT_DEVICE_MSP;
 906         } else {
 907             device = osdConfig()->displayPortDevice;
 908         }
 909
 910         switch(device) {
 911
 912         case OSD_DISPLAYPORT_DEVICE_AUTO:
 913             FALLTHROUGH;
 914
 915 #if defined(USE_FRSKYOSD)
 916         // Test OSD_DISPLAYPORT_DEVICE_FRSKYOSD first, since an FC could
 917         // have a builtin MAX7456 but also an FRSKYOSD connected to an
 918         // uart.
 919         case OSD_DISPLAYPORT_DEVICE_FRSKYOSD:
 920             osdDisplayPort = frskyOsdDisplayPortInit(vcdProfile()->video_system);
 921             if (osdDisplayPort || device == OSD_DISPLAYPORT_DEVICE_FRSKYOSD) {
 922                 osdDisplayPortDevice = OSD_DISPLAYPORT_DEVICE_FRSKYOSD;
 923                 break;
 924             }
 925             FALLTHROUGH;
 926 #endif
 927
 928 #if defined(USE_MAX7456)
 929         case OSD_DISPLAYPORT_DEVICE_MAX7456:
 930             // If there is a max7456 chip for the OSD configured and detected then use it.
 931             if (max7456DisplayPortInit(vcdProfile(), &osdDisplayPort) || device == OSD_DISPLAYPORT_DEVICE_MAX7456) {
 932                 osdDisplayPortDevice = OSD_DISPLAYPORT_DEVICE_MAX7456;
 933                 break;
 934             }
 935             FALLTHROUGH;
 936 #endif
 937
 938 #if defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT) && defined(USE_OSD_OVER_MSP_DISPLAYPORT)
 939         case OSD_DISPLAYPORT_DEVICE_MSP:
 940             osdDisplayPort = displayPortMspInit();
 941             if (osdDisplayPort || device == OSD_DISPLAYPORT_DEVICE_MSP) {
 942                 osdDisplayPortDevice = OSD_DISPLAYPORT_DEVICE_MSP;
 943                 break;
 944             }
 945             FALLTHROUGH;
 946 #endif
 947
 948         // Other device cases can be added here
 949
 950         case OSD_DISPLAYPORT_DEVICE_NONE:
 951         default:
 952             break;
 953         }
 954
 955         // osdInit will register with CMS by itself.
 956         osdInit(osdDisplayPort, osdDisplayPortDevice);
 957
 958         if (osdDisplayPortDevice == OSD_DISPLAYPORT_DEVICE_NONE) {
 959             featureDisableImmediate(FEATURE_OSD);
 960         }
 961     }
 962 #endif // USE_OSD
 963
 964 #if defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT)
 965     // If BFOSD is not active, then register MSP_DISPLAYPORT as a CMS device.
 966     if (!osdDisplayPort) {
 967         cmsDisplayPortRegister(displayPortMspInit());
 968     }
 969 #endif
 970
 971 #ifdef USE_DASHBOARD
 972     // Dashbord will register with CMS by itself.
 973     if (featureIsEnabled(FEATURE_DASHBOARD)) {
 974         dashboardInit();
 975 #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
 976         dashboardShowFixedPage(PAGE_GPS);
 977 #else
 978         dashboardResetPageCycling();
 979         dashboardEnablePageCycling();
 980 #endif
 981     }
 982 #endif
 983
 984 #ifdef USE_TELEMETRY
 985     // Telemetry will initialise displayport and register with CMS by itself.
 986     if (featureIsEnabled(FEATURE_TELEMETRY)) {
 987         telemetryInit();
 988     }
 989 #endif
 990
 991     setArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME);
 992
 993 // allocate SPI DMA streams before motor timers
 994 #if defined(USE_SPI) && defined(USE_SPI_DMA_ENABLE_EARLY)
 995     // Attempt to enable DMA on all SPI busses
 996     spiInitBusDMA();
 997 #endif
 998
 999 #ifdef USE_MOTOR
1000     motorPostInit();
1001     motorEnable();
1002 #endif
1003
1004 // allocate SPI DMA streams after motor timers as SPI DMA allocate will always be possible
1005 #if defined(USE_SPI) && defined(USE_SPI_DMA_ENABLE_LATE) && !defined(USE_SPI_DMA_ENABLE_EARLY)
1006     // Attempt to enable DMA on all SPI busses
1007     spiInitBusDMA();
1008 #endif
1009
1010 // autopilot must be initialised before modes that require the autopilot pids
1011 #ifdef USE_ALT_HOLD_MODE
1012     altHoldInit();
1013 #endif
1014
1015 #ifdef USE_GPS_RESCUE
1016     if (featureIsEnabled(FEATURE_GPS)) {
1017         gpsRescueInit();
1018     }
1019 #endif
1020
1021     debugInit();
1022
1023     unusedPinsInit();
1024
1025     tasksInit();
1026
1027     systemState |= SYSTEM_STATE_READY;
1028 }