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