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/dma.h"
  43 #include "drivers/io.h"
  44 #include "drivers/light_led.h"
  45 #include "drivers/sound_beeper.h"
  46 #include "drivers/timer.h"
  47 #include "drivers/serial.h"
  48 #include "drivers/serial_softserial.h"
  49 #include "drivers/serial_uart.h"
  50 #include "drivers/accgyro.h"
  51 #include "drivers/compass.h"
  52 #include "drivers/pwm_esc_detect.h"
  53 #include "drivers/rx_pwm.h"
  54 #include "drivers/pwm_output.h"
  55 #include "drivers/adc.h"
  56 #include "drivers/bus_i2c.h"
  57 #include "drivers/bus_spi.h"
  58 #include "drivers/buttons.h"
  59 #include "drivers/inverter.h"
  60 #include "drivers/flash_m25p16.h"
  61 #include "drivers/sonar_hcsr04.h"
  62 #include "drivers/sdcard.h"
  63 #include "drivers/usb_io.h"
  64 #include "drivers/transponder_ir.h"
  65 #include "drivers/exti.h"
  66 #include "drivers/max7456.h"
  67 #include "drivers/vtx_rtc6705.h"
  68 #include "drivers/vtx_common.h"
  69
  70 #include "fc/config.h"
  71 #include "fc/fc_init.h"
  72 #include "fc/fc_msp.h"
  73 #include "fc/fc_tasks.h"
  74 #include "fc/rc_controls.h"
  75 #include "fc/runtime_config.h"
  76 #include "fc/cli.h"
  77
  78 #include "msp/msp_serial.h"
  79
  80 #include "rx/rx.h"
  81 #include "rx/spektrum.h"
  82
  83 #include "io/beeper.h"
  84 #include "io/displayport_max7456.h"
  85 #include "io/serial.h"
  86 #include "io/flashfs.h"
  87 #include "io/gps.h"
  88 #include "io/motors.h"
  89 #include "io/servos.h"
  90 #include "io/gimbal.h"
  91 #include "io/ledstrip.h"
  92 #include "io/dashboard.h"
  93 #include "io/asyncfatfs/asyncfatfs.h"
  94 #include "io/transponder_ir.h"
  95 #include "io/osd.h"
  96 #include "io/osd_slave.h"
  97 #include "io/displayport_msp.h"
  98 #include "io/vtx_rtc6705.h"
  99 #include "io/vtx_control.h"
 100 #include "io/vtx_smartaudio.h"
 101 #include "io/vtx_tramp.h"
 102
 103 #include "scheduler/scheduler.h"
 104
 105 #include "sensors/acceleration.h"
 106 #include "sensors/barometer.h"
 107 #include "sensors/battery.h"
 108 #include "sensors/boardalignment.h"
 109 #include "sensors/compass.h"
 110 #include "sensors/esc_sensor.h"
 111 #include "sensors/gyro.h"
 112 #include "sensors/initialisation.h"
 113 #include "sensors/sensors.h"
 114 #include "sensors/sonar.h"
 115
 116 #include "telemetry/telemetry.h"
 117
 118 #include "flight/failsafe.h"
 119 #include "flight/imu.h"
 120 #include "flight/mixer.h"
 121 #include "flight/navigation.h"
 122 #include "flight/pid.h"
 123 #include "flight/servos.h"
 124
 125
 126 #ifdef USE_HARDWARE_REVISION_DETECTION
 127 #include "hardware_revision.h"
 128 #endif
 129
 130 #include "build/build_config.h"
 131 #include "build/debug.h"
 132
 133 #ifdef TARGET_PREINIT
 134 void targetPreInit(void);
 135 #endif
 136
 137 #ifdef TARGET_BUS_INIT
 138 void targetBusInit(void);
 139 #endif
 140
 141 extern uint8_t motorControlEnable;
 142
 143 #ifdef SOFTSERIAL_LOOPBACK
 144 serialPort_t *loopbackPort;
 145 #endif
 146
 147 uint8_t systemState = SYSTEM_STATE_INITIALISING;
 148
 149 void processLoopback(void)
 150 {
 151 #ifdef SOFTSERIAL_LOOPBACK
 152     if (loopbackPort) {
 153         uint8_t bytesWaiting;
 154         while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) {
 155             uint8_t b = serialRead(loopbackPort);
 156             serialWrite(loopbackPort, b);
 157         };
 158     }
 159 #endif
 160 }
 161
 162
 163 #ifdef VTX_RTC6705
 164 bool canUpdateVTX(void)
 165 {
 166 #if defined(MAX7456_SPI_INSTANCE) && defined(RTC6705_SPI_INSTANCE) && defined(SPI_SHARED_MAX7456_AND_RTC6705)
 167     if (feature(FEATURE_OSD)) {
 168         return !max7456DmaInProgress();
 169     }
 170 #endif
 171     return true;
 172 }
 173 #endif
 174
 175 #ifdef BUS_SWITCH_PIN
 176 void busSwitchInit(void)
 177 {
 178 static IO_t busSwitchResetPin        = IO_NONE;
 179
 180     busSwitchResetPin = IOGetByTag(IO_TAG(BUS_SWITCH_PIN));
 181     IOInit(busSwitchResetPin, OWNER_SYSTEM, 0);
 182     IOConfigGPIO(busSwitchResetPin, IOCFG_OUT_PP);
 183
 184     // ENABLE
 185     IOLo(busSwitchResetPin);
 186 }
 187 #endif
 188
 189 void init(void)
 190 {
 191 #ifdef USE_HAL_DRIVER
 192     HAL_Init();
 193 #endif
 194
 195     printfSupportInit();
 196
 197     systemInit();
 198
 199     // initialize IO (needed for all IO operations)
 200     IOInitGlobal();
 201
 202 #ifdef USE_HARDWARE_REVISION_DETECTION
 203     detectHardwareRevision();
 204 #endif
 205
 206 #ifdef BRUSHED_ESC_AUTODETECT
 207     detectBrushedESC();
 208 #endif
 209
 210     initEEPROM();
 211
 212     ensureEEPROMContainsValidData();
 213     readEEPROM();
 214
 215     systemState |= SYSTEM_STATE_CONFIG_LOADED;
 216
 217     //i2cSetOverclock(masterConfig.i2c_overclock);
 218
 219     debugMode = systemConfig()->debug_mode;
 220
 221     // Latch active features to be used for feature() in the remainder of init().
 222     latchActiveFeatures();
 223
 224 #ifdef TARGET_PREINIT
 225     targetPreInit();
 226 #endif
 227
 228     ledInit(statusLedConfig());
 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(AVOID_UART1_FOR_PWM_PPM)
 285     serialInit(feature(FEATURE_SOFTSERIAL),
 286             feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
 287 #elif defined(AVOID_UART2_FOR_PWM_PPM)
 288     serialInit(feature(FEATURE_SOFTSERIAL),
 289             feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
 290 #elif defined(AVOID_UART3_FOR_PWM_PPM)
 291     serialInit(feature(FEATURE_SOFTSERIAL),
 292             feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
 293 #else
 294     serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
 295 #endif
 296
 297     mixerInit(mixerConfig()->mixerMode);
 298 #ifdef USE_SERVOS
 299     servosInit();
 300 #endif
 301
 302     uint16_t idlePulse = motorConfig()->mincommand;
 303     if (feature(FEATURE_3D)) {
 304         idlePulse = flight3DConfig()->neutral3d;
 305     }
 306
 307     if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
 308         featureClear(FEATURE_3D);
 309         idlePulse = 0; // brushed motors
 310     }
 311
 312     mixerConfigureOutput();
 313     motorDevInit(&motorConfig()->dev, idlePulse, getMotorCount());
 314
 315 #ifdef USE_SERVOS
 316     servoConfigureOutput();
 317     if (isMixerUsingServos()) {
 318         //pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
 319         servoDevInit(&servoConfig()->dev);
 320     }
 321 #endif
 322
 323     if (0) {}
 324 #if defined(USE_PPM)
 325     else if (feature(FEATURE_RX_PPM)) {
 326           ppmRxInit(ppmConfig(), motorConfig()->dev.motorPwmProtocol);
 327     }
 328 #endif
 329 #if defined(USE_PWM)
 330     else if (feature(FEATURE_RX_PARALLEL_PWM)) {
 331           pwmRxInit(pwmConfig());
 332     }
 333 #endif
 334
 335     systemState |= SYSTEM_STATE_MOTORS_READY;
 336
 337 #ifdef BEEPER
 338     beeperInit(beeperDevConfig());
 339 #endif
 340 /* temp until PGs are implemented. */
 341 #ifdef USE_INVERTER
 342     initInverters();
 343 #endif
 344
 345 #ifdef TARGET_BUS_INIT
 346     targetBusInit();
 347 #else
 348
 349 #ifdef USE_SPI
 350 #ifdef USE_SPI_DEVICE_1
 351     spiInit(SPIDEV_1);
 352 #endif
 353 #ifdef USE_SPI_DEVICE_2
 354     spiInit(SPIDEV_2);
 355 #endif
 356 #ifdef USE_SPI_DEVICE_3
 357     spiInit(SPIDEV_3);
 358 #endif
 359 #ifdef USE_SPI_DEVICE_4
 360     spiInit(SPIDEV_4);
 361 #endif
 362 #endif /* USE_SPI */
 363
 364 #ifdef USE_I2C
 365 #ifdef USE_I2C_DEVICE_1
 366     i2cInit(I2CDEV_1);
 367 #endif
 368 #ifdef USE_I2C_DEVICE_2
 369     i2cInit(I2CDEV_2);
 370 #endif
 371 #ifdef USE_I2C_DEVICE_3
 372     i2cInit(I2CDEV_3);
 373 #endif
 374 #ifdef USE_I2C_DEVICE_4
 375     i2cInit(I2CDEV_4);
 376 #endif
 377 #endif /* USE_I2C */
 378
 379 #endif /* TARGET_BUS_INIT */
 380
 381 #ifdef USE_HARDWARE_REVISION_DETECTION
 382     updateHardwareRevision();
 383 #endif
 384
 385 #ifdef VTX_RTC6705
 386     rtc6705IOInit();
 387 #endif
 388
 389 #if defined(SONAR_SOFTSERIAL2_EXCLUSIVE) && defined(SONAR) && defined(USE_SOFTSERIAL2)
 390     if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
 391         serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
 392     }
 393 #endif
 394
 395 #if defined(SONAR_SOFTSERIAL1_EXCLUSIVE) && defined(SONAR) && defined(USE_SOFTSERIAL1)
 396     if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
 397         serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
 398     }
 399 #endif
 400
 401 #ifdef USE_ADC
 402     adcConfigMutable()->vbat.enabled = (batteryConfig()->voltageMeterSource == VOLTAGE_METER_ADC);
 403     adcConfigMutable()->current.enabled = (batteryConfig()->currentMeterSource == CURRENT_METER_ADC);
 404
 405     adcConfigMutable()->rssi.enabled = feature(FEATURE_RSSI_ADC);
 406     adcInit(adcConfig());
 407 #endif
 408
 409     initBoardAlignment(boardAlignment());
 410
 411     if (!sensorsAutodetect()) {
 412         // if gyro was not detected due to whatever reason, we give up now.
 413         failureMode(FAILURE_MISSING_ACC);
 414     }
 415
 416     systemState |= SYSTEM_STATE_SENSORS_READY;
 417
 418     LED1_ON;
 419     LED0_OFF;
 420     LED2_OFF;
 421
 422     for (int i = 0; i < 10; i++) {
 423         LED1_TOGGLE;
 424         LED0_TOGGLE;
 425         delay(25);
 426         if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) BEEP_ON;
 427         delay(25);
 428         BEEP_OFF;
 429     }
 430     LED0_OFF;
 431     LED1_OFF;
 432
 433     // gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidInit()
 434     pidInit(currentPidProfile);
 435
 436     imuInit();
 437
 438     mspFcInit();
 439     mspSerialInit();
 440
 441 #ifdef USE_CLI
 442     cliInit(serialConfig());
 443 #endif
 444
 445     failsafeInit();
 446
 447     rxInit();
 448
 449 /*
 450  * CMS, display devices and OSD
 451  */
 452 #ifdef CMS
 453     cmsInit();
 454 #endif
 455
 456 #if (defined(OSD) || (defined(USE_MSP_DISPLAYPORT) && defined(CMS)) || defined(USE_OSD_SLAVE))
 457     displayPort_t *osdDisplayPort = NULL;
 458 #endif
 459
 460 #if defined(OSD) && !defined(USE_OSD_SLAVE)
 461     //The OSD need to be initialised after GYRO to avoid GYRO initialisation failure on some targets
 462
 463     if (feature(FEATURE_OSD)) {
 464 #if defined(USE_MAX7456)
 465         // If there is a max7456 chip for the OSD then use it
 466         osdDisplayPort = max7456DisplayPortInit(vcdProfile());
 467 #elif defined(USE_OSD_OVER_MSP_DISPLAYPORT) // OSD over MSP; not supported (yet)
 468         osdDisplayPort = displayPortMspInit();
 469 #endif
 470         // osdInit  will register with CMS by itself.
 471         osdInit(osdDisplayPort);
 472     }
 473 #endif
 474
 475 #if defined(USE_OSD_SLAVE) && !defined(OSD)
 476 #if defined(USE_MAX7456)
 477     // If there is a max7456 chip for the OSD then use it
 478     osdDisplayPort = max7456DisplayPortInit(vcdProfile());
 479     // osdInit  will register with CMS by itself.
 480     osdSlaveInit(osdDisplayPort);
 481 #endif
 482 #endif
 483
 484 #if defined(CMS) && defined(USE_MSP_DISPLAYPORT)
 485     // If BFOSD is not active, then register MSP_DISPLAYPORT as a CMS device.
 486     if (!osdDisplayPort)
 487         cmsDisplayPortRegister(displayPortMspInit());
 488 #endif
 489
 490 #ifdef USE_DASHBOARD
 491     // Dashbord will register with CMS by itself.
 492     if (feature(FEATURE_DASHBOARD)) {
 493         dashboardInit();
 494     }
 495 #endif
 496
 497
 498 #ifdef GPS
 499     if (feature(FEATURE_GPS)) {
 500         gpsInit();
 501         navigationInit();
 502     }
 503 #endif
 504
 505 #ifdef LED_STRIP
 506     ledStripInit();
 507
 508     if (feature(FEATURE_LED_STRIP)) {
 509         ledStripEnable();
 510     }
 511 #endif
 512
 513 #ifdef TELEMETRY
 514     if (feature(FEATURE_TELEMETRY)) {
 515         telemetryInit();
 516     }
 517 #endif
 518
 519 #ifdef USE_ESC_SENSOR
 520     if (feature(FEATURE_ESC_SENSOR)) {
 521         escSensorInit();
 522     }
 523 #endif
 524
 525 #ifdef USB_DETECT_PIN
 526     usbCableDetectInit();
 527 #endif
 528
 529 #ifdef TRANSPONDER
 530     if (feature(FEATURE_TRANSPONDER)) {
 531         transponderInit();
 532         transponderStartRepeating();
 533         systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
 534     }
 535 #endif
 536
 537 #ifdef USE_FLASHFS
 538 #if defined(USE_FLASH_M25P16)
 539     m25p16_init(flashConfig());
 540 #endif
 541     flashfsInit();
 542 #endif
 543
 544 #ifdef USE_SDCARD
 545     if (blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD) {
 546         sdcardInsertionDetectInit();
 547         sdcard_init(sdcardConfig()->useDma);
 548         afatfs_init();
 549     }
 550 #endif
 551
 552 #ifdef BLACKBOX
 553     initBlackbox();
 554 #endif
 555
 556     if (mixerConfig()->mixerMode == MIXER_GIMBAL) {
 557         accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
 558     }
 559     gyroSetCalibrationCycles();
 560 #ifdef BARO
 561     baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
 562 #endif
 563
 564 #ifdef VTX_CONTROL
 565     vtxControlInit();
 566
 567     vtxCommonInit();
 568
 569 #ifdef VTX_SMARTAUDIO
 570     vtxSmartAudioInit();
 571 #endif
 572
 573 #ifdef VTX_TRAMP
 574     vtxTrampInit();
 575 #endif
 576
 577 #ifdef VTX_RTC6705
 578 #ifdef VTX_RTC6705OPTIONAL
 579     if (!vtxCommonDeviceRegistered()) // external VTX takes precedence when configured.
 580 #endif
 581     {
 582         vtxRTC6705Init();
 583     }
 584 #endif
 585
 586 #endif // VTX_CONTROL
 587
 588     // start all timers
 589     // TODO - not implemented yet
 590     timerStart();
 591
 592     ENABLE_STATE(SMALL_ANGLE);
 593     DISABLE_ARMING_FLAG(PREVENT_ARMING);
 594
 595 #ifdef SOFTSERIAL_LOOPBACK
 596     // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
 597     loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
 598     if (!loopbackPort->vTable) {
 599         loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
 600     }
 601     serialPrint(loopbackPort, "LOOPBACK\r\n");
 602 #endif
 603
 604     batteryInit(); // always needs doing, regardless of features.
 605
 606 #ifdef USE_DASHBOARD
 607     if (feature(FEATURE_DASHBOARD)) {
 608 #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
 609         dashboardShowFixedPage(PAGE_GPS);
 610 #else
 611         dashboardResetPageCycling();
 612         dashboardEnablePageCycling();
 613 #endif
 614     }
 615 #endif
 616
 617 #ifdef CJMCU
 618     LED2_ON;
 619 #endif
 620
 621     // Latch active features AGAIN since some may be modified by init().
 622     latchActiveFeatures();
 623     motorControlEnable = true;
 624
 625 #ifdef USE_OSD_SLAVE
 626     osdSlaveTasksInit();
 627 #else
 628     fcTasksInit();
 629 #endif
 630     systemState |= SYSTEM_STATE_READY;
 631 }