src/main/fc/fc_init.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21
  22 #include "platform.h"
  23
  24 #include "blackbox/blackbox.h"
  25
  26 #include "common/axis.h"
  27 #include "common/color.h"
  28 #include "common/maths.h"
  29 #include "common/printf.h"
  30
  31 #include "config/config_eeprom.h"
  32 #include "config/feature.h"
  33 #include "config/parameter_group.h"
  34 #include "config/parameter_group_ids.h"
  35
  36 #include "cms/cms.h"
  37 #include "cms/cms_types.h"
  38
  39 #include "drivers/nvic.h"
  40 #include "drivers/sensor.h"
  41 #include "drivers/system.h"
  42 #include "drivers/time.h"
  43 #include "drivers/dma.h"
  44 #include "drivers/io.h"
  45 #include "drivers/light_led.h"
  46 #include "drivers/sound_beeper.h"
  47 #include "drivers/timer.h"
  48 #include "drivers/serial.h"
  49 #include "drivers/serial_softserial.h"
  50 #include "drivers/serial_uart.h"
  51 #include "drivers/accgyro/accgyro.h"
  52 #include "drivers/compass/compass.h"
  53 #include "drivers/pwm_esc_detect.h"
  54 #include "drivers/rx_pwm.h"
  55 #include "drivers/pwm_output.h"
  56 #include "drivers/adc.h"
  57 #include "drivers/bus.h"
  58 #include "drivers/bus_i2c.h"
  59 #include "drivers/bus_spi.h"
  60 #include "drivers/buttons.h"
  61 #include "drivers/inverter.h"
  62 #include "drivers/flash_m25p16.h"
  63 #include "drivers/sonar_hcsr04.h"
  64 #include "drivers/sdcard.h"
  65 #include "drivers/usb_io.h"
  66 #include "drivers/transponder_ir.h"
  67 #include "drivers/exti.h"
  68 #include "drivers/max7456.h"
  69 #include "drivers/vtx_rtc6705.h"
  70 #include "drivers/vtx_common.h"
  71
  72 #include "fc/config.h"
  73 #include "fc/fc_init.h"
  74 #include "fc/fc_msp.h"
  75 #include "fc/fc_tasks.h"
  76 #include "fc/rc_controls.h"
  77 #include "fc/runtime_config.h"
  78 #include "fc/cli.h"
  79
  80 #include "msp/msp_serial.h"
  81
  82 #include "rx/rx.h"
  83 #include "rx/spektrum.h"
  84
  85 #include "io/beeper.h"
  86 #include "io/displayport_max7456.h"
  87 #include "io/serial.h"
  88 #include "io/flashfs.h"
  89 #include "io/gps.h"
  90 #include "io/motors.h"
  91 #include "io/servos.h"
  92 #include "io/gimbal.h"
  93 #include "io/ledstrip.h"
  94 #include "io/dashboard.h"
  95 #include "io/asyncfatfs/asyncfatfs.h"
  96 #include "io/transponder_ir.h"
  97 #include "io/osd.h"
  98 #include "io/osd_slave.h"
  99 #include "io/displayport_msp.h"
 100 #include "io/vtx_rtc6705.h"
 101 #include "io/vtx_control.h"
 102 #include "io/vtx_smartaudio.h"
 103 #include "io/vtx_tramp.h"
 104
 105 #include "scheduler/scheduler.h"
 106
 107 #include "sensors/acceleration.h"
 108 #include "sensors/barometer.h"
 109 #include "sensors/battery.h"
 110 #include "sensors/boardalignment.h"
 111 #include "sensors/compass.h"
 112 #include "sensors/esc_sensor.h"
 113 #include "sensors/gyro.h"
 114 #include "sensors/initialisation.h"
 115 #include "sensors/sensors.h"
 116 #include "sensors/sonar.h"
 117
 118 #include "telemetry/telemetry.h"
 119
 120 #include "flight/failsafe.h"
 121 #include "flight/imu.h"
 122 #include "flight/mixer.h"
 123 #include "flight/navigation.h"
 124 #include "flight/pid.h"
 125 #include "flight/servos.h"
 126
 127
 128 #ifdef USE_HARDWARE_REVISION_DETECTION
 129 #include "hardware_revision.h"
 130 #endif
 131
 132 #include "build/build_config.h"
 133 #include "build/debug.h"
 134
 135 #ifdef TARGET_PREINIT
 136 void targetPreInit(void);
 137 #endif
 138
 139 extern uint8_t motorControlEnable;
 140
 141 #ifdef SOFTSERIAL_LOOPBACK
 142 serialPort_t *loopbackPort;
 143 #endif
 144
 145 uint8_t systemState = SYSTEM_STATE_INITIALISING;
 146
 147 void processLoopback(void)
 148 {
 149 #ifdef SOFTSERIAL_LOOPBACK
 150     if (loopbackPort) {
 151         uint8_t bytesWaiting;
 152         while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) {
 153             uint8_t b = serialRead(loopbackPort);
 154             serialWrite(loopbackPort, b);
 155         };
 156     }
 157 #endif
 158 }
 159
 160
 161 #ifdef VTX_RTC6705
 162 bool canUpdateVTX(void)
 163 {
 164 #if defined(MAX7456_SPI_INSTANCE) && defined(RTC6705_SPI_INSTANCE) && defined(SPI_SHARED_MAX7456_AND_RTC6705)
 165     if (feature(FEATURE_OSD)) {
 166         return !max7456DmaInProgress();
 167     }
 168 #endif
 169     return true;
 170 }
 171 #endif
 172
 173 #ifdef BUS_SWITCH_PIN
 174 void busSwitchInit(void)
 175 {
 176 static IO_t busSwitchResetPin        = IO_NONE;
 177
 178     busSwitchResetPin = IOGetByTag(IO_TAG(BUS_SWITCH_PIN));
 179     IOInit(busSwitchResetPin, OWNER_SYSTEM, 0);
 180     IOConfigGPIO(busSwitchResetPin, IOCFG_OUT_PP);
 181
 182     // ENABLE
 183     IOLo(busSwitchResetPin);
 184 }
 185 #endif
 186
 187 void init(void)
 188 {
 189 #ifdef USE_HAL_DRIVER
 190     HAL_Init();
 191 #endif
 192
 193     printfSupportInit();
 194
 195     systemInit();
 196
 197     // initialize IO (needed for all IO operations)
 198     IOInitGlobal();
 199
 200 #ifdef USE_HARDWARE_REVISION_DETECTION
 201     detectHardwareRevision();
 202 #endif
 203
 204 #ifdef BRUSHED_ESC_AUTODETECT
 205     detectBrushedESC();
 206 #endif
 207
 208     initEEPROM();
 209
 210     ensureEEPROMContainsValidData();
 211     readEEPROM();
 212
 213     systemState |= SYSTEM_STATE_CONFIG_LOADED;
 214
 215     //i2cSetOverclock(masterConfig.i2c_overclock);
 216
 217     debugMode = systemConfig()->debug_mode;
 218
 219     // Latch active features to be used for feature() in the remainder of init().
 220     latchActiveFeatures();
 221
 222 #ifdef TARGET_PREINIT
 223     targetPreInit();
 224 #endif
 225
 226 #if !defined(UNIT_TEST) && !defined(USE_FAKE_LED)
 227     ledInit(statusLedConfig());
 228 #endif
 229     LED2_ON;
 230
 231 #ifdef USE_EXTI
 232     EXTIInit();
 233 #endif
 234
 235 #if defined(BUTTONS)
 236
 237     buttonsInit();
 238
 239     // Check status of bind plug and exit if not active
 240     delayMicroseconds(10);  // allow configuration to settle
 241
 242     if (!isMPUSoftReset()) {
 243 #if defined(BUTTON_A_PIN) && defined(BUTTON_B_PIN)
 244         // two buttons required
 245         uint8_t secondsRemaining = 5;
 246         bool bothButtonsHeld;
 247         do {
 248             bothButtonsHeld = buttonAPressed() && buttonBPressed();
 249             if (bothButtonsHeld) {
 250                 if (--secondsRemaining == 0) {
 251                     resetEEPROM();
 252                     systemReset();
 253                 }
 254                 delay(1000);
 255                 LED0_TOGGLE;
 256             }
 257         } while (bothButtonsHeld);
 258 #endif
 259     }
 260 #endif
 261
 262 #ifdef SPEKTRUM_BIND_PIN
 263     if (feature(FEATURE_RX_SERIAL)) {
 264         switch (rxConfig()->serialrx_provider) {
 265         case SERIALRX_SPEKTRUM1024:
 266         case SERIALRX_SPEKTRUM2048:
 267             // Spektrum satellite binding if enabled on startup.
 268             // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
 269             // The rest of Spektrum initialization will happen later - via spektrumInit()
 270             spektrumBind(rxConfigMutable());
 271             break;
 272         }
 273     }
 274 #endif
 275
 276     delay(100);
 277
 278     timerInit();  // timer must be initialized before any channel is allocated
 279
 280 #ifdef BUS_SWITCH_PIN
 281     busSwitchInit();
 282 #endif
 283
 284 #if defined(USE_UART) && !defined(SITL)
 285     uartPinConfigure(serialPinConfig());
 286 #endif
 287
 288 #if defined(AVOID_UART1_FOR_PWM_PPM)
 289     serialInit(feature(FEATURE_SOFTSERIAL),
 290             feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
 291 #elif defined(AVOID_UART2_FOR_PWM_PPM)
 292     serialInit(feature(FEATURE_SOFTSERIAL),
 293             feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
 294 #elif defined(AVOID_UART3_FOR_PWM_PPM)
 295     serialInit(feature(FEATURE_SOFTSERIAL),
 296             feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
 297 #else
 298     serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
 299 #endif
 300
 301     mixerInit(mixerConfig()->mixerMode);
 302 #ifdef USE_SERVOS
 303     servosInit();
 304 #endif
 305
 306     uint16_t idlePulse = motorConfig()->mincommand;
 307     if (feature(FEATURE_3D)) {
 308         idlePulse = flight3DConfig()->neutral3d;
 309     }
 310
 311     if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
 312         featureClear(FEATURE_3D);
 313         idlePulse = 0; // brushed motors
 314     }
 315
 316     mixerConfigureOutput();
 317     motorDevInit(&motorConfig()->dev, idlePulse, getMotorCount());
 318
 319 #ifdef USE_SERVOS
 320     servoConfigureOutput();
 321     if (isMixerUsingServos()) {
 322         //pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
 323         servoDevInit(&servoConfig()->dev);
 324     }
 325 #endif
 326
 327     if (0) {}
 328 #if defined(USE_PPM)
 329     else if (feature(FEATURE_RX_PPM)) {
 330           ppmRxInit(ppmConfig(), motorConfig()->dev.motorPwmProtocol);
 331     }
 332 #endif
 333 #if defined(USE_PWM)
 334     else if (feature(FEATURE_RX_PARALLEL_PWM)) {
 335           pwmRxInit(pwmConfig());
 336     }
 337 #endif
 338
 339     systemState |= SYSTEM_STATE_MOTORS_READY;
 340
 341 #ifdef BEEPER
 342     beeperInit(beeperDevConfig());
 343 #endif
 344 /* temp until PGs are implemented. */
 345 #if defined(USE_INVERTER) && !defined(SITL)
 346     initInverters(serialPinConfig());
 347 #endif
 348
 349 #ifdef TARGET_BUS_INIT
 350     targetBusInit();
 351 #else
 352
 353 #ifdef USE_SPI
 354 #ifdef USE_SPI_DEVICE_1
 355     spiInit(SPIDEV_1);
 356 #endif
 357 #ifdef USE_SPI_DEVICE_2
 358     spiInit(SPIDEV_2);
 359 #endif
 360 #ifdef USE_SPI_DEVICE_3
 361     spiInit(SPIDEV_3);
 362 #endif
 363 #ifdef USE_SPI_DEVICE_4
 364     spiInit(SPIDEV_4);
 365 #endif
 366 #endif /* USE_SPI */
 367
 368 #ifdef USE_I2C
 369     i2cHardwareConfigure();
 370
 371     // Note: Unlike UARTs which are configured when client is present,
 372     // I2C buses are initialized unconditionally if they are configured.
 373
 374 #ifdef USE_I2C_DEVICE_1
 375     i2cInit(I2CDEV_1);
 376 #endif
 377 #ifdef USE_I2C_DEVICE_2
 378     i2cInit(I2CDEV_2);
 379 #endif
 380 #ifdef USE_I2C_DEVICE_3
 381     i2cInit(I2CDEV_3);
 382 #endif
 383 #ifdef USE_I2C_DEVICE_4
 384     i2cInit(I2CDEV_4);
 385 #endif
 386 #endif /* USE_I2C */
 387
 388 #endif /* TARGET_BUS_INIT */
 389
 390 #ifdef USE_HARDWARE_REVISION_DETECTION
 391     updateHardwareRevision();
 392 #endif
 393
 394 #ifdef VTX_RTC6705
 395     rtc6705IOInit();
 396 #endif
 397
 398 #if defined(SONAR_SOFTSERIAL2_EXCLUSIVE) && defined(SONAR) && defined(USE_SOFTSERIAL2)
 399     if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
 400         serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
 401     }
 402 #endif
 403
 404 #if defined(SONAR_SOFTSERIAL1_EXCLUSIVE) && defined(SONAR) && defined(USE_SOFTSERIAL1)
 405     if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
 406         serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
 407     }
 408 #endif
 409
 410 #ifdef USE_ADC
 411     adcConfigMutable()->vbat.enabled = (batteryConfig()->voltageMeterSource == VOLTAGE_METER_ADC);
 412     adcConfigMutable()->current.enabled = (batteryConfig()->currentMeterSource == CURRENT_METER_ADC);
 413
 414     adcConfigMutable()->rssi.enabled = feature(FEATURE_RSSI_ADC);
 415     adcInit(adcConfig());
 416 #endif
 417
 418     initBoardAlignment(boardAlignment());
 419
 420     if (!sensorsAutodetect()) {
 421         // if gyro was not detected due to whatever reason, we give up now.
 422         failureMode(FAILURE_MISSING_ACC);
 423     }
 424
 425     systemState |= SYSTEM_STATE_SENSORS_READY;
 426
 427     LED1_ON;
 428     LED0_OFF;
 429     LED2_OFF;
 430
 431     for (int i = 0; i < 10; i++) {
 432         LED1_TOGGLE;
 433         LED0_TOGGLE;
 434         delay(25);
 435         if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) BEEP_ON;
 436         delay(25);
 437         BEEP_OFF;
 438     }
 439     LED0_OFF;
 440     LED1_OFF;
 441
 442     // gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidInit()
 443     pidInit(currentPidProfile);
 444
 445     imuInit();
 446
 447     mspFcInit();
 448     mspSerialInit();
 449
 450 #ifdef USE_CLI
 451     cliInit(serialConfig());
 452 #endif
 453
 454     failsafeInit();
 455
 456     rxInit();
 457
 458 /*
 459  * CMS, display devices and OSD
 460  */
 461 #ifdef CMS
 462     cmsInit();
 463 #endif
 464
 465 #if (defined(OSD) || (defined(USE_MSP_DISPLAYPORT) && defined(CMS)) || defined(USE_OSD_SLAVE))
 466     displayPort_t *osdDisplayPort = NULL;
 467 #endif
 468
 469 #if defined(OSD) && !defined(USE_OSD_SLAVE)
 470     //The OSD need to be initialised after GYRO to avoid GYRO initialisation failure on some targets
 471
 472     if (feature(FEATURE_OSD)) {
 473 #if defined(USE_MAX7456)
 474         // If there is a max7456 chip for the OSD then use it
 475         osdDisplayPort = max7456DisplayPortInit(vcdProfile());
 476 #elif defined(USE_OSD_OVER_MSP_DISPLAYPORT) // OSD over MSP; not supported (yet)
 477         osdDisplayPort = displayPortMspInit();
 478 #endif
 479         // osdInit  will register with CMS by itself.
 480         osdInit(osdDisplayPort);
 481     }
 482 #endif
 483
 484 #if defined(USE_OSD_SLAVE) && !defined(OSD)
 485 #if defined(USE_MAX7456)
 486     // If there is a max7456 chip for the OSD then use it
 487     osdDisplayPort = max7456DisplayPortInit(vcdProfile());
 488     // osdInit  will register with CMS by itself.
 489     osdSlaveInit(osdDisplayPort);
 490 #endif
 491 #endif
 492
 493 #if defined(CMS) && defined(USE_MSP_DISPLAYPORT)
 494     // If BFOSD is not active, then register MSP_DISPLAYPORT as a CMS device.
 495     if (!osdDisplayPort)
 496         cmsDisplayPortRegister(displayPortMspInit());
 497 #endif
 498
 499 #ifdef USE_DASHBOARD
 500     // Dashbord will register with CMS by itself.
 501     if (feature(FEATURE_DASHBOARD)) {
 502         dashboardInit();
 503     }
 504 #endif
 505
 506
 507 #ifdef GPS
 508     if (feature(FEATURE_GPS)) {
 509         gpsInit();
 510         navigationInit();
 511     }
 512 #endif
 513
 514 #ifdef LED_STRIP
 515     ledStripInit();
 516
 517     if (feature(FEATURE_LED_STRIP)) {
 518         ledStripEnable();
 519     }
 520 #endif
 521
 522 #ifdef TELEMETRY
 523     if (feature(FEATURE_TELEMETRY)) {
 524         telemetryInit();
 525     }
 526 #endif
 527
 528 #ifdef USE_ESC_SENSOR
 529     if (feature(FEATURE_ESC_SENSOR)) {
 530         escSensorInit();
 531     }
 532 #endif
 533
 534 #ifdef USB_DETECT_PIN
 535     usbCableDetectInit();
 536 #endif
 537
 538 #ifdef TRANSPONDER
 539     if (feature(FEATURE_TRANSPONDER)) {
 540         transponderInit();
 541         transponderStartRepeating();
 542         systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
 543     }
 544 #endif
 545
 546 #ifdef USE_FLASHFS
 547 #if defined(USE_FLASH_M25P16)
 548     m25p16_init(flashConfig());
 549 #endif
 550     flashfsInit();
 551 #endif
 552
 553 #ifdef USE_SDCARD
 554     if (blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD) {
 555         sdcardInsertionDetectInit();
 556         sdcard_init(sdcardConfig()->useDma);
 557         afatfs_init();
 558     }
 559 #endif
 560
 561 #ifdef BLACKBOX
 562     blackboxInit();
 563 #endif
 564
 565     if (mixerConfig()->mixerMode == MIXER_GIMBAL) {
 566         accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
 567     }
 568     gyroStartCalibration();
 569 #ifdef BARO
 570     baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
 571 #endif
 572
 573 #ifdef VTX_CONTROL
 574     vtxControlInit();
 575
 576     vtxCommonInit();
 577
 578 #ifdef VTX_SMARTAUDIO
 579     vtxSmartAudioInit();
 580 #endif
 581
 582 #ifdef VTX_TRAMP
 583     vtxTrampInit();
 584 #endif
 585
 586 #ifdef VTX_RTC6705
 587 #ifdef VTX_RTC6705OPTIONAL
 588     if (!vtxCommonDeviceRegistered()) // external VTX takes precedence when configured.
 589 #endif
 590     {
 591         vtxRTC6705Init();
 592     }
 593 #endif
 594
 595 #endif // VTX_CONTROL
 596
 597     // start all timers
 598     // TODO - not implemented yet
 599     timerStart();
 600
 601     ENABLE_STATE(SMALL_ANGLE);
 602     DISABLE_ARMING_FLAG(PREVENT_ARMING);
 603
 604 #ifdef SOFTSERIAL_LOOPBACK
 605     // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
 606     loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
 607     if (!loopbackPort->vTable) {
 608         loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
 609     }
 610     serialPrint(loopbackPort, "LOOPBACK\r\n");
 611 #endif
 612
 613     batteryInit(); // always needs doing, regardless of features.
 614
 615 #ifdef USE_DASHBOARD
 616     if (feature(FEATURE_DASHBOARD)) {
 617 #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
 618         dashboardShowFixedPage(PAGE_GPS);
 619 #else
 620         dashboardResetPageCycling();
 621         dashboardEnablePageCycling();
 622 #endif
 623     }
 624 #endif
 625
 626 #ifdef CJMCU
 627     LED2_ON;
 628 #endif
 629
 630     // Latch active features AGAIN since some may be modified by init().
 631     latchActiveFeatures();
 632     motorControlEnable = true;
 633
 634 #ifdef USE_OSD_SLAVE
 635     osdSlaveTasksInit();
 636 #else
 637     fcTasksInit();
 638 #endif
 639     systemState |= SYSTEM_STATE_READY;
 640 }