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 "cms/cms.h"
  32 #include "cms/cms_types.h"
  33
  34 #include "drivers/nvic.h"
  35 #include "drivers/sensor.h"
  36 #include "drivers/system.h"
  37 #include "drivers/dma.h"
  38 #include "drivers/io.h"
  39 #include "drivers/light_led.h"
  40 #include "drivers/sound_beeper.h"
  41 #include "drivers/timer.h"
  42 #include "drivers/serial.h"
  43 #include "drivers/serial_softserial.h"
  44 #include "drivers/serial_uart.h"
  45 #include "drivers/accgyro.h"
  46 #include "drivers/compass.h"
  47 #include "drivers/pwm_esc_detect.h"
  48 #include "drivers/rx_pwm.h"
  49 #include "drivers/pwm_output.h"
  50 #include "drivers/adc.h"
  51 #include "drivers/bus_i2c.h"
  52 #include "drivers/bus_spi.h"
  53 #include "drivers/inverter.h"
  54 #include "drivers/flash_m25p16.h"
  55 #include "drivers/sonar_hcsr04.h"
  56 #include "drivers/sdcard.h"
  57 #include "drivers/usb_io.h"
  58 #include "drivers/transponder_ir.h"
  59 #include "drivers/exti.h"
  60 #include "drivers/vtx_soft_spi_rtc6705.h"
  61
  62 #ifdef USE_BST
  63 #include "bus_bst.h"
  64 #endif
  65
  66 #include "fc/config.h"
  67 #include "fc/fc_init.h"
  68 #include "fc/fc_msp.h"
  69 #include "fc/fc_tasks.h"
  70 #include "fc/rc_controls.h"
  71 #include "fc/runtime_config.h"
  72 #include "fc/cli.h"
  73
  74 #include "msp/msp_serial.h"
  75
  76 #include "rx/rx.h"
  77 #include "rx/spektrum.h"
  78
  79 #include "io/beeper.h"
  80 #include "io/displayport_max7456.h"
  81 #include "io/serial.h"
  82 #include "io/flashfs.h"
  83 #include "io/gps.h"
  84 #include "io/motors.h"
  85 #include "io/servos.h"
  86 #include "io/gimbal.h"
  87 #include "io/ledstrip.h"
  88 #include "io/dashboard.h"
  89 #include "io/asyncfatfs/asyncfatfs.h"
  90 #include "io/transponder_ir.h"
  91 #include "io/osd.h"
  92 #include "io/displayport_msp.h"
  93 #include "io/vtx.h"
  94 #include "io/vtx_smartaudio.h"
  95 #include "io/vtx_tramp.h"
  96
  97 #include "scheduler/scheduler.h"
  98
  99 #include "sensors/sensors.h"
 100 #include "sensors/sonar.h"
 101 #include "sensors/barometer.h"
 102 #include "sensors/compass.h"
 103 #include "sensors/acceleration.h"
 104 #include "sensors/gyro.h"
 105 #include "sensors/battery.h"
 106 #include "sensors/boardalignment.h"
 107 #include "sensors/initialisation.h"
 108
 109 #include "telemetry/telemetry.h"
 110 #include "sensors/esc_sensor.h"
 111
 112 #include "flight/pid.h"
 113 #include "flight/imu.h"
 114 #include "flight/mixer.h"
 115 #include "flight/failsafe.h"
 116 #include "flight/navigation.h"
 117
 118 #include "config/config_eeprom.h"
 119 #include "config/config_profile.h"
 120 #include "config/config_master.h"
 121 #include "config/feature.h"
 122
 123 #ifdef USE_HARDWARE_REVISION_DETECTION
 124 #include "hardware_revision.h"
 125 #endif
 126
 127 #include "build/build_config.h"
 128 #include "build/debug.h"
 129
 130 #ifdef TARGET_PREINIT
 131 void targetPreInit(void);
 132 #endif
 133
 134 #ifdef TARGET_BUS_INIT
 135 void targetBusInit(void);
 136 #endif
 137
 138 extern uint8_t motorControlEnable;
 139
 140 #ifdef SOFTSERIAL_LOOPBACK
 141 serialPort_t *loopbackPort;
 142 #endif
 143
 144 uint8_t systemState = SYSTEM_STATE_INITIALISING;
 145
 146 void processLoopback(void)
 147 {
 148 #ifdef SOFTSERIAL_LOOPBACK
 149     if (loopbackPort) {
 150         uint8_t bytesWaiting;
 151         while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) {
 152             uint8_t b = serialRead(loopbackPort);
 153             serialWrite(loopbackPort, b);
 154         };
 155     }
 156 #endif
 157 }
 158
 159 void init(void)
 160 {
 161 #ifdef USE_HAL_DRIVER
 162     HAL_Init();
 163 #endif
 164
 165     printfSupportInit();
 166
 167     systemInit();
 168
 169     // initialize IO (needed for all IO operations)
 170     IOInitGlobal();
 171
 172 #ifdef USE_HARDWARE_REVISION_DETECTION
 173     detectHardwareRevision();
 174 #endif
 175
 176 #ifdef BRUSHED_ESC_AUTODETECT
 177     detectBrushedESC();
 178 #endif
 179
 180     initEEPROM();
 181
 182     ensureEEPROMContainsValidData();
 183     readEEPROM();
 184
 185     systemState |= SYSTEM_STATE_CONFIG_LOADED;
 186
 187     //i2cSetOverclock(masterConfig.i2c_overclock);
 188
 189     debugMode = masterConfig.debug_mode;
 190
 191     // Latch active features to be used for feature() in the remainder of init().
 192     latchActiveFeatures();
 193
 194 #ifdef TARGET_PREINIT
 195     targetPreInit();
 196 #endif
 197
 198     ledInit(statusLedConfig());
 199     LED2_ON;
 200
 201 #ifdef USE_EXTI
 202     EXTIInit();
 203 #endif
 204
 205 #if defined(BUTTONS)
 206 #ifdef BUTTON_A_PIN
 207     IO_t buttonAPin = IOGetByTag(IO_TAG(BUTTON_A_PIN));
 208     IOInit(buttonAPin, OWNER_SYSTEM, 0);
 209     IOConfigGPIO(buttonAPin, IOCFG_IPU);
 210 #endif
 211
 212 #ifdef BUTTON_B_PIN
 213     IO_t buttonBPin = IOGetByTag(IO_TAG(BUTTON_B_PIN));
 214     IOInit(buttonBPin, OWNER_SYSTEM, 0);
 215     IOConfigGPIO(buttonBPin, IOCFG_IPU);
 216 #endif
 217
 218     // Check status of bind plug and exit if not active
 219     delayMicroseconds(10);  // allow configuration to settle
 220
 221     if (!isMPUSoftReset()) {
 222 #if defined(BUTTON_A_PIN) && defined(BUTTON_B_PIN)
 223         // two buttons required
 224         uint8_t secondsRemaining = 5;
 225         bool bothButtonsHeld;
 226         do {
 227             bothButtonsHeld = !IORead(buttonAPin) && !IORead(buttonBPin);
 228             if (bothButtonsHeld) {
 229                 if (--secondsRemaining == 0) {
 230                     resetEEPROM();
 231                     systemReset();
 232                 }
 233                 delay(1000);
 234                 LED0_TOGGLE;
 235             }
 236         } while (bothButtonsHeld);
 237 #endif
 238     }
 239 #endif
 240
 241
 242 #ifdef SPEKTRUM_BIND
 243     if (feature(FEATURE_RX_SERIAL)) {
 244         switch (rxConfig()->serialrx_provider) {
 245             case SERIALRX_SPEKTRUM1024:
 246             case SERIALRX_SPEKTRUM2048:
 247                 // Spektrum satellite binding if enabled on startup.
 248                 // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
 249                 // The rest of Spektrum initialization will happen later - via spektrumInit()
 250                 spektrumBind(rxConfig());
 251                 break;
 252         }
 253     }
 254     delay(200); // Make sure the satellite times out and closes the bind window. (Delay(100) below is not enough and needed for non-sat configs)
 255 #endif
 256
 257     delay(100);
 258
 259     timerInit();  // timer must be initialized before any channel is allocated
 260
 261 #if defined(AVOID_UART1_FOR_PWM_PPM)
 262     serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL),
 263             feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
 264 #elif defined(AVOID_UART2_FOR_PWM_PPM)
 265     serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL),
 266             feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
 267 #elif defined(AVOID_UART3_FOR_PWM_PPM)
 268     serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL),
 269             feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
 270 #else
 271     serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
 272 #endif
 273
 274     mixerInit(mixerConfig()->mixerMode, masterConfig.customMotorMixer);
 275 #ifdef USE_SERVOS
 276     servoMixerInit(masterConfig.customServoMixer);
 277 #endif
 278
 279     uint16_t idlePulse = motorConfig()->mincommand;
 280     if (feature(FEATURE_3D)) {
 281         idlePulse = flight3DConfig()->neutral3d;
 282     }
 283
 284     if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) {
 285         featureClear(FEATURE_3D);
 286         idlePulse = 0; // brushed motors
 287     }
 288
 289     mixerConfigureOutput();
 290     motorInit(motorConfig(), idlePulse, getMotorCount());
 291
 292 #ifdef USE_SERVOS
 293     servoConfigureOutput();
 294     if (isMixerUsingServos()) {
 295         //pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
 296         servoInit(servoConfig());
 297     }
 298 #endif
 299
 300 #if defined(USE_PWM) || defined(USE_PPM)
 301     if (feature(FEATURE_RX_PPM)) {
 302         ppmRxInit(ppmConfig(), motorConfig()->motorPwmProtocol);
 303     } else if (feature(FEATURE_RX_PARALLEL_PWM)) {
 304         pwmRxInit(pwmConfig());
 305     }
 306 #endif
 307
 308     systemState |= SYSTEM_STATE_MOTORS_READY;
 309
 310 #ifdef BEEPER
 311     beeperInit(beeperConfig());
 312 #endif
 313 /* temp until PGs are implemented. */
 314 #ifdef USE_INVERTER
 315     initInverters();
 316 #endif
 317
 318 #ifdef USE_BST
 319     bstInit(BST_DEVICE);
 320 #endif
 321
 322 #ifdef TARGET_BUS_INIT
 323     targetBusInit();
 324 #else
 325     #ifdef USE_SPI
 326         #ifdef USE_SPI_DEVICE_1
 327             spiInit(SPIDEV_1);
 328         #endif
 329         #ifdef USE_SPI_DEVICE_2
 330             spiInit(SPIDEV_2);
 331         #endif
 332         #ifdef USE_SPI_DEVICE_3
 333             spiInit(SPIDEV_3);
 334         #endif
 335         #ifdef USE_SPI_DEVICE_4
 336             spiInit(SPIDEV_4);
 337         #endif
 338     #endif
 339
 340     #ifdef USE_I2C
 341         i2cInit(I2C_DEVICE);
 342     #endif
 343 #endif
 344
 345 #ifdef USE_HARDWARE_REVISION_DETECTION
 346     updateHardwareRevision();
 347 #endif
 348
 349 #ifdef VTX
 350     vtxInit();
 351 #endif
 352
 353 #if defined(SONAR_SOFTSERIAL2_EXCLUSIVE) && defined(SONAR) && defined(USE_SOFTSERIAL2)
 354     if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
 355         serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
 356     }
 357 #endif
 358
 359 #if defined(SONAR_SOFTSERIAL1_EXCLUSIVE) && defined(SONAR) && defined(USE_SOFTSERIAL1)
 360     if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
 361         serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
 362     }
 363 #endif
 364
 365 #ifdef USE_ADC
 366     /* these can be removed from features! */
 367     adcConfig()->vbat.enabled = feature(FEATURE_VBAT);
 368     adcConfig()->currentMeter.enabled = feature(FEATURE_CURRENT_METER);
 369     adcConfig()->rssi.enabled = feature(FEATURE_RSSI_ADC);
 370     adcInit(adcConfig());
 371 #endif
 372
 373     initBoardAlignment(boardAlignment());
 374
 375 #ifdef CMS
 376     cmsInit();
 377 #endif
 378
 379 #ifdef USE_DASHBOARD
 380     if (feature(FEATURE_DASHBOARD)) {
 381         dashboardInit(rxConfig());
 382     }
 383 #endif
 384
 385 #ifdef USE_RTC6705
 386     if (feature(FEATURE_VTX)) {
 387         rtc6705_soft_spi_init();
 388         current_vtx_channel = masterConfig.vtx_channel;
 389         rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
 390         rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power);
 391     }
 392 #endif
 393
 394 #ifdef OSD
 395     if (feature(FEATURE_OSD)) {
 396 #ifdef USE_MAX7456
 397         // if there is a max7456 chip for the OSD then use it, otherwise use MSP
 398         displayPort_t *osdDisplayPort = max7456DisplayPortInit(vcdProfile(), displayPortProfileMax7456());
 399 #else
 400         displayPort_t *osdDisplayPort = displayPortMspInit(displayPortProfileMax7456());
 401 #endif
 402         osdInit(osdDisplayPort);
 403     }
 404 #endif
 405
 406 #ifdef SONAR
 407     const sonarConfig_t *sonarConfig = sonarConfig();
 408 #else
 409     const void *sonarConfig = NULL;
 410 #endif
 411     if (!sensorsAutodetect(gyroConfig(), accelerometerConfig(), compassConfig(), barometerConfig(), sonarConfig)) {
 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 pidSetTargetLooptime()
 434     pidSetTargetLooptime(gyro.targetLooptime * pidConfig()->pid_process_denom); // Initialize pid looptime
 435     pidInitFilters(&currentProfile->pidProfile);
 436     pidInitConfig(&currentProfile->pidProfile);
 437
 438     imuInit();
 439
 440     mspFcInit();
 441     mspSerialInit();
 442
 443 #if defined(USE_MSP_DISPLAYPORT) && defined(CMS)
 444     cmsDisplayPortRegister(displayPortMspInit(displayPortProfileMsp()));
 445 #endif
 446
 447 #ifdef USE_CLI
 448     cliInit(serialConfig());
 449 #endif
 450
 451     failsafeInit(rxConfig(), flight3DConfig()->deadband3d_throttle);
 452
 453     rxInit(rxConfig(), modeActivationProfile()->modeActivationConditions);
 454
 455 #ifdef GPS
 456     if (feature(FEATURE_GPS)) {
 457         gpsInit(
 458             serialConfig(),
 459             gpsConfig()
 460         );
 461         navigationInit(
 462             gpsProfile(),
 463             &currentProfile->pidProfile
 464         );
 465     }
 466 #endif
 467
 468 #ifdef LED_STRIP
 469     ledStripInit(ledStripConfig());
 470
 471     if (feature(FEATURE_LED_STRIP)) {
 472         ledStripEnable();
 473     }
 474 #endif
 475
 476 #ifdef TELEMETRY
 477     if (feature(FEATURE_TELEMETRY)) {
 478         telemetryInit();
 479     }
 480 #endif
 481
 482 #ifdef USE_ESC_SENSOR
 483     if (feature(FEATURE_ESC_SENSOR)) {
 484         escSensorInit();
 485     }
 486 #endif
 487
 488 #ifdef USB_CABLE_DETECTION
 489     usbCableDetectInit();
 490 #endif
 491
 492 #ifdef TRANSPONDER
 493     if (feature(FEATURE_TRANSPONDER)) {
 494         transponderInit(masterConfig.transponderData);
 495         transponderStartRepeating();
 496         systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
 497     }
 498 #endif
 499
 500 #ifdef USE_FLASHFS
 501     if (blackboxConfig()->device == BLACKBOX_SPIFLASH) {
 502 #if defined(USE_FLASH_M25P16)
 503         m25p16_init(flashConfig());
 504 #endif
 505         flashfsInit();
 506     }
 507 #endif
 508
 509 #ifdef USE_SDCARD
 510     if (feature(FEATURE_SDCARD) && blackboxConfig()->device == BLACKBOX_SDCARD) {
 511         sdcardInsertionDetectInit();
 512         sdcard_init(sdcardConfig()->useDma);
 513         afatfs_init();
 514     }
 515 #endif
 516
 517 #ifdef BLACKBOX
 518     initBlackbox();
 519 #endif
 520
 521     if (mixerConfig()->mixerMode == MIXER_GIMBAL) {
 522         accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
 523     }
 524     gyroSetCalibrationCycles();
 525 #ifdef BARO
 526     baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
 527 #endif
 528
 529 #ifdef VTX_CONTROL
 530
 531 #ifdef VTX_SMARTAUDIO
 532     smartAudioInit();
 533 #endif
 534
 535 #ifdef VTX_TRAMP
 536     trampInit();
 537 #endif
 538
 539 #endif // VTX_CONTROL
 540
 541     // start all timers
 542     // TODO - not implemented yet
 543     timerStart();
 544
 545     ENABLE_STATE(SMALL_ANGLE);
 546     DISABLE_ARMING_FLAG(PREVENT_ARMING);
 547
 548 #ifdef SOFTSERIAL_LOOPBACK
 549     // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
 550     loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
 551     if (!loopbackPort->vTable) {
 552         loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
 553     }
 554     serialPrint(loopbackPort, "LOOPBACK\r\n");
 555 #endif
 556
 557     // Now that everything has powered up the voltage and cell count be determined.
 558
 559     if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
 560         batteryInit(batteryConfig());
 561
 562 #ifdef USE_DASHBOARD
 563     if (feature(FEATURE_DASHBOARD)) {
 564 #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
 565         dashboardShowFixedPage(PAGE_GPS);
 566 #else
 567         dashboardResetPageCycling();
 568         dashboardEnablePageCycling();
 569 #endif
 570     }
 571 #endif
 572
 573 #ifdef CJMCU
 574     LED2_ON;
 575 #endif
 576
 577     // Latch active features AGAIN since some may be modified by init().
 578     latchActiveFeatures();
 579     motorControlEnable = true;
 580
 581     fcTasksInit();
 582     systemState |= SYSTEM_STATE_READY;
 583 }