src/main/fc/init.c

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