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