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
  51 #include "rx/rx.h"
  52
  53 #include "io/serial.h"
  54 #include "io/flashfs.h"
  55 #include "io/gps.h"
  56 #include "io/escservo.h"
  57 #include "io/rc_controls.h"
  58 #include "io/gimbal.h"
  59 #include "io/ledstrip.h"
  60 #include "io/display.h"
  61
  62 #include "sensors/sensors.h"
  63 #include "sensors/sonar.h"
  64 #include "sensors/barometer.h"
  65 #include "sensors/compass.h"
  66 #include "sensors/acceleration.h"
  67 #include "sensors/gyro.h"
  68 #include "sensors/battery.h"
  69 #include "sensors/boardalignment.h"
  70
  71 #include "telemetry/telemetry.h"
  72 #include "blackbox/blackbox.h"
  73
  74 #include "flight/pid.h"
  75 #include "flight/imu.h"
  76 #include "flight/mixer.h"
  77 #include "flight/failsafe.h"
  78 #include "flight/navigation.h"
  79
  80 #include "config/runtime_config.h"
  81 #include "config/config.h"
  82 #include "config/config_profile.h"
  83 #include "config/config_master.h"
  84
  85 #ifdef USE_HARDWARE_REVISION_DETECTION
  86 #include "hardware_revision.h"
  87 #endif
  88
  89 #include "build_config.h"
  90
  91 #ifdef DEBUG_SECTION_TIMES
  92 uint32_t sectionTimes[2][4];
  93 #endif
  94 extern uint32_t previousTime;
  95
  96 #ifdef SOFTSERIAL_LOOPBACK
  97 serialPort_t *loopbackPort;
  98 #endif
  99
 100 failsafe_t *failsafe;
 101
 102 void printfSupportInit(void);
 103 void timerInit(void);
 104 void telemetryInit(void);
 105 void serialInit(serialConfig_t *initialSerialConfig);
 106 failsafe_t* failsafeInit(rxConfig_t *intialRxConfig);
 107 pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init);
 108 void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMixers);
 109 void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
 110 void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe);
 111 void beepcodeInit(failsafe_t *initialFailsafe);
 112 void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
 113 void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
 114 bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int8_t magHardwareToUse, int16_t magDeclinationFromConfig);
 115 void imuInit(void);
 116 void displayInit(rxConfig_t *intialRxConfig);
 117 void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse);
 118 void loop(void);
 119 void spektrumBind(rxConfig_t *rxConfig);
 120
 121 #ifdef STM32F303xC
 122 // from system_stm32f30x.c
 123 void SetSysClock(void);
 124 #endif
 125 #ifdef STM32F10X
 126 // from system_stm32f10x.c
 127 void SetSysClock(bool overclock);
 128 #endif
 129
 130 void init(void)
 131 {
 132     uint8_t i;
 133     drv_pwm_config_t pwm_params;
 134     bool sensorsOK = false;
 135
 136     printfSupportInit();
 137
 138     initEEPROM();
 139
 140     ensureEEPROMContainsValidData();
 141     readEEPROM();
 142
 143 #ifdef STM32F303
 144     // start fpu
 145     SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
 146 #endif
 147
 148 #ifdef STM32F303xC
 149     SetSysClock();
 150 #endif
 151 #ifdef STM32F10X
 152     // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
 153     // Configure the Flash Latency cycles and enable prefetch buffer
 154     SetSysClock(masterConfig.emf_avoidance);
 155 #endif
 156
 157 #ifdef USE_HARDWARE_REVISION_DETECTION
 158     detectHardwareRevision();
 159 #endif
 160
 161     systemInit();
 162
 163     ledInit();
 164
 165 #ifdef SPEKTRUM_BIND
 166     if (feature(FEATURE_RX_SERIAL)) {
 167         switch (masterConfig.rxConfig.serialrx_provider) {
 168             case SERIALRX_SPEKTRUM1024:
 169             case SERIALRX_SPEKTRUM2048:
 170                 // Spektrum satellite binding if enabled on startup.
 171                 // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
 172                 // The rest of Spektrum initialization will happen later - via spektrumInit()
 173                 spektrumBind(&masterConfig.rxConfig);
 174                 break;
 175         }
 176     }
 177 #endif
 178
 179     delay(100);
 180
 181     timerInit();  // timer must be initialized before any channel is allocated
 182
 183     mixerInit(masterConfig.mixerMode, masterConfig.customMixer);
 184
 185     memset(&pwm_params, 0, sizeof(pwm_params));
 186     // when using airplane/wing mixer, servo/motor outputs are remapped
 187     if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING)
 188         pwm_params.airplane = true;
 189     else
 190         pwm_params.airplane = false;
 191 #if defined(USE_USART2) && defined(STM32F10X)
 192     pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
 193 #endif
 194     pwm_params.useVbat = feature(FEATURE_VBAT);
 195     pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
 196     pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
 197     pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
 198     pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER)
 199         && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
 200     pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
 201     pwm_params.usePPM = feature(FEATURE_RX_PPM);
 202     pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
 203
 204 #ifdef USE_SERVOS
 205     pwm_params.useServos = isMixerUsingServos();
 206     pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX;
 207     pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
 208     pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
 209 #endif
 210
 211     pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
 212     pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
 213     pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
 214     if (feature(FEATURE_3D))
 215         pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
 216     if (pwm_params.motorPwmRate > 500)
 217         pwm_params.idlePulse = 0; // brushed motors
 218
 219     pwmRxInit(masterConfig.inputFilteringMode);
 220
 221     pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
 222
 223     mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
 224
 225 #ifdef BEEPER
 226     beeperConfig_t beeperConfig = {
 227         .gpioPin = BEEP_PIN,
 228         .gpioPort = BEEP_GPIO,
 229         .gpioPeripheral = BEEP_PERIPHERAL,
 230 #ifdef BEEPER_INVERTED
 231         .gpioMode = Mode_Out_PP,
 232         .isInverted = true
 233 #else
 234         .gpioMode = Mode_Out_OD,
 235         .isInverted = false
 236 #endif
 237     };
 238 #ifdef NAZE
 239     if (hardwareRevision >= NAZE32_REV5) {
 240         // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
 241         beeperConfig.gpioMode = Mode_Out_PP;
 242         beeperConfig.isInverted = true;
 243     }
 244 #endif
 245
 246     beeperInit(&beeperConfig);
 247 #endif
 248
 249 #ifdef INVERTER
 250     initInverter();
 251 #endif
 252
 253
 254 #ifdef USE_SPI
 255     spiInit(SPI1);
 256     spiInit(SPI2);
 257 #endif
 258
 259 #ifdef USE_HARDWARE_REVISION_DETECTION
 260     updateHardwareRevision();
 261 #endif
 262
 263 #ifdef USE_I2C
 264 #if defined(NAZE)
 265     if (hardwareRevision != NAZE32_SP) {
 266         i2cInit(I2C_DEVICE);
 267     }
 268 #elif defined(CC3D)
 269     if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
 270         i2cInit(I2C_DEVICE);
 271     }
 272 #else
 273     i2cInit(I2C_DEVICE);
 274 #endif
 275 #endif
 276
 277 #ifdef USE_ADC
 278     drv_adc_config_t adc_params;
 279
 280     adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
 281     adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
 282     adc_params.enableExternal1 = false;
 283 #ifdef OLIMEXINO
 284     adc_params.enableExternal1 = true;
 285 #endif
 286 #ifdef NAZE
 287     // optional ADC5 input on rev.5 hardware
 288     adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5);
 289 #endif
 290
 291     adcInit(&adc_params);
 292 #endif
 293
 294
 295     initBoardAlignment(&masterConfig.boardAlignment);
 296
 297 #ifdef DISPLAY
 298     if (feature(FEATURE_DISPLAY)) {
 299         displayInit(&masterConfig.rxConfig);
 300     }
 301 #endif
 302
 303     sensorsOK = sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination);
 304
 305     // if gyro was not detected due to whatever reason, we give up now.
 306     if (!sensorsOK)
 307         failureMode(3);
 308
 309     LED1_ON;
 310     LED0_OFF;
 311     for (i = 0; i < 10; i++) {
 312         LED1_TOGGLE;
 313         LED0_TOGGLE;
 314         delay(25);
 315         BEEP_ON;
 316         delay(25);
 317         BEEP_OFF;
 318     }
 319     LED0_OFF;
 320     LED1_OFF;
 321
 322 #ifdef MAG
 323     if (sensors(SENSOR_MAG))
 324         compassInit();
 325 #endif
 326
 327     imuInit();
 328
 329     serialInit(&masterConfig.serialConfig);
 330
 331     failsafe = failsafeInit(&masterConfig.rxConfig);
 332     beepcodeInit(failsafe);
 333     rxInit(&masterConfig.rxConfig, failsafe);
 334
 335 #ifdef GPS
 336     if (feature(FEATURE_GPS)) {
 337         gpsInit(
 338             &masterConfig.serialConfig,
 339             &masterConfig.gpsConfig
 340         );
 341         navigationInit(
 342             &currentProfile->gpsProfile,
 343             &currentProfile->pidProfile
 344         );
 345     }
 346 #endif
 347
 348 #ifdef SONAR
 349     if (feature(FEATURE_SONAR)) {
 350         sonarInit();
 351     }
 352 #endif
 353
 354 #ifdef LED_STRIP
 355     ledStripInit(masterConfig.ledConfigs, masterConfig.colors, failsafe);
 356
 357     if (feature(FEATURE_LED_STRIP)) {
 358         ledStripEnable();
 359     }
 360 #endif
 361
 362 #ifdef TELEMETRY
 363     if (feature(FEATURE_TELEMETRY))
 364         telemetryInit();
 365 #endif
 366
 367 #ifdef USE_FLASHFS
 368 #ifdef NAZE
 369     if (hardwareRevision == NAZE32_REV5) {
 370         m25p16_init();
 371     }
 372 #endif
 373 #ifdef SPRACINGF3
 374     m25p16_init();
 375 #endif
 376     flashfsInit();
 377 #endif
 378
 379 #ifdef BLACKBOX
 380     initBlackbox();
 381 #endif
 382
 383     previousTime = micros();
 384
 385     if (masterConfig.mixerMode == MIXER_GIMBAL) {
 386         accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
 387     }
 388     gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
 389 #ifdef BARO
 390     baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
 391 #endif
 392
 393     // start all timers
 394     // TODO - not implemented yet
 395     timerStart();
 396
 397     ENABLE_STATE(SMALL_ANGLE);
 398     DISABLE_ARMING_FLAG(PREVENT_ARMING);
 399
 400 #ifdef SOFTSERIAL_LOOPBACK
 401     // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
 402     loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
 403     if (!loopbackPort->vTable) {
 404         loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
 405     }
 406     serialPrint(loopbackPort, "LOOPBACK\r\n");
 407 #endif
 408
 409     // Now that everything has powered up the voltage and cell count be determined.
 410
 411     if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
 412         batteryInit(&masterConfig.batteryConfig);
 413
 414 #ifdef DISPLAY
 415     if (feature(FEATURE_DISPLAY)) {
 416 #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
 417         displayShowFixedPage(PAGE_GPS);
 418 #else
 419         displayResetPageCycling();
 420         displayEnablePageCycling();
 421 #endif
 422     }
 423 #endif
 424
 425 #ifdef CJMCU
 426     LED2_ON;
 427 #endif
 428 }
 429
 430 #ifdef SOFTSERIAL_LOOPBACK
 431 void processLoopback(void) {
 432     if (loopbackPort) {
 433         uint8_t bytesWaiting;
 434         while ((bytesWaiting = serialTotalBytesWaiting(loopbackPort))) {
 435             uint8_t b = serialRead(loopbackPort);
 436             serialWrite(loopbackPort, b);
 437         };
 438     }
 439 }
 440 #else
 441 #define processLoopback()
 442 #endif
 443
 444 int main(void) {
 445     init();
 446
 447     while (1) {
 448         loop();
 449         processLoopback();
 450     }
 451 }
 452
 453 void HardFault_Handler(void)
 454 {
 455     // fall out of the sky
 456     writeAllMotors(masterConfig.escAndServoConfig.mincommand);
 457     while (1);
 458 }