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