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