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