src/main/main.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 <stdlib.h>
  21 #include <string.h>
  22
  23 #include "platform.h"
  24
  25 #include "scheduler/scheduler.h"
  26
  27 #include "common/axis.h"
  28 #include "common/color.h"
  29 #include "common/atomic.h"
  30 #include "common/maths.h"
  31
  32 #include "drivers/nvic.h"
  33
  34 #include "drivers/sensor.h"
  35 #include "drivers/system.h"
  36 #include "drivers/gpio.h"
  37 #include "drivers/light_led.h"
  38 #include "drivers/sound_beeper.h"
  39 #include "drivers/timer.h"
  40 #include "drivers/serial.h"
  41 #include "drivers/serial_softserial.h"
  42 #include "drivers/serial_uart.h"
  43 #include "drivers/accgyro.h"
  44 #include "drivers/compass.h"
  45 #include "drivers/pwm_mapping.h"
  46 #include "drivers/pwm_rx.h"
  47 #include "drivers/adc.h"
  48 #include "drivers/bus_i2c.h"
  49 #include "drivers/bus_spi.h"
  50 #include "drivers/inverter.h"
  51 #include "drivers/flash_m25p16.h"
  52 #include "drivers/sonar_hcsr04.h"
  53 #include "drivers/gyro_sync.h"
  54
  55 #include "rx/rx.h"
  56
  57 #include "io/beeper.h"
  58 #include "io/serial.h"
  59 #include "io/flashfs.h"
  60 #include "io/gps.h"
  61 #include "io/escservo.h"
  62 #include "io/rc_controls.h"
  63 #include "io/gimbal.h"
  64 #include "io/ledstrip.h"
  65 #include "io/display.h"
  66
  67 #include "sensors/sensors.h"
  68 #include "sensors/sonar.h"
  69 #include "sensors/barometer.h"
  70 #include "sensors/compass.h"
  71 #include "sensors/acceleration.h"
  72 #include "sensors/gyro.h"
  73 #include "sensors/battery.h"
  74 #include "sensors/boardalignment.h"
  75 #include "sensors/initialisation.h"
  76
  77 #include "telemetry/telemetry.h"
  78 #include "blackbox/blackbox.h"
  79
  80 #include "flight/pid.h"
  81 #include "flight/imu.h"
  82 #include "flight/mixer.h"
  83 #include "flight/failsafe.h"
  84 #include "flight/navigation_rewrite.h"
  85
  86 #include "config/runtime_config.h"
  87 #include "config/config.h"
  88 #include "config/config_profile.h"
  89 #include "config/config_master.h"
  90
  91 #ifdef USE_HARDWARE_REVISION_DETECTION
  92 #include "hardware_revision.h"
  93 #endif
  94
  95 #include "build_config.h"
  96 #include "debug.h"
  97
  98 extern uint8_t motorControlEnable;
  99
 100 #ifdef SOFTSERIAL_LOOPBACK
 101 serialPort_t *loopbackPort;
 102 #endif
 103
 104 void printfSupportInit(void);
 105 void timerInit(void);
 106 void telemetryInit(void);
 107 void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled);
 108 void mspInit(serialConfig_t *serialConfig);
 109 void cliInit(serialConfig_t *serialConfig);
 110 void failsafeInit(rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle);
 111 pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init);
 112 #ifdef USE_SERVOS
 113 void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers, servoMixer_t *customServoMixers);
 114 #else
 115 void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers);
 116 #endif
 117 void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
 118 void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationConditions);
 119 void gpsPreInit(gpsConfig_t *initialGpsConfig);
 120 void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
 121 void imuInit(void);
 122 void displayInit(rxConfig_t *intialRxConfig);
 123 void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse);
 124 void spektrumBind(rxConfig_t *rxConfig);
 125 const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig);
 126 void sonarInit(const sonarHardware_t *sonarHardware);
 127
 128 #ifdef STM32F303xC
 129 // from system_stm32f30x.c
 130 void SetSysClock(void);
 131 #endif
 132 #ifdef STM32F10X
 133 // from system_stm32f10x.c
 134 void SetSysClock(bool overclock);
 135 #endif
 136
 137 typedef enum {
 138     SYSTEM_STATE_INITIALISING   = 0,
 139     SYSTEM_STATE_CONFIG_LOADED  = (1 << 0),
 140     SYSTEM_STATE_SENSORS_READY  = (1 << 1),
 141     SYSTEM_STATE_MOTORS_READY   = (1 << 2),
 142     SYSTEM_STATE_READY          = (1 << 7)
 143 } systemState_e;
 144
 145 static uint8_t systemState = SYSTEM_STATE_INITIALISING;
 146
 147 void flashLedsAndBeep(void)
 148 {
 149     LED1_ON;
 150     LED0_OFF;
 151     for (uint8_t i = 0; i < 10; i++) {
 152         LED1_TOGGLE;
 153         LED0_TOGGLE;
 154         delay(25);
 155         if (!(getPreferedBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1))))
 156                 BEEP_ON;
 157         delay(25);
 158         BEEP_OFF;
 159     }
 160     LED0_OFF;
 161     LED1_OFF;
 162 }
 163
 164 void init(void)
 165 {
 166     uint8_t i;
 167     drv_pwm_config_t pwm_params;
 168
 169     printfSupportInit();
 170
 171     initEEPROM();
 172
 173     ensureEEPROMContainsValidData();
 174     readEEPROM();
 175
 176     systemState |= SYSTEM_STATE_CONFIG_LOADED;
 177
 178 #ifdef STM32F303
 179     // start fpu
 180     SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
 181 #endif
 182
 183 #ifdef STM32F303xC
 184     SetSysClock();
 185 #endif
 186 #ifdef STM32F10X
 187     // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
 188     // Configure the Flash Latency cycles and enable prefetch buffer
 189     SetSysClock(masterConfig.emf_avoidance);
 190 #endif
 191     i2cSetOverclock(masterConfig.i2c_overclock);
 192
 193 #ifdef USE_HARDWARE_REVISION_DETECTION
 194     detectHardwareRevision();
 195 #endif
 196
 197     systemInit();
 198
 199     // Latch active features to be used for feature() in the remainder of init().
 200     latchActiveFeatures();
 201
 202     ledInit();
 203
 204 #ifdef SPEKTRUM_BIND
 205     if (feature(FEATURE_RX_SERIAL)) {
 206         switch (masterConfig.rxConfig.serialrx_provider) {
 207             case SERIALRX_SPEKTRUM1024:
 208             case SERIALRX_SPEKTRUM2048:
 209                 // Spektrum satellite binding if enabled on startup.
 210                 // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
 211                 // The rest of Spektrum initialization will happen later - via spektrumInit()
 212                 spektrumBind(&masterConfig.rxConfig);
 213                 break;
 214         }
 215     }
 216 #endif
 217
 218     delay(500);
 219
 220     timerInit();  // timer must be initialized before any channel is allocated
 221
 222     serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL));
 223
 224 #ifdef USE_SERVOS
 225     mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer);
 226 #else
 227     mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
 228 #endif
 229
 230     memset(&pwm_params, 0, sizeof(pwm_params));
 231
 232 #ifdef SONAR
 233     const sonarHardware_t *sonarHardware = NULL;
 234
 235     if (feature(FEATURE_SONAR)) {
 236         sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig);
 237         sonarGPIOConfig_t sonarGPIOConfig = {
 238             .gpio = SONAR_GPIO,
 239             .triggerPin = sonarHardware->echo_pin,
 240             .echoPin = sonarHardware->trigger_pin,
 241         };
 242         pwm_params.sonarGPIOConfig = &sonarGPIOConfig;
 243     }
 244 #endif
 245
 246     // when using airplane/wing mixer, servo/motor outputs are remapped
 247     if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE)
 248         pwm_params.airplane = true;
 249     else
 250         pwm_params.airplane = false;
 251 #if defined(USE_USART2) && defined(STM32F10X)
 252     pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
 253 #endif
 254 #ifdef STM32F303xC
 255     pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3);
 256 #endif
 257     pwm_params.useVbat = feature(FEATURE_VBAT);
 258     pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
 259     pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
 260     pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
 261     pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER)
 262         && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
 263     pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
 264     pwm_params.usePPM = feature(FEATURE_RX_PPM);
 265     pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
 266 #ifdef SONAR
 267     pwm_params.useSonar = feature(FEATURE_SONAR);
 268 #endif
 269
 270 #ifdef USE_SERVOS
 271     pwm_params.useServos = isMixerUsingServos();
 272     pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
 273     pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
 274     pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
 275 #endif
 276
 277     pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
 278     pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
 279     pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
 280     if (feature(FEATURE_3D))
 281         pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
 282     if (pwm_params.motorPwmRate > 500)
 283         pwm_params.idlePulse = 0; // brushed motors
 284
 285     pwmRxInit(masterConfig.inputFilteringMode);
 286
 287     pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
 288
 289     mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
 290
 291     if (!feature(FEATURE_ONESHOT125))
 292         motorControlEnable = true;
 293
 294     systemState |= SYSTEM_STATE_MOTORS_READY;
 295
 296 #ifdef BEEPER
 297     beeperConfig_t beeperConfig = {
 298         .gpioPeripheral = BEEP_PERIPHERAL,
 299         .gpioPin = BEEP_PIN,
 300         .gpioPort = BEEP_GPIO,
 301 #ifdef BEEPER_INVERTED
 302         .gpioMode = Mode_Out_PP,
 303         .isInverted = true
 304 #else
 305         .gpioMode = Mode_Out_OD,
 306         .isInverted = false
 307 #endif
 308     };
 309 #ifdef NAZE
 310     if (hardwareRevision >= NAZE32_REV5) {
 311         // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
 312         beeperConfig.gpioMode = Mode_Out_PP;
 313         beeperConfig.isInverted = true;
 314     }
 315 #endif
 316
 317     beeperInit(&beeperConfig);
 318 #endif
 319
 320 #ifdef INVERTER
 321     initInverter();
 322 #endif
 323
 324
 325 #ifdef USE_SPI
 326     spiInit(SPI1);
 327     spiInit(SPI2);
 328 #endif
 329
 330 #ifdef USE_HARDWARE_REVISION_DETECTION
 331     updateHardwareRevision();
 332 #endif
 333
 334 #if defined(NAZE)
 335     if (hardwareRevision == NAZE32_SP) {
 336         serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
 337     } else  {
 338         serialRemovePort(SERIAL_PORT_USART3);
 339     }
 340 #endif
 341
 342 #if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2)
 343     if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
 344         serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
 345     }
 346 #endif
 347
 348
 349 #ifdef USE_I2C
 350 #if defined(NAZE)
 351     if (hardwareRevision != NAZE32_SP) {
 352         i2cInit(I2C_DEVICE);
 353     } else {
 354         if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
 355             i2cInit(I2C_DEVICE);
 356         }
 357     }
 358 #elif defined(CC3D)
 359     if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
 360         i2cInit(I2C_DEVICE);
 361     }
 362 #else
 363     i2cInit(I2C_DEVICE);
 364 #endif
 365 #endif
 366
 367 #ifdef USE_ADC
 368     drv_adc_config_t adc_params;
 369
 370     adc_params.enableVBat = feature(FEATURE_VBAT);
 371     adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
 372     adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
 373     adc_params.enableExternal1 = false;
 374 #ifdef OLIMEXINO
 375     adc_params.enableExternal1 = true;
 376 #endif
 377 #ifdef NAZE
 378     // optional ADC5 input on rev.5 hardware
 379     adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5);
 380 #endif
 381
 382     adcInit(&adc_params);
 383 #endif
 384
 385
 386     initBoardAlignment(&masterConfig.boardAlignment);
 387
 388 #ifdef DISPLAY
 389     if (feature(FEATURE_DISPLAY)) {
 390         displayInit(&masterConfig.rxConfig);
 391     }
 392 #endif
 393
 394 #ifdef GPS
 395     if (feature(FEATURE_GPS)) {
 396         gpsPreInit(&masterConfig.gpsConfig);
 397     }
 398 #endif
 399
 400     if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf,
 401         masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination,
 402         masterConfig.looptime, masterConfig.gyroSync, masterConfig.gyroSyncDenominator)) {
 403
 404         // if gyro was not detected due to whatever reason, we give up now.
 405         failureMode(FAILURE_MISSING_ACC);
 406     }
 407
 408     systemState |= SYSTEM_STATE_SENSORS_READY;
 409
 410     LED1_ON;
 411     LED0_OFF;
 412     for (i = 0; i < 10; i++) {
 413         LED1_TOGGLE;
 414         LED0_TOGGLE;
 415         delay(25);
 416         BEEP_ON;
 417         delay(25);
 418         BEEP_OFF;
 419     }
 420     LED0_OFF;
 421     LED1_OFF;
 422
 423 #ifdef MAG
 424     if (sensors(SENSOR_MAG))
 425         compassInit();
 426 #endif
 427
 428     imuInit();
 429
 430     mspInit(&masterConfig.serialConfig);
 431
 432 #ifdef USE_CLI
 433     cliInit(&masterConfig.serialConfig);
 434 #endif
 435
 436     failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 437
 438     rxInit(&masterConfig.rxConfig, currentProfile->modeActivationConditions);
 439
 440 #ifdef GPS
 441     if (feature(FEATURE_GPS)) {
 442         gpsInit(
 443             &masterConfig.serialConfig,
 444             &masterConfig.gpsConfig
 445         );
 446     }
 447 #endif
 448
 449 #ifdef NAV
 450         navigationInit(
 451             &masterConfig.navConfig,
 452             &currentProfile->pidProfile,
 453             &currentProfile->rcControlsConfig,
 454             &masterConfig.rxConfig,
 455             &masterConfig.flight3DConfig,
 456             &masterConfig.escAndServoConfig
 457         );
 458 #endif
 459
 460 #ifdef SONAR
 461     if (feature(FEATURE_SONAR)) {
 462         sonarInit(sonarHardware);
 463     }
 464 #endif
 465
 466 #ifdef LED_STRIP
 467     ledStripInit(masterConfig.ledConfigs, masterConfig.colors);
 468
 469     if (feature(FEATURE_LED_STRIP)) {
 470         ledStripEnable();
 471     }
 472 #endif
 473
 474 #ifdef TELEMETRY
 475     if (feature(FEATURE_TELEMETRY)) {
 476         telemetryInit();
 477     }
 478 #endif
 479
 480 #ifdef USE_FLASHFS
 481 #ifdef NAZE
 482     if (hardwareRevision == NAZE32_REV5) {
 483         m25p16_init();
 484     }
 485 #elif defined(USE_FLASH_M25P16)
 486     m25p16_init();
 487 #endif
 488
 489     flashfsInit();
 490 #endif
 491
 492 #ifdef BLACKBOX
 493     initBlackbox();
 494 #endif
 495
 496     if (masterConfig.mixerMode == MIXER_GIMBAL) {
 497         accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
 498     }
 499     gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
 500 #ifdef BARO
 501     baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
 502 #endif
 503
 504     // start all timers
 505     // TODO - not implemented yet
 506     timerStart();
 507
 508     ENABLE_STATE(SMALL_ANGLE);
 509     DISABLE_ARMING_FLAG(PREVENT_ARMING);
 510
 511 #ifdef SOFTSERIAL_LOOPBACK
 512     // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
 513     loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
 514     if (!loopbackPort->vTable) {
 515         loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
 516     }
 517     serialPrint(loopbackPort, "LOOPBACK\r\n");
 518 #endif
 519
 520     // Now that everything has powered up the voltage and cell count be determined.
 521
 522     if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
 523         batteryInit(&masterConfig.batteryConfig);
 524
 525 #ifdef CJMCU
 526     LED2_ON;
 527 #endif
 528
 529     // Latch active features AGAIN since some may be modified by init().
 530     latchActiveFeatures();
 531     motorControlEnable = true;
 532
 533     systemState |= SYSTEM_STATE_READY;
 534 }
 535
 536 #ifdef SOFTSERIAL_LOOPBACK
 537 void processLoopback(void) {
 538     if (loopbackPort) {
 539         uint8_t bytesWaiting;
 540         while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) {
 541             uint8_t b = serialRead(loopbackPort);
 542             serialWrite(loopbackPort, b);
 543         };
 544     }
 545 }
 546 #else
 547 #define processLoopback()
 548 #endif
 549
 550 int main(void) {
 551     init();
 552
 553     /* Setup scheduler */
 554     schedulerInit();
 555
 556     rescheduleTask(TASK_GYROPID, targetLooptime);
 557     setTaskEnabled(TASK_GYROPID, true);
 558
 559     setTaskEnabled(TASK_SERIAL, true);
 560 #ifdef BEEPER
 561     setTaskEnabled(TASK_BEEPER, true);
 562 #endif
 563     setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER));
 564     setTaskEnabled(TASK_RX, true);
 565 #ifdef GPS
 566     setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
 567 #endif
 568 #ifdef MAG
 569     setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
 570 #endif
 571 #ifdef BARO
 572     setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
 573 #endif
 574 #ifdef SONAR
 575     setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
 576 #endif
 577 #ifdef DISPLAY
 578     setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY));
 579 #endif
 580 #ifdef TELEMETRY
 581     setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
 582 #endif
 583 #ifdef LED_STRIP
 584     setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
 585 #endif
 586
 587     while (1) {
 588         scheduler();
 589         processLoopback();
 590     }
 591 }
 592
 593 void HardFault_Handler(void)
 594 {
 595     // fall out of the sky
 596     uint8_t requiredState = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY;
 597     if ((systemState & requiredState) == requiredState) {
 598         stopMotors();
 599     }
 600     while (1);
 601 }