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