diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.h b/libraries/AP_BoardConfig/AP_BoardConfig.h index 5b4530ceb5..936cdd169e 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.h +++ b/libraries/AP_BoardConfig/AP_BoardConfig.h @@ -100,6 +100,7 @@ private: void px4_setup_sbus(void); void px4_setup_canbus(void); void px4_setup_drivers(void); + void px4_setup_peripherals(void); void px4_sensor_error(const char *reason); #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 diff --git a/libraries/AP_BoardConfig/px4_drivers.cpp b/libraries/AP_BoardConfig/px4_drivers.cpp index 5605955d07..4f19ac9bc4 100644 --- a/libraries/AP_BoardConfig/px4_drivers.cpp +++ b/libraries/AP_BoardConfig/px4_drivers.cpp @@ -51,6 +51,8 @@ extern "C" { int pwm_input_main(int, char **); int uavcan_main(int, char **); int fmu_main(int, char **); + int px4io_main(int, char **); + int adc_main(int, char **); }; @@ -739,6 +741,43 @@ void AP_BoardConfig::px4_setup_drivers(void) hal.scheduler->delay(1000); } +/* + setup required peripherals like adc, rcinput and rcoutput + */ +void AP_BoardConfig::px4_setup_peripherals(void) +{ + // always start adc + if (px4_start_driver(adc_main, "adc", "start")) { + hal.analogin->init(); + printf("ADC started OK\n"); + } else { + px4_sensor_error("no ADC found"); + } + +#ifndef CONFIG_ARCH_BOARD_PX4FMU_V4 + if (px4_start_driver(px4io_main, "px4io", "start norc")) { + printf("px4io started OK\n"); + } else { + px4_sensor_error("px4io start failed found"); + } +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + const char *fmu_mode = "mode_serial"; +#else + const char *fmu_mode = "mode_pwm4"; +#endif + if (px4_start_driver(fmu_main, "fmu", fmu_mode)) { + printf("fmu %s started OK\n", fmu_mode); + } else { + px4_sensor_error("fmu start failed"); + } + + hal.gpio->init(); + hal.rcin->init(); + hal.rcout->init(); +} + /* fail startup of a required sensor */ @@ -762,6 +801,7 @@ void AP_BoardConfig::px4_sensor_error(const char *reason) */ void AP_BoardConfig::px4_setup() { + px4_setup_peripherals(); px4_setup_pwm(); px4_setup_safety(); px4_setup_uart();