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