mirror of https://github.com/ArduPilot/ardupilot
AP_BoardConfig: startup core peripherals for PX4
This commit is contained in:
parent
84f93439ca
commit
7e803df221
|
@ -100,6 +100,7 @@ private:
|
||||||
void px4_setup_sbus(void);
|
void px4_setup_sbus(void);
|
||||||
void px4_setup_canbus(void);
|
void px4_setup_canbus(void);
|
||||||
void px4_setup_drivers(void);
|
void px4_setup_drivers(void);
|
||||||
|
void px4_setup_peripherals(void);
|
||||||
void px4_sensor_error(const char *reason);
|
void px4_sensor_error(const char *reason);
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||||
|
|
|
@ -51,6 +51,8 @@ extern "C" {
|
||||||
int pwm_input_main(int, char **);
|
int pwm_input_main(int, char **);
|
||||||
int uavcan_main(int, char **);
|
int uavcan_main(int, char **);
|
||||||
int fmu_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);
|
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
|
fail startup of a required sensor
|
||||||
*/
|
*/
|
||||||
|
@ -762,6 +801,7 @@ void AP_BoardConfig::px4_sensor_error(const char *reason)
|
||||||
*/
|
*/
|
||||||
void AP_BoardConfig::px4_setup()
|
void AP_BoardConfig::px4_setup()
|
||||||
{
|
{
|
||||||
|
px4_setup_peripherals();
|
||||||
px4_setup_pwm();
|
px4_setup_pwm();
|
||||||
px4_setup_safety();
|
px4_setup_safety();
|
||||||
px4_setup_uart();
|
px4_setup_uart();
|
||||||
|
|
Loading…
Reference in New Issue