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 "cli/cli.h"
  34
  35 #include "cms/cms.h"
  36 #include "cms/cms_types.h"
  37
  38 #include "common/axis.h"
  39 #include "common/color.h"
  40 #include "common/maths.h"
  41 #include "common/printf_serial.h"
  42
  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/pwm_esc_detect.h"
  65 #include "drivers/pwm_output.h"
  66 #include "drivers/rx/rx_pwm.h"
  67 #include "drivers/sensor.h"
  68 #include "drivers/serial.h"
  69 #include "drivers/serial_softserial.h"
  70 #include "drivers/serial_uart.h"
  71 #include "drivers/sdcard.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 "fc/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/pg.h"
 145 #include "pg/rx.h"
 146 #include "pg/rx_spi.h"
 147 #include "pg/rx_pwm.h"
 148 #include "pg/sdcard.h"
 149 #include "pg/vcd.h"
 150 #include "pg/vtx_io.h"
 151
 152 #include "rx/rx.h"
 153 #include "rx/rx_spi.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 #include "sensors/sensors.h"
 167
 168 #include "telemetry/telemetry.h"
 169
 170 #ifdef USE_HARDWARE_REVISION_DETECTION
 171 #include "hardware_revision.h"
 172 #endif
 173
 174 #ifdef TARGET_PREINIT
 175 void targetPreInit(void);
 176 #endif
 177
 178 #ifdef SOFTSERIAL_LOOPBACK
 179 serialPort_t *loopbackPort;
 180 #endif
 181
 182 uint8_t systemState = SYSTEM_STATE_INITIALISING;
 183
 184 void SDIO_GPIO_Init(void);
 185
 186 void processLoopback(void)
 187 {
 188 #ifdef SOFTSERIAL_LOOPBACK
 189     if (loopbackPort) {
 190         uint8_t bytesWaiting;
 191         while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) {
 192             uint8_t b = serialRead(loopbackPort);
 193             serialWrite(loopbackPort, b);
 194         };
 195     }
 196 #endif
 197 }
 198
 199 #ifdef BUS_SWITCH_PIN
 200 void busSwitchInit(void)
 201 {
 202 static IO_t busSwitchResetPin        = IO_NONE;
 203
 204     busSwitchResetPin = IOGetByTag(IO_TAG(BUS_SWITCH_PIN));
 205     IOInit(busSwitchResetPin, OWNER_SYSTEM, 0);
 206     IOConfigGPIO(busSwitchResetPin, IOCFG_OUT_PP);
 207
 208     // ENABLE
 209     IOLo(busSwitchResetPin);
 210 }
 211 #endif
 212
 213 static void configureSPIAndQuadSPI(void)
 214 {
 215 #ifdef USE_SPI
 216     spiPinConfigure(spiPinConfig(0));
 217 #endif
 218
 219     sensorsPreInit();
 220
 221 #ifdef USE_SPI
 222     spiPreinit();
 223
 224 #ifdef USE_SPI_DEVICE_1
 225     spiInit(SPIDEV_1);
 226 #endif
 227 #ifdef USE_SPI_DEVICE_2
 228     spiInit(SPIDEV_2);
 229 #endif
 230 #ifdef USE_SPI_DEVICE_3
 231     spiInit(SPIDEV_3);
 232 #endif
 233 #ifdef USE_SPI_DEVICE_4
 234     spiInit(SPIDEV_4);
 235 #endif
 236 #ifdef USE_SPI_DEVICE_5
 237     spiInit(SPIDEV_5);
 238 #endif
 239 #ifdef USE_SPI_DEVICE_6
 240     spiInit(SPIDEV_6);
 241 #endif
 242 #endif // USE_SPI
 243
 244 #ifdef USE_QUADSPI
 245     quadSpiPinConfigure(quadSpiConfig(0));
 246
 247 #ifdef USE_QUADSPI_DEVICE_1
 248     quadSpiInit(QUADSPIDEV_1);
 249 #endif
 250 #endif // USE_QUAD_SPI
 251 }
 252
 253 void sdCardAndFSInit()
 254 {
 255     sdcard_init(sdcardConfig());
 256     afatfs_init();
 257 }
 258
 259
 260 void init(void)
 261 {
 262 #ifdef SERIAL_PORT_COUNT
 263     printfSerialInit();
 264 #endif
 265
 266     systemInit();
 267
 268     // initialize IO (needed for all IO operations)
 269     IOInitGlobal();
 270
 271 #ifdef USE_HARDWARE_REVISION_DETECTION
 272     detectHardwareRevision();
 273 #endif
 274
 275 #ifdef USE_BRUSHED_ESC_AUTODETECT
 276     // Opportunistically use the first motor pin of the default configuration for detection.
 277     // We are doing this as with some boards, timing seems to be important, and the later detection will fail.
 278     ioTag_t motorIoTag = timerioTagGetByUsage(TIM_USE_MOTOR, 0);
 279
 280     if (motorIoTag) {
 281         detectBrushedESC(motorIoTag);
 282     }
 283 #endif
 284
 285     enum {
 286         FLASH_INIT_ATTEMPTED            = (1 << 0),
 287         SD_INIT_ATTEMPTED               = (1 << 1),
 288         SPI_AND_QSPI_INIT_ATTEMPTED      = (1 << 2),
 289     };
 290     uint8_t initFlags = 0;
 291
 292
 293 #ifdef EEPROM_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 EEPROM_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 "EEPROM_IN_SDCARD and TARGET_BUS_INIT are mutually exclusive"
 316 #endif
 317
 318     pgResetAll();
 319
 320 #if defined(STM32H7) && defined(USE_SDCARD_SDIO) // H7 only for now, likely should be applied to F4/F7 too
 321     SDIO_GPIO_Init();
 322 #endif
 323 #ifdef USE_SDCARD_SPI
 324     configureSPIAndQuadSPI();
 325     initFlags |= SPI_AND_QSPI_INIT_ATTEMPTED;
 326 #endif
 327
 328     sdCardAndFSInit();
 329     initFlags |= SD_INIT_ATTEMPTED;
 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 // EEPROM_IN_SDCARD
 340
 341 #ifdef EEPROM_IN_EXTERNAL_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 We need to add configuration somewhere, e.g. bootloader image or reserved flash area, that can be read by the firmware.
 347     // it's currently possible for the firmware resource allocation to be wrong after the config is loaded if the user changes the settings.
 348     // This would cause undefined behaviour once the config is loaded.  so for now, users must NOT change flash/pin configs needed for
 349     // the system to boot and/or to save the config.
 350     //
 351     // note that target specific FLASH/SPI/QUADSPI configs are
 352     // also not supported in USE_TARGET_CONFIG/targetConfigure() when using EEPROM_IN_EXTERNAL_FLASH.
 353     //
 354
 355     //
 356     // IMPORTANT: all default flash and pin configurations must be valid for the target after pgResetAll() is called.
 357     // Target designers must ensure other devices connected the same SPI/QUADSPI interface as the flash chip do not
 358     // cause communication issues with the flash chip.  e.g. use external pullups on SPI/QUADSPI CS lines.
 359     //
 360     pgResetAll();
 361
 362 #ifdef TARGET_BUS_INIT
 363 #error "EEPROM_IN_EXTERNAL_FLASH and TARGET_BUS_INIT are mutually exclusive"
 364 #endif
 365
 366     configureSPIAndQuadSPI();
 367     initFlags |= SPI_AND_QSPI_INIT_ATTEMPTED;
 368
 369
 370 #ifndef USE_FLASH_CHIP
 371 #error "EEPROM_IN_EXTERNAL_FLASH requires USE_FLASH_CHIP to be defined."
 372 #endif
 373
 374     bool haveFlash = flashInit(flashConfig());
 375
 376     if (!haveFlash) {
 377         failureMode(FAILURE_EXTERNAL_FLASH_INIT_FAILED);
 378     }
 379     initFlags |= FLASH_INIT_ATTEMPTED;
 380
 381 #endif // EEPROM_IN_EXTERNAL_FLASH
 382
 383     initEEPROM();
 384
 385     ensureEEPROMStructureIsValid();
 386
 387     bool readSuccess = readEEPROM();
 388
 389 #if defined(USE_BOARD_INFO)
 390     initBoardInformation();
 391 #endif
 392
 393     if (!readSuccess || !isEEPROMVersionValid() || strncasecmp(systemConfig()->boardIdentifier, TARGET_BOARD_IDENTIFIER, sizeof(TARGET_BOARD_IDENTIFIER))) {
 394         resetEEPROM();
 395     }
 396
 397     systemState |= SYSTEM_STATE_CONFIG_LOADED;
 398
 399 #ifdef USE_BRUSHED_ESC_AUTODETECT
 400     // Now detect again with the actually configured pin for motor 1, if it is not the default pin.
 401     ioTag_t configuredMotorIoTag = motorConfig()->dev.ioTags[0];
 402
 403     if (configuredMotorIoTag && configuredMotorIoTag != motorIoTag) {
 404         detectBrushedESC(configuredMotorIoTag);
 405     }
 406 #endif
 407
 408     debugMode = systemConfig()->debug_mode;
 409
 410 #ifdef TARGET_PREINIT
 411     targetPreInit();
 412 #endif
 413
 414 #if !defined(USE_FAKE_LED)
 415     ledInit(statusLedConfig());
 416 #endif
 417     LED2_ON;
 418
 419 #ifdef USE_EXTI
 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 #ifdef STM32F4
 484     // Only F4 has non-8MHz boards
 485     systemClockSetHSEValue(systemConfig()->hseMhz * 1000000U);
 486 #endif
 487
 488 #ifdef USE_OVERCLOCK
 489     OverclockRebootIfNecessary(systemConfig()->cpu_overclock);
 490 #endif
 491
 492     // Configure MCO output after config is stable
 493 #ifdef USE_MCO
 494     mcoInit(mcoConfig());
 495 #endif
 496
 497 #ifdef USE_TIMER
 498     timerInit();  // timer must be initialized before any channel is allocated
 499 #endif
 500
 501 #ifdef BUS_SWITCH_PIN
 502     busSwitchInit();
 503 #endif
 504
 505 #if defined(USE_UART)
 506     uartPinConfigure(serialPinConfig());
 507 #endif
 508
 509 #if defined(AVOID_UART1_FOR_PWM_PPM)
 510     serialInit(featureIsEnabled(FEATURE_SOFTSERIAL),
 511             featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
 512 #elif defined(AVOID_UART2_FOR_PWM_PPM)
 513     serialInit(featureIsEnabled(FEATURE_SOFTSERIAL),
 514             featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
 515 #elif defined(AVOID_UART3_FOR_PWM_PPM)
 516     serialInit(featureIsEnabled(FEATURE_SOFTSERIAL),
 517             featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
 518 #else
 519     serialInit(featureIsEnabled(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
 520 #endif
 521
 522     mixerInit(mixerConfig()->mixerMode);
 523     mixerConfigureOutput();
 524
 525     uint16_t idlePulse = motorConfig()->mincommand;
 526     if (featureIsEnabled(FEATURE_3D)) {
 527         idlePulse = flight3DConfig()->neutral3d;
 528     }
 529     if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
 530         idlePulse = 0; // brushed motors
 531     }
 532 #ifdef USE_MOTOR
 533     /* Motors needs to be initialized soon as posible because hardware initialization
 534      * may send spurious pulses to esc's causing their early initialization. Also ppm
 535      * receiver may share timer with motors so motors MUST be initialized here. */
 536     motorDevInit(&motorConfig()->dev, idlePulse, getMotorCount());
 537     systemState |= SYSTEM_STATE_MOTORS_READY;
 538 #else
 539     UNUSED(idlePulse);
 540 #endif
 541
 542     if (0) {}
 543 #if defined(USE_PPM)
 544     else if (featureIsEnabled(FEATURE_RX_PPM)) {
 545         ppmRxInit(ppmConfig());
 546     }
 547 #endif
 548 #if defined(USE_PWM)
 549     else if (featureIsEnabled(FEATURE_RX_PARALLEL_PWM)) {
 550         pwmRxInit(pwmConfig());
 551     }
 552 #endif
 553
 554 #ifdef USE_BEEPER
 555     beeperInit(beeperDevConfig());
 556 #endif
 557 /* temp until PGs are implemented. */
 558 #if defined(USE_INVERTER) && !defined(SIMULATOR_BUILD)
 559     initInverters(serialPinConfig());
 560 #endif
 561
 562
 563 #ifdef TARGET_BUS_INIT
 564     targetBusInit();
 565
 566 #else
 567
 568     // Depending on compilation options SPI/QSPI initialisation may already be done.
 569     if (!(initFlags & SPI_AND_QSPI_INIT_ATTEMPTED)) {
 570         configureSPIAndQuadSPI();
 571         initFlags |= SPI_AND_QSPI_INIT_ATTEMPTED;
 572     }
 573
 574 #ifdef USE_USB_MSC
 575 /* MSC mode will start after init, but will not allow scheduler to run,
 576  *  so there is no bottleneck in reading and writing data */
 577     mscInit();
 578     if (mscCheckBoot() || mscCheckButton()) {
 579         if (mscStart() == 0) {
 580              mscWaitForButton();
 581         } else {
 582             systemResetFromMsc();
 583         }
 584     }
 585 #endif
 586
 587 #ifdef USE_PERSISTENT_MSC_RTC
 588     // if we didn't enter MSC mode then clear the persistent RTC value
 589     persistentObjectWrite(PERSISTENT_OBJECT_RTC_HIGH, 0);
 590     persistentObjectWrite(PERSISTENT_OBJECT_RTC_LOW, 0);
 591 #endif
 592
 593 #ifdef USE_I2C
 594     i2cHardwareConfigure(i2cConfig(0));
 595
 596     // Note: Unlike UARTs which are configured when client is present,
 597     // I2C buses are initialized unconditionally if they are configured.
 598
 599 #ifdef USE_I2C_DEVICE_1
 600     i2cInit(I2CDEV_1);
 601 #endif
 602 #ifdef USE_I2C_DEVICE_2
 603     i2cInit(I2CDEV_2);
 604 #endif
 605 #ifdef USE_I2C_DEVICE_3
 606     i2cInit(I2CDEV_3);
 607 #endif
 608 #ifdef USE_I2C_DEVICE_4
 609     i2cInit(I2CDEV_4);
 610 #endif
 611 #endif // USE_I2C
 612
 613 #endif // TARGET_BUS_INIT
 614
 615 #ifdef USE_HARDWARE_REVISION_DETECTION
 616     updateHardwareRevision();
 617 #endif
 618
 619 #if defined(STM32H7) && defined(USE_SDCARD_SDIO) // H7 only for now, likely should be applied to F4/F7 too
 620     if (!(initFlags & SD_INIT_ATTEMPTED)) {
 621         SDIO_GPIO_Init();
 622     }
 623 #endif
 624
 625
 626 #ifdef USE_VTX_RTC6705
 627     bool useRTC6705 = rtc6705IOInit(vtxIOConfig());
 628 #endif
 629
 630 #ifdef USE_CAMERA_CONTROL
 631     cameraControlInit();
 632 #endif
 633
 634 // XXX These kind of code should goto target/config.c?
 635 // XXX And these no longer work properly as FEATURE_RANGEFINDER does control HCSR04 runtime configuration.
 636 #if defined(RANGEFINDER_HCSR04_SOFTSERIAL2_EXCLUSIVE) && defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL2)
 637     if (featureIsEnabled(FEATURE_RANGEFINDER) && featureIsEnabled(FEATURE_SOFTSERIAL)) {
 638         serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
 639     }
 640 #endif
 641
 642 #if defined(RANGEFINDER_HCSR04_SOFTSERIAL1_EXCLUSIVE) && defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL1)
 643     if (featureIsEnabled(FEATURE_RANGEFINDER) && featureIsEnabled(FEATURE_SOFTSERIAL)) {
 644         serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
 645     }
 646 #endif
 647
 648 #ifdef USE_ADC
 649     adcConfigMutable()->vbat.enabled = (batteryConfig()->voltageMeterSource == VOLTAGE_METER_ADC);
 650     adcConfigMutable()->current.enabled = (batteryConfig()->currentMeterSource == CURRENT_METER_ADC);
 651
 652     // The FrSky D SPI RX sends RSSI_ADC_PIN (if configured) as A2
 653     adcConfigMutable()->rssi.enabled = featureIsEnabled(FEATURE_RSSI_ADC);
 654 #ifdef USE_RX_SPI
 655     adcConfigMutable()->rssi.enabled |= (featureIsEnabled(FEATURE_RX_SPI) && rxSpiConfig()->rx_spi_protocol == RX_SPI_FRSKY_D);
 656 #endif
 657     adcInit(adcConfig());
 658 #endif
 659
 660     initBoardAlignment(boardAlignment());
 661
 662     if (!sensorsAutodetect()) {
 663         // if gyro was not detected due to whatever reason, notify and don't arm.
 664         if (isSystemConfigured()) {
 665             indicateFailure(FAILURE_MISSING_ACC, 2);
 666         }
 667         setArmingDisabled(ARMING_DISABLED_NO_GYRO);
 668     }
 669
 670     systemState |= SYSTEM_STATE_SENSORS_READY;
 671
 672     // gyro.targetLooptime set in sensorsAutodetect(),
 673     // so we are ready to call validateAndFixGyroConfig(), pidInit(), and setAccelerationFilter()
 674     validateAndFixGyroConfig();
 675     pidInit(currentPidProfile);
 676 #ifdef USE_ACC
 677     accInitFilters();
 678 #endif
 679
 680 #ifdef USE_PID_AUDIO
 681     pidAudioInit();
 682 #endif
 683
 684 #ifdef USE_SERVOS
 685     servosInit();
 686     servoConfigureOutput();
 687     if (isMixerUsingServos()) {
 688         //pwm_params.useChannelForwarding = featureIsEnabled(FEATURE_CHANNEL_FORWARDING);
 689         servoDevInit(&servoConfig()->dev);
 690     }
 691     servosFilterInit();
 692 #endif
 693
 694 #ifdef USE_PINIO
 695     pinioInit(pinioConfig());
 696 #endif
 697
 698 #ifdef USE_PINIOBOX
 699     pinioBoxInit(pinioBoxConfig());
 700 #endif
 701
 702     LED1_ON;
 703     LED0_OFF;
 704     LED2_OFF;
 705
 706     for (int i = 0; i < 10; i++) {
 707         LED1_TOGGLE;
 708         LED0_TOGGLE;
 709 #if defined(USE_BEEPER)
 710         delay(25);
 711         if (!(beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(BEEPER_SYSTEM_INIT))) {
 712             BEEP_ON;
 713         }
 714         delay(25);
 715         BEEP_OFF;
 716 #else
 717         delay(50);
 718 #endif
 719     }
 720     LED0_OFF;
 721     LED1_OFF;
 722
 723     imuInit();
 724
 725     mspInit();
 726     mspSerialInit();
 727
 728 #ifdef USE_CLI
 729     cliInit(serialConfig());
 730 #endif
 731
 732     failsafeInit();
 733
 734     rxInit();
 735
 736 /*
 737  * CMS, display devices and OSD
 738  */
 739 #ifdef USE_CMS
 740     cmsInit();
 741 #endif
 742
 743 #if (defined(USE_OSD) || (defined(USE_MSP_DISPLAYPORT) && defined(USE_CMS)))
 744     displayPort_t *osdDisplayPort = NULL;
 745 #endif
 746
 747 #if defined(USE_OSD)
 748     //The OSD need to be initialised after GYRO to avoid GYRO initialisation failure on some targets
 749
 750     if (featureIsEnabled(FEATURE_OSD)) {
 751 #if defined(USE_MAX7456)
 752         // If there is a max7456 chip for the OSD then use it
 753         osdDisplayPort = max7456DisplayPortInit(vcdProfile());
 754 #elif defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT) && defined(USE_OSD_OVER_MSP_DISPLAYPORT) // OSD over MSP; not supported (yet)
 755         osdDisplayPort = displayPortMspInit();
 756 #endif
 757         // osdInit  will register with CMS by itself.
 758         osdInit(osdDisplayPort);
 759     }
 760 #endif
 761
 762 #if defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT)
 763     // If BFOSD is not active, then register MSP_DISPLAYPORT as a CMS device.
 764     if (!osdDisplayPort)
 765         cmsDisplayPortRegister(displayPortMspInit());
 766 #endif
 767
 768 #ifdef USE_DASHBOARD
 769     // Dashbord will register with CMS by itself.
 770     if (featureIsEnabled(FEATURE_DASHBOARD)) {
 771         dashboardInit();
 772     }
 773 #endif
 774
 775 #if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
 776     // Register the srxl Textgen telemetry sensor as a displayport device
 777     cmsDisplayPortRegister(displayPortSrxlInit());
 778 #endif
 779
 780 #ifdef USE_GPS
 781     if (featureIsEnabled(FEATURE_GPS)) {
 782         gpsInit();
 783     }
 784 #endif
 785
 786 #ifdef USE_LED_STRIP
 787     ledStripInit();
 788
 789     if (featureIsEnabled(FEATURE_LED_STRIP)) {
 790         ledStripEnable();
 791     }
 792 #endif
 793
 794 #ifdef USE_TELEMETRY
 795     if (featureIsEnabled(FEATURE_TELEMETRY)) {
 796         telemetryInit();
 797     }
 798 #endif
 799
 800 #ifdef USE_ESC_SENSOR
 801     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
 802         escSensorInit();
 803     }
 804 #endif
 805
 806 #ifdef USE_USB_DETECT
 807     usbCableDetectInit();
 808 #endif
 809
 810 #ifdef USE_TRANSPONDER
 811     if (featureIsEnabled(FEATURE_TRANSPONDER)) {
 812         transponderInit();
 813         transponderStartRepeating();
 814         systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
 815     }
 816 #endif
 817
 818 #ifdef USE_FLASH_CHIP
 819     if (!(initFlags & FLASH_INIT_ATTEMPTED)) {
 820         flashInit(flashConfig());
 821         initFlags |= FLASH_INIT_ATTEMPTED;
 822     }
 823 #endif
 824 #ifdef USE_FLASHFS
 825     flashfsInit();
 826 #endif
 827
 828 #ifdef USE_BLACKBOX
 829 #ifdef USE_SDCARD
 830     if (blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD) {
 831         if (sdcardConfig()->mode) {
 832             if (!(initFlags & SD_INIT_ATTEMPTED)) {
 833                 initFlags |= SD_INIT_ATTEMPTED;
 834                 sdCardAndFSInit();
 835             }
 836         } else {
 837             blackboxConfigMutable()->device = BLACKBOX_DEVICE_NONE;
 838         }
 839     }
 840 #endif
 841     blackboxInit();
 842 #endif
 843
 844 #ifdef USE_ACC
 845     if (mixerConfig()->mixerMode == MIXER_GIMBAL) {
 846         accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
 847     }
 848 #endif
 849     gyroStartCalibration(false);
 850 #ifdef USE_BARO
 851     baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
 852 #endif
 853
 854 #if defined(USE_VTX_COMMON) || defined(USE_VTX_CONTROL)
 855     vtxTableInit();
 856 #endif
 857
 858 #ifdef USE_VTX_CONTROL
 859     vtxControlInit();
 860
 861 #if defined(USE_VTX_COMMON)
 862     vtxCommonInit();
 863 #endif
 864
 865 #ifdef USE_VTX_SMARTAUDIO
 866     vtxSmartAudioInit();
 867 #endif
 868
 869 #ifdef USE_VTX_TRAMP
 870     vtxTrampInit();
 871 #endif
 872
 873 #ifdef USE_VTX_RTC6705
 874     if (!vtxCommonDevice() && useRTC6705) { // external VTX takes precedence when configured.
 875         vtxRTC6705Init();
 876     }
 877 #endif
 878
 879 #endif // VTX_CONTROL
 880
 881 #ifdef USE_TIMER
 882     // start all timers
 883     // TODO - not implemented yet
 884     timerStart();
 885 #endif
 886
 887     ENABLE_STATE(SMALL_ANGLE);
 888
 889 #ifdef SOFTSERIAL_LOOPBACK
 890     // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
 891     loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
 892     if (!loopbackPort->vTable) {
 893         loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
 894     }
 895     serialPrint(loopbackPort, "LOOPBACK\r\n");
 896 #endif
 897
 898     batteryInit(); // always needs doing, regardless of features.
 899
 900 #ifdef USE_DASHBOARD
 901     if (featureIsEnabled(FEATURE_DASHBOARD)) {
 902 #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
 903         dashboardShowFixedPage(PAGE_GPS);
 904 #else
 905         dashboardResetPageCycling();
 906         dashboardEnablePageCycling();
 907 #endif
 908     }
 909 #endif
 910
 911 #ifdef USE_RCDEVICE
 912     rcdeviceInit();
 913 #endif // USE_RCDEVICE
 914
 915 #ifdef USE_MOTOR
 916     motorEnable();
 917 #endif
 918
 919 #ifdef USE_PERSISTENT_STATS
 920     statsInit();
 921 #endif
 922
 923     setArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME);
 924
 925     fcTasksInit();
 926
 927     systemState |= SYSTEM_STATE_READY;
 928 }