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