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