msp: expose number of vtx power levels, bands and channels
[inav.git] / src / main / fc / fc_init.c
blobbc84d380a37079682e90b4a4d31add96d0ef8996
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 <string.h>
22 #include "platform.h"
24 #include "blackbox/blackbox.h"
25 #include "blackbox/blackbox_io.h"
27 #include "build/assert.h"
28 #include "build/atomic.h"
29 #include "build/build_config.h"
30 #include "build/debug.h"
32 #include "common/axis.h"
33 #include "common/color.h"
34 #include "common/log.h"
35 #include "common/maths.h"
36 #include "common/memory.h"
37 #include "common/printf.h"
38 #include "programming/global_variables.h"
40 #include "config/config_eeprom.h"
41 #include "config/feature.h"
42 #include "config/parameter_group.h"
43 #include "config/parameter_group_ids.h"
45 #include "cms/cms.h"
47 #include "drivers/1-wire.h"
48 #include "drivers/accgyro/accgyro.h"
49 #include "drivers/adc.h"
50 #include "drivers/compass/compass.h"
51 #include "drivers/bus.h"
52 #include "drivers/dma.h"
53 #include "drivers/exti.h"
54 #include "drivers/io.h"
55 #include "drivers/flash.h"
56 #include "drivers/gimbal_common.h"
57 #include "drivers/headtracker_common.h"
58 #include "drivers/light_led.h"
59 #include "drivers/nvic.h"
60 #include "drivers/osd.h"
61 #include "drivers/persistent.h"
62 #include "drivers/pwm_esc_detect.h"
63 #include "drivers/pwm_mapping.h"
64 #include "drivers/pwm_output.h"
65 #include "drivers/sensor.h"
66 #include "drivers/serial.h"
67 #include "drivers/serial_softserial.h"
68 #include "drivers/serial_uart.h"
69 #include "drivers/serial_usb_vcp.h"
70 #include "drivers/sound_beeper.h"
71 #include "drivers/system.h"
72 #include "drivers/time.h"
73 #include "drivers/timer.h"
74 #include "drivers/uart_inverter.h"
75 #include "drivers/io.h"
76 #include "drivers/vtx_common.h"
77 #ifdef USE_USB_MSC
78 #include "drivers/usb_msc.h"
79 #include "msc/emfat_file.h"
80 #endif
81 #include "drivers/sdcard/sdcard.h"
82 #include "drivers/sdio.h"
83 #include "drivers/io_port_expander.h"
85 #include "fc/cli.h"
86 #include "fc/config.h"
87 #include "fc/fc_msp.h"
88 #include "fc/fc_tasks.h"
89 #include "fc/rc_controls.h"
90 #include "fc/runtime_config.h"
91 #include "fc/firmware_update.h"
93 #include "flight/failsafe.h"
94 #include "flight/imu.h"
95 #include "flight/mixer.h"
96 #include "flight/pid.h"
97 #include "flight/power_limits.h"
98 #include "flight/rpm_filter.h"
99 #include "flight/servos.h"
100 #include "flight/ez_tune.h"
102 #include "io/asyncfatfs/asyncfatfs.h"
103 #include "io/beeper.h"
104 #include "io/lights.h"
105 #include "io/dashboard.h"
106 #include "io/displayport_frsky_osd.h"
107 #include "io/displayport_msp.h"
108 #include "io/displayport_max7456.h"
109 #include "io/displayport_msp_osd.h"
110 #include "io/displayport_srxl.h"
111 #include "io/flashfs.h"
112 #include "io/gimbal_serial.h"
113 #include "io/headtracker_msp.h"
114 #include "io/gps.h"
115 #include "io/ledstrip.h"
116 #include "io/osd.h"
117 #include "io/osd_dji_hd.h"
118 #include "io/rcdevice_cam.h"
119 #include "io/serial.h"
120 #include "io/displayport_msp.h"
121 #include "io/smartport_master.h"
122 #include "io/vtx.h"
123 #include "io/vtx_control.h"
124 #include "io/vtx_smartaudio.h"
125 #include "io/vtx_tramp.h"
126 #include "io/vtx_msp.h"
127 #include "io/vtx_ffpv24g.h"
128 #include "io/piniobox.h"
130 #include "msp/msp_serial.h"
132 #include "navigation/navigation.h"
134 #include "rx/rx.h"
135 #include "rx/spektrum.h"
137 #include "sensors/acceleration.h"
138 #include "sensors/barometer.h"
139 #include "sensors/battery.h"
140 #include "sensors/boardalignment.h"
141 #include "sensors/compass.h"
142 #include "sensors/gyro.h"
143 #include "sensors/initialisation.h"
144 #include "sensors/pitotmeter.h"
145 #include "sensors/rangefinder.h"
146 #include "sensors/sensors.h"
147 #include "sensors/esc_sensor.h"
149 #include "scheduler/scheduler.h"
151 #include "telemetry/telemetry.h"
153 #if defined(SITL_BUILD)
154 #include "target/SITL/serial_proxy.h"
155 #endif
157 #ifdef USE_HARDWARE_REVISION_DETECTION
158 #include "hardware_revision.h"
159 #endif
161 #ifdef USE_HARDWARE_PREBOOT_SETUP
162 extern void initialisePreBootHardware(void);
163 #endif
165 extern uint8_t motorControlEnable;
167 typedef enum {
168 SYSTEM_STATE_INITIALISING = 0,
169 SYSTEM_STATE_CONFIG_LOADED = (1 << 0),
170 SYSTEM_STATE_SENSORS_READY = (1 << 1),
171 SYSTEM_STATE_MOTORS_READY = (1 << 2),
172 SYSTEM_STATE_TRANSPONDER_ENABLED = (1 << 3),
173 SYSTEM_STATE_READY = (1 << 7)
174 } systemState_e;
176 uint8_t systemState = SYSTEM_STATE_INITIALISING;
178 void flashLedsAndBeep(void)
180 LED1_ON;
181 LED0_OFF;
182 for (uint8_t i = 0; i < 10; i++) {
183 LED1_TOGGLE;
184 LED0_TOGGLE;
185 delay(25);
186 if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1))))
187 BEEP_ON;
188 delay(25);
189 BEEP_OFF;
191 LED0_OFF;
192 LED1_OFF;
195 void init(void)
197 #if defined(USE_FLASHFS)
198 bool flashDeviceInitialized = false;
199 #endif
201 #ifdef USE_HAL_DRIVER
202 HAL_Init();
203 #endif
205 systemState = SYSTEM_STATE_INITIALISING;
206 printfSupportInit();
208 // Initialize system and CPU clocks to their initial values
209 systemInit();
211 #if !defined(SITL_BUILD)
212 __enable_irq();
213 #endif
215 // initialize IO (needed for all IO operations)
216 IOInitGlobal();
218 #ifdef USE_HARDWARE_REVISION_DETECTION
219 detectHardwareRevision();
220 #endif
222 #ifdef USE_BRUSHED_ESC_AUTODETECT
223 detectBrushedESC();
224 #endif
226 #ifdef CONFIG_IN_EXTERNAL_FLASH
227 // Reset config to defaults. Note: Default flash config must be functional for config in external flash to work.
228 pgResetAll(0);
230 flashDeviceInitialized = flashInit();
231 #endif
233 #if defined(SITL_BUILD)
234 serialProxyInit();
235 #endif
237 initEEPROM();
238 ensureEEPROMContainsValidData();
239 suspendRxSignal();
240 readEEPROM();
241 resumeRxSignal();
243 #ifdef USE_I2C
244 i2cSetSpeed(systemConfig()->i2c_speed);
245 #endif
247 #ifdef USE_HARDWARE_PREBOOT_SETUP
248 initialisePreBootHardware();
249 #endif
251 systemState |= SYSTEM_STATE_CONFIG_LOADED;
253 debugMode = systemConfig()->debug_mode;
255 // Latch active features to be used for feature() in the remainder of init().
256 latchActiveFeatures();
258 ledInit(false);
259 #if !defined(SITL_BUILD)
260 EXTIInit();
261 #endif
263 #if defined(USE_SPEKTRUM_BIND) && defined(USE_SERIALRX_SPEKTRUM)
264 if (rxConfig()->receiverType == RX_TYPE_SERIAL) {
265 switch (rxConfig()->serialrx_provider) {
266 case SERIALRX_SPEKTRUM1024:
267 case SERIALRX_SPEKTRUM2048:
268 // Spektrum satellite binding if enabled on startup.
269 // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
270 // The rest of Spektrum initialization will happen later - via spektrumInit()
271 spektrumBind(rxConfigMutable());
272 break;
275 #endif
277 #ifdef USE_VCP
278 // Early initialize USB hardware
279 usbVcpInitHardware();
280 #endif
282 timerInit(); // timer must be initialized before any channel is allocated
284 serialInit(feature(FEATURE_SOFTSERIAL));
286 // Initialize MSP serial ports here so LOG can share a port with MSP.
287 // XXX: Don't call mspFcInit() yet, since it initializes the boxes and needs
288 // to run after the sensors have been detected.
289 mspSerialInit();
291 #if defined(USE_DJI_HD_OSD)
292 // DJI OSD uses a special flavour of MSP (subset of Betaflight 4.1.1 MSP) - process as part of serial task
293 djiOsdSerialInit();
294 #endif
296 #if defined(USE_SMARTPORT_MASTER)
297 smartportMasterInit();
298 #endif
300 #if defined(USE_LOG)
301 // LOG might use serial output, so we only can init it after serial port is ready
302 // From this point on we can use LOG_*() to produce real-time debugging information
303 logInit();
304 #endif
306 #ifdef USE_PROGRAMMING_FRAMEWORK
307 gvInit();
308 #endif
310 // Initialize servo and motor mixers
311 // This needs to be called early to set up platform type correctly and count required motors & servos
312 mixerConfigInit();
314 // Some sanity checking
315 if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) {
316 featureClear(FEATURE_REVERSIBLE_MOTORS);
318 if (!STATE(ALTITUDE_CONTROL)) {
319 featureClear(FEATURE_AIRMODE);
321 #if !defined(SITL_BUILD)
322 // Initialize motor and servo outpus
323 if (pwmMotorAndServoInit()) {
324 DISABLE_ARMING_FLAG(ARMING_DISABLED_PWM_OUTPUT_ERROR);
326 else {
327 ENABLE_ARMING_FLAG(ARMING_DISABLED_PWM_OUTPUT_ERROR);
329 #else
330 DISABLE_ARMING_FLAG(ARMING_DISABLED_PWM_OUTPUT_ERROR);
331 #endif
332 systemState |= SYSTEM_STATE_MOTORS_READY;
334 #ifdef USE_ESC_SENSOR
335 // DSHOT supports a dedicated wire ESC telemetry. Kick off the ESC-sensor receiver initialization
336 // We may, however, do listen_only, so need to init this anyway
337 // Initialize escSensor after having done it with outputs
338 escSensorInitialize();
339 #endif
341 #ifdef BEEPER
342 beeperDevConfig_t beeperDevConfig = {
343 .ioTag = IO_TAG(BEEPER),
344 #ifdef BEEPER_INVERTED
345 .isOD = false,
346 .isInverted = true
347 #else
348 .isOD = true,
349 .isInverted = false
350 #endif
353 beeperInit(&beeperDevConfig);
354 #endif
355 #ifdef USE_LIGHTS
356 lightsInit();
357 #endif
359 #ifdef USE_UART_INVERTER
360 uartInverterInit();
361 #endif
363 // Initialize buses
364 busInit();
366 #ifdef CONFIG_IN_EXTERNAL_FLASH
367 // busInit re-configures the SPI pins. Init flash again so it is ready to write settings
368 flashDeviceInitialized = flashInit();
369 #endif
371 #ifdef USE_HARDWARE_REVISION_DETECTION
372 updateHardwareRevision();
373 #endif
375 #if defined(USE_SDCARD_SDIO) && defined(STM32H7)
376 sdioPinConfigure();
377 SDIO_GPIO_Init();
378 #endif
380 #ifdef USE_USB_MSC
381 /* MSC mode will start after init, but will not allow scheduler to run,
382 * so there is no bottleneck in reading and writing data
384 mscInit();
385 #if defined(USE_FLASHFS)
386 // If the blackbox device is onboard flash, then initialize and scan
387 // it to identify the log files *before* starting the USB device to
388 // prevent timeouts of the mass storage device.
389 if (blackboxConfig()->device == BLACKBOX_DEVICE_FLASH) {
390 // Must initialise the device to read _anything_
391 if (!flashDeviceInitialized) {
392 flashDeviceInitialized = flashInit();
394 emfat_init_files();
396 #endif
398 if (mscCheckBoot() || mscCheckButton()) {
399 if (mscStart() == 0) {
400 mscWaitForButton();
401 } else {
402 NVIC_SystemReset();
405 #endif
407 #ifdef USE_I2C
408 #ifdef USE_I2C_DEVICE_1
409 #ifdef I2C_DEVICE_1_SHARES_UART3
410 if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
411 i2cInit(I2CDEV_1);
413 #else
414 i2cInit(I2CDEV_1);
415 #endif
416 #endif
418 #ifdef USE_I2C_DEVICE_2
419 #ifdef I2C_DEVICE_2_SHARES_UART3
420 if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
421 i2cInit(I2CDEV_2);
423 #else
424 i2cInit(I2CDEV_2);
425 #endif
426 #endif
428 #ifdef USE_I2C_DEVICE_3
429 i2cInit(I2CDEV_3);
430 #endif
432 #ifdef USE_I2C_DEVICE_4
433 i2cInit(I2CDEV_4);
434 #endif
436 #ifdef USE_I2C_DEVICE_EMULATED
437 #ifdef I2C_DEVICE_EMULATED_SHARES_UART3
438 if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
439 i2cInit(I2CDEV_EMULATED);
441 #else
442 i2cInit(I2CDEV_EMULATED);
443 #endif
444 #endif
445 #endif
447 #ifdef USE_ADC
448 drv_adc_config_t adc_params;
449 memset(&adc_params, 0, sizeof(adc_params));
451 // Allocate and initialize ADC channels if features are configured - can't rely on sensor detection here, it's done later
452 if (feature(FEATURE_VBAT)) {
453 adc_params.adcFunctionChannel[ADC_BATTERY] = adcChannelConfig()->adcFunctionChannel[ADC_BATTERY];
456 if (feature(FEATURE_RSSI_ADC)) {
457 adc_params.adcFunctionChannel[ADC_RSSI] = adcChannelConfig()->adcFunctionChannel[ADC_RSSI];
460 if (feature(FEATURE_CURRENT_METER) && batteryMetersConfig()->current.type == CURRENT_SENSOR_ADC) {
461 adc_params.adcFunctionChannel[ADC_CURRENT] = adcChannelConfig()->adcFunctionChannel[ADC_CURRENT];
464 #if defined(USE_PITOT) && defined(USE_ADC) && defined(USE_PITOT_ADC)
465 if (pitotmeterConfig()->pitot_hardware == PITOT_ADC || pitotmeterConfig()->pitot_hardware == PITOT_AUTODETECT) {
466 adc_params.adcFunctionChannel[ADC_AIRSPEED] = adcChannelConfig()->adcFunctionChannel[ADC_AIRSPEED];
468 #endif
470 adcInit(&adc_params);
471 #endif
473 #ifdef USE_PINIO
474 pinioInit();
475 #endif
477 #ifdef USE_PINIOBOX
478 pinioBoxInit();
479 #endif
481 #if defined(USE_GPS) || defined(USE_MAG)
482 delay(500);
484 /* Extra 500ms delay prior to initialising hardware if board is cold-booting */
485 if (!isMPUSoftReset()) {
486 LED1_ON;
487 LED0_OFF;
489 for (int i = 0; i < 5; i++) {
490 LED1_TOGGLE;
491 LED0_TOGGLE;
492 delay(100);
495 LED0_OFF;
496 LED1_OFF;
498 #endif
500 initBoardAlignment();
502 #ifdef USE_CMS
503 cmsInit();
504 #endif
506 #ifdef USE_DASHBOARD
507 if (feature(FEATURE_DASHBOARD)) {
508 dashboardInit();
510 #endif
512 #ifdef USE_GPS
513 if (feature(FEATURE_GPS)) {
514 gpsPreInit();
516 #endif
518 // 1-Wire IF chip
519 #ifdef USE_1WIRE
520 owInit();
521 #endif
523 #ifdef USE_EZ_TUNE
524 ezTuneUpdate();
525 #endif
527 #ifndef USE_GEOZONE
528 featureClear(FEATURE_GEOZONE);
529 #endif
531 if (!sensorsAutodetect()) {
532 // if gyro was not detected due to whatever reason, we give up now.
533 failureMode(FAILURE_MISSING_ACC);
536 systemState |= SYSTEM_STATE_SENSORS_READY;
538 flashLedsAndBeep();
540 pidInitFilters();
542 imuInit();
544 // Sensors have now been detected, mspFcInit() can now be called
545 // to set the boxes up
546 mspFcInit();
548 cliInit(serialConfig());
550 failsafeInit();
552 rxInit();
554 #if defined(USE_OSD)
555 displayPort_t *osdDisplayPort = NULL;
556 #endif
558 #ifdef USE_OSD
559 if (feature(FEATURE_OSD)) {
560 #if defined(USE_FRSKYOSD)
561 if (!osdDisplayPort) {
562 osdDisplayPort = frskyOSDDisplayPortInit(osdConfig()->video_system);
564 #endif
565 #ifdef USE_MSP_OSD
566 if (!osdDisplayPort) {
567 osdDisplayPort = mspOsdDisplayPortInit(osdConfig()->video_system);
569 #endif
570 #if defined(USE_MAX7456)
571 // If there is a max7456 chip for the OSD and we have no
572 // external OSD initialized, use it.
573 if (!osdDisplayPort) {
574 osdDisplayPort = max7456DisplayPortInit(osdConfig()->video_system);
576 #elif defined(USE_OSD_OVER_MSP_DISPLAYPORT) // OSD over MSP; not supported (yet)
577 if (!osdDisplayPort) {
578 osdDisplayPort = displayPortMspInit();
580 #endif
581 // osdInit will register with CMS by itself.
582 osdInit(osdDisplayPort);
584 #endif
586 #if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
587 // Register the srxl Textgen telemetry sensor as a displayport device
588 cmsDisplayPortRegister(displayPortSrxlInit());
589 #endif
591 #ifdef USE_GPS
592 if (feature(FEATURE_GPS)) {
593 gpsInit();
595 #endif
598 navigationInit();
600 #ifdef USE_LED_STRIP
601 ledStripInit();
603 if (feature(FEATURE_LED_STRIP)) {
604 ledStripEnable();
606 #endif
608 #ifdef USE_TELEMETRY
609 if (feature(FEATURE_TELEMETRY)) {
610 telemetryInit();
612 #endif
614 #ifdef USE_BLACKBOX
616 //Do not allow blackbox to be run faster that 1kHz. It can cause UAV to drop dead when digital ESC protocol is used
617 const uint32_t blackboxLooptime = getLooptime() * blackboxConfig()->rate_denom / blackboxConfig()->rate_num;
619 if (blackboxLooptime < 1000) {
620 blackboxConfigMutable()->rate_num = 1;
621 blackboxConfigMutable()->rate_denom = ceil(1000 / getLooptime());
624 // SDCARD and FLASHFS are used only for blackbox
625 // Make sure we only init what's necessary for blackbox
626 switch (blackboxConfig()->device) {
627 #ifdef USE_FLASHFS
628 case BLACKBOX_DEVICE_FLASH:
629 if (!flashDeviceInitialized) {
630 flashDeviceInitialized = flashInit();
632 if (flashDeviceInitialized) {
633 // do not initialize flashfs if no flash was found
634 flashfsInit();
636 break;
637 #endif
639 #ifdef USE_SDCARD
640 case BLACKBOX_DEVICE_SDCARD:
641 sdcardInsertionDetectInit();
642 sdcard_init();
643 afatfs_init();
644 break;
645 #endif
646 default:
647 break;
650 blackboxInit();
651 #endif
653 gyroStartCalibration();
655 #ifdef USE_BARO
656 baroStartCalibration();
657 #endif
659 #ifdef USE_PITOT
660 pitotStartCalibration();
661 #endif
663 #if defined(USE_VTX_CONTROL)
664 vtxControlInit();
665 vtxCommonInit();
666 vtxInit();
668 #ifdef USE_VTX_SMARTAUDIO
669 vtxSmartAudioInit();
670 #endif
672 #ifdef USE_VTX_TRAMP
673 vtxTrampInit();
674 #endif
676 #ifdef USE_VTX_FFPV
677 vtxFuriousFPVInit();
678 #endif
680 #ifdef USE_VTX_MSP
681 vtxMspInit();
682 #endif
684 #endif // USE_VTX_CONTROL
686 // Now that everything has powered up the voltage and cell count be determined.
687 if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
688 batteryInit();
690 #ifdef USE_RCDEVICE
691 rcdeviceInit();
692 #endif // USE_RCDEVICE
694 #ifdef USE_DSHOT
695 initDShotCommands();
696 #endif
698 #ifdef USE_SERIAL_GIMBAL
699 gimbalCommonInit();
700 // Needs to be called before gimbalSerialHeadTrackerInit
701 gimbalSerialInit();
702 #endif
704 #ifdef USE_HEADTRACKER
705 headTrackerCommonInit();
706 #ifdef USE_HEADTRACKER_SERIAL
707 // Needs to be called after gimbalSerialInit
708 gimbalSerialHeadTrackerInit();
709 #endif
710 #ifdef USE_HEADTRACKER_MSP
711 mspHeadTrackerInit();
712 #endif
713 #endif
715 // Latch active features AGAIN since some may be modified by init().
716 latchActiveFeatures();
717 motorControlEnable = true;
719 fcTasksInit();
721 #ifdef USE_OSD
722 if (feature(FEATURE_OSD) && (osdDisplayPort != NULL)) {
723 setTaskEnabled(TASK_OSD, feature(FEATURE_OSD));
725 #endif
727 #ifdef USE_RPM_FILTER
728 disableRpmFilters();
729 if (STATE(ESC_SENSOR_ENABLED) && (rpmFilterConfig()->gyro_filter_enabled || rpmFilterConfig()->dterm_filter_enabled)) {
730 rpmFiltersInit();
731 setTaskEnabled(TASK_RPM_FILTER, true);
733 #endif
735 #ifdef USE_I2C_IO_EXPANDER
736 ioPortExpanderInit();
737 #endif
739 #ifdef USE_POWER_LIMITS
740 powerLimiterInit();
741 #endif
743 #if !defined(SITL_BUILD)
744 // Considering that the persistent reset reason is only used during init
745 persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE);
746 #endif
748 systemState |= SYSTEM_STATE_READY;