main.c 13.7 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
/*
 * This file is part of Cleanflight.
 *
 * Cleanflight is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Cleanflight is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#include "platform.h"

#include "common/axis.h"
26
#include "common/color.h"
Petr Ledvina's avatar
Petr Ledvina committed
27
#include "common/atomic.h"
28
29
#include "common/maths.h"

Petr Ledvina's avatar
Petr Ledvina committed
30
#include "drivers/nvic.h"
31

32
#include "drivers/sensor.h"
33
34
35
36
37
38
39
40
41
#include "drivers/system.h"
#include "drivers/gpio.h"
#include "drivers/light_led.h"
#include "drivers/sound_beeper.h"
#include "drivers/timer.h"
#include "drivers/serial.h"
#include "drivers/serial_softserial.h"
#include "drivers/serial_uart.h"
#include "drivers/accgyro.h"
42
#include "drivers/compass.h"
43
44
45
#include "drivers/pwm_mapping.h"
#include "drivers/pwm_rx.h"
#include "drivers/adc.h"
46
47
#include "drivers/bus_i2c.h"
#include "drivers/bus_spi.h"
48
#include "drivers/inverter.h"
49
#include "drivers/flash_m25p16.h"
Dominic Clifton's avatar
Dominic Clifton committed
50
#include "drivers/sonar_hcsr04.h"
51

52
#include "rx/rx.h"
53
54

#include "io/serial.h"
55
#include "io/flashfs.h"
56
57
58
59
#include "io/gps.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "io/gimbal.h"
60
#include "io/ledstrip.h"
61
#include "io/display.h"
62

63
64
65
66
67
68
69
70
#include "sensors/sensors.h"
#include "sensors/sonar.h"
#include "sensors/barometer.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
71
#include "sensors/initialisation.h"
72
73
74
75

#include "telemetry/telemetry.h"
#include "blackbox/blackbox.h"

76
#include "flight/pid.h"
77
78
79
80
81
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/failsafe.h"
#include "flight/navigation.h"

82
83
84
85
86
#include "config/runtime_config.h"
#include "config/config.h"
#include "config/config_profile.h"
#include "config/config_master.h"

87
88
#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"
89
90
#endif

91
#include "build_config.h"
Dominic Clifton's avatar
Dominic Clifton committed
92
#include "debug.h"
93
94

extern uint32_t previousTime;
95
extern uint8_t motorControlEnable;
96
97
98
99
100

#ifdef SOFTSERIAL_LOOPBACK
serialPort_t *loopbackPort;
#endif

Dominic Clifton's avatar
Dominic Clifton committed
101
void printfSupportInit(void);
102
void timerInit(void);
Dominic Clifton's avatar
Dominic Clifton committed
103
void telemetryInit(void);
104
void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled);
105
106
void mspInit(serialConfig_t *serialConfig);
void cliInit(serialConfig_t *serialConfig);
107
void failsafeInit(rxConfig_t *intialRxConfig);
108
pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init);
109
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMixers);
110
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
111
void rxInit(rxConfig_t *rxConfig);
112
113
void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
Dominic Clifton's avatar
Dominic Clifton committed
114
void imuInit(void);
115
void displayInit(rxConfig_t *intialRxConfig);
116
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse);
117
void loop(void);
118
void spektrumBind(rxConfig_t *rxConfig);
Dominic Clifton's avatar
Dominic Clifton committed
119
120
const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig);
void sonarInit(const sonarHardware_t *sonarHardware);
121
122
123
124
125
126
127
128
129
130

#ifdef STM32F303xC
// from system_stm32f30x.c
void SetSysClock(void);
#endif
#ifdef STM32F10X
// from system_stm32f10x.c
void SetSysClock(bool overclock);
#endif

131
132
133
134
135
136
137
138
139
140
typedef enum {
    SYSTEM_STATE_INITIALISING   = 0,
    SYSTEM_STATE_CONFIG_LOADED  = (1 << 0),
    SYSTEM_STATE_SENSORS_READY  = (1 << 1),
    SYSTEM_STATE_MOTORS_READY   = (1 << 2),
    SYSTEM_STATE_READY          = (1 << 7)
} systemState_e;

static uint8_t systemState = SYSTEM_STATE_INITIALISING;

141
142
143
144
145
void init(void)
{
    uint8_t i;
    drv_pwm_config_t pwm_params;

Dominic Clifton's avatar
Dominic Clifton committed
146
    printfSupportInit();
147
148
149
150
151
152

    initEEPROM();

    ensureEEPROMContainsValidData();
    readEEPROM();

153
154
    systemState |= SYSTEM_STATE_CONFIG_LOADED;

155
156
157
158
159
160
161
162
163
164
165
166
167
168
#ifdef STM32F303
    // start fpu
    SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
#endif

#ifdef STM32F303xC
    SetSysClock();
#endif
#ifdef STM32F10X
    // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
    // Configure the Flash Latency cycles and enable prefetch buffer
    SetSysClock(masterConfig.emf_avoidance);
#endif

169
#ifdef USE_HARDWARE_REVISION_DETECTION
170
171
172
173
174
    detectHardwareRevision();
#endif

    systemInit();

ProDrone's avatar
ProDrone committed
175
    // Latch active features to be used for feature() in the remainder of init().
176
177
    latchActiveFeatures();

178
179
    ledInit();

Petr Ledvina's avatar
Petr Ledvina committed
180
#ifdef SPEKTRUM_BIND
Michael Jakob's avatar
Michael Jakob committed
181
    if (feature(FEATURE_RX_SERIAL)) {
182
183
184
185
186
187
188
189
190
191
        switch (masterConfig.rxConfig.serialrx_provider) {
            case SERIALRX_SPEKTRUM1024:
            case SERIALRX_SPEKTRUM2048:
                // Spektrum satellite binding if enabled on startup.
                // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
                // The rest of Spektrum initialization will happen later - via spektrumInit()
                spektrumBind(&masterConfig.rxConfig);
                break;
        }
    }
192
#endif
193

194
195
    delay(100);

Petr Ledvina's avatar
Petr Ledvina committed
196
197
    timerInit();  // timer must be initialized before any channel is allocated

198
    serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL));
199

200
201
    mixerInit(masterConfig.mixerMode, masterConfig.customMixer);

202
    memset(&pwm_params, 0, sizeof(pwm_params));
Dominic Clifton's avatar
Dominic Clifton committed
203
204
205
206
207
208
209

#ifdef SONAR
    const sonarHardware_t *sonarHardware = NULL;

    if (feature(FEATURE_SONAR)) {
        sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig);
        sonarGPIOConfig_t sonarGPIOConfig = {
210
211
212
            .gpio = SONAR_GPIO,
            .triggerPin = sonarHardware->echo_pin,
            .echoPin = sonarHardware->trigger_pin,
Dominic Clifton's avatar
Dominic Clifton committed
213
214
215
216
217
        };
        pwm_params.sonarGPIOConfig = &sonarGPIOConfig;
    }
#endif

218
219
220
221
222
223
224
    // when using airplane/wing mixer, servo/motor outputs are remapped
    if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING)
        pwm_params.airplane = true;
    else
        pwm_params.airplane = false;
#if defined(USE_USART2) && defined(STM32F10X)
    pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
225
226
227
#endif
#ifdef STM32F303xC
    pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3);
228
229
230
231
232
233
234
235
236
237
#endif
    pwm_params.useVbat = feature(FEATURE_VBAT);
    pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
    pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
    pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
    pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER)
        && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
    pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
    pwm_params.usePPM = feature(FEATURE_RX_PPM);
    pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
238
239
240
#ifdef SONAR
    pwm_params.useSonar = feature(FEATURE_SONAR);
#endif
241
242

#ifdef USE_SERVOS
243
244
    pwm_params.useServos = isMixerUsingServos();
    pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX;
245
    pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
246
    pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
247
248
249
250
#endif

    pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
    pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
251
    pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
252
253
254
255
256
257
258
259
260
261
262
    if (feature(FEATURE_3D))
        pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
    if (pwm_params.motorPwmRate > 500)
        pwm_params.idlePulse = 0; // brushed motors

    pwmRxInit(masterConfig.inputFilteringMode);

    pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);

    mixerUsePWMOutputConfiguration(pwmOutputConfiguration);

263
264
265
    if (!feature(FEATURE_ONESHOT125))
        motorControlEnable = true;

266
267
    systemState |= SYSTEM_STATE_MOTORS_READY;

268
269
#ifdef BEEPER
    beeperConfig_t beeperConfig = {
270
        .gpioPeripheral = BEEP_PERIPHERAL,
271
272
        .gpioPin = BEEP_PIN,
        .gpioPort = BEEP_GPIO,
273
274
275
276
277
#ifdef BEEPER_INVERTED
        .gpioMode = Mode_Out_PP,
        .isInverted = true
#else
        .gpioMode = Mode_Out_OD,
278
        .isInverted = false
279
#endif
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
    };
#ifdef NAZE
    if (hardwareRevision >= NAZE32_REV5) {
        // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
        beeperConfig.gpioMode = Mode_Out_PP;
        beeperConfig.isInverted = true;
    }
#endif

    beeperInit(&beeperConfig);
#endif

#ifdef INVERTER
    initInverter();
#endif


297
298
299
300
301
#ifdef USE_SPI
    spiInit(SPI1);
    spiInit(SPI2);
#endif

302
#ifdef USE_HARDWARE_REVISION_DETECTION
303
304
305
    updateHardwareRevision();
#endif

306
307
308
309
310
311
312
313
#if defined(NAZE)
    if (hardwareRevision == NAZE32_SP) {
        serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
    } else  {
        serialRemovePort(SERIAL_PORT_USART3);
    }
#endif

314
#ifdef USE_I2C
315
#if defined(NAZE)
316
317
    if (hardwareRevision != NAZE32_SP) {
        i2cInit(I2C_DEVICE);
318
319
320
321
    } else {
        if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
            i2cInit(I2C_DEVICE);
        }
322
    }
323
324
325
326
#elif defined(CC3D)
    if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
        i2cInit(I2C_DEVICE);
    }
327
328
329
330
#else
    i2cInit(I2C_DEVICE);
#endif
#endif
331

332
#ifdef USE_ADC
333
334
    drv_adc_config_t adc_params;

335
    adc_params.enableVBat = feature(FEATURE_VBAT);
336
337
    adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
    adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
338
339
340
341
342
343
344
345
    adc_params.enableExternal1 = false;
#ifdef OLIMEXINO
    adc_params.enableExternal1 = true;
#endif
#ifdef NAZE
    // optional ADC5 input on rev.5 hardware
    adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5);
#endif
346
347

    adcInit(&adc_params);
348
349
#endif

350
351
352

    initBoardAlignment(&masterConfig.boardAlignment);

353
#ifdef DISPLAY
354
    if (feature(FEATURE_DISPLAY)) {
355
        displayInit(&masterConfig.rxConfig);
356
    }
357
#endif
Dominic Clifton's avatar
Dominic Clifton committed
358

359
360
    if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination)) {
        // if gyro was not detected due to whatever reason, we give up now.
361
        failureMode(3);
362
363
364
    }

    systemState |= SYSTEM_STATE_SENSORS_READY;
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383

    LED1_ON;
    LED0_OFF;
    for (i = 0; i < 10; i++) {
        LED1_TOGGLE;
        LED0_TOGGLE;
        delay(25);
        BEEP_ON;
        delay(25);
        BEEP_OFF;
    }
    LED0_OFF;
    LED1_OFF;

#ifdef MAG
    if (sensors(SENSOR_MAG))
        compassInit();
#endif

384
385
    imuInit();

386
    mspInit(&masterConfig.serialConfig);
387
388

#ifdef USE_CLI
389
    cliInit(&masterConfig.serialConfig);
390
#endif
391

392
    failsafeInit(&masterConfig.rxConfig);
393

394
    rxInit(&masterConfig.rxConfig);
395
396
397
398
399
400
401
402

#ifdef GPS
    if (feature(FEATURE_GPS)) {
        gpsInit(
            &masterConfig.serialConfig,
            &masterConfig.gpsConfig
        );
        navigationInit(
403
404
            &currentProfile->gpsProfile,
            &currentProfile->pidProfile
405
406
407
408
409
410
        );
    }
#endif

#ifdef SONAR
    if (feature(FEATURE_SONAR)) {
Dominic Clifton's avatar
Dominic Clifton committed
411
        sonarInit(sonarHardware);
412
413
414
415
    }
#endif

#ifdef LED_STRIP
416
    ledStripInit(masterConfig.ledConfigs, masterConfig.colors);
417

418
    if (feature(FEATURE_LED_STRIP)) {
419
        ledStripEnable();
420
421
422
423
    }
#endif

#ifdef TELEMETRY
424
    if (feature(FEATURE_TELEMETRY)) {
Dominic Clifton's avatar
Dominic Clifton committed
425
        telemetryInit();
426
    }
427
428
#endif

429
#ifdef USE_FLASHFS
430
431
#ifdef NAZE
    if (hardwareRevision == NAZE32_REV5) {
432
        m25p16_init();
433
434
    }
#endif
435
#if defined(SPRACINGF3) || defined(CC3D)
436
437
    m25p16_init();
#endif
438
    flashfsInit();
439
440
#endif

Nicholas Sherlock's avatar
Nicholas Sherlock committed
441
442
443
444
#ifdef BLACKBOX
    initBlackbox();
#endif

445
446
    previousTime = micros();

447
    if (masterConfig.mixerMode == MIXER_GIMBAL) {
448
449
450
451
452
453
454
        accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
    }
    gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#ifdef BARO
    baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif

Petr Ledvina's avatar
Petr Ledvina committed
455
456
457
458
    // start all timers
    // TODO - not implemented yet
    timerStart();

459
460
    ENABLE_STATE(SMALL_ANGLE);
    DISABLE_ARMING_FLAG(PREVENT_ARMING);
461
462
463
464
465
466
467
468
469
470
471
472

#ifdef SOFTSERIAL_LOOPBACK
    // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
    loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
    if (!loopbackPort->vTable) {
        loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
    }
    serialPrint(loopbackPort, "LOOPBACK\r\n");
#endif

    // Now that everything has powered up the voltage and cell count be determined.

473
    if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
474
        batteryInit(&masterConfig.batteryConfig);
475
476
477

#ifdef DISPLAY
    if (feature(FEATURE_DISPLAY)) {
478
479
480
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
        displayShowFixedPage(PAGE_GPS);
#else
481
        displayResetPageCycling();
482
        displayEnablePageCycling();
483
#endif
484
485
    }
#endif
486
487
488
489

#ifdef CJMCU
    LED2_ON;
#endif
490

ProDrone's avatar
ProDrone committed
491
    // Latch active features AGAIN since some may be modified by init().
492
    latchActiveFeatures();
493
    motorControlEnable = true;
494

495
    systemState |= SYSTEM_STATE_READY;
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
}

#ifdef SOFTSERIAL_LOOPBACK
void processLoopback(void) {
    if (loopbackPort) {
        uint8_t bytesWaiting;
        while ((bytesWaiting = serialTotalBytesWaiting(loopbackPort))) {
            uint8_t b = serialRead(loopbackPort);
            serialWrite(loopbackPort, b);
        };
    }
}
#else
#define processLoopback()
#endif

int main(void) {
    init();

    while (1) {
        loop();
        processLoopback();
    }
}

void HardFault_Handler(void)
{
    // fall out of the sky
524
525
526
527
    uint8_t requiredState = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY;
    if ((systemState & requiredState) == requiredState) {
        stopMotors();
    }
528
529
    while (1);
}