mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-22 15:53:56 -04:00
AP_BoardConfig: implement PX4 driver start in C++
this brings driver startup into AP_BoardConfig, so we can start to make driver startup depend on parameters
This commit is contained in:
Andrew Tridgell
parent
4643d6f314
commit
36e0c7229a
@ -136,131 +136,9 @@ extern "C" int uavcan_main(int argc, const char *argv[]);
|
||||
void AP_BoardConfig::init()
|
||||
{
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
||||
/* configure the FMU driver for the right number of PWMs */
|
||||
static const struct {
|
||||
uint8_t mode_parm;
|
||||
uint8_t mode_value;
|
||||
uint8_t num_gpios;
|
||||
} mode_table[] = {
|
||||
/* table mapping BRD_PWM_COUNT to ioctl arguments */
|
||||
{ 0, PWM_SERVO_MODE_NONE, 6 },
|
||||
{ 2, PWM_SERVO_MODE_2PWM, 4 },
|
||||
{ 4, PWM_SERVO_MODE_4PWM, 2 },
|
||||
{ 6, PWM_SERVO_MODE_6PWM, 0 },
|
||||
{ 7, PWM_SERVO_MODE_3PWM1CAP, 2 },
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
||||
{ 8, PWM_SERVO_MODE_12PWM, 0 },
|
||||
px4_setup();
|
||||
#endif
|
||||
};
|
||||
uint8_t mode_parm = (uint8_t)px4.pwm_count.get();
|
||||
uint8_t i;
|
||||
for (i=0; i<ARRAY_SIZE(mode_table); i++) {
|
||||
if (mode_table[i].mode_parm == mode_parm) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (i == ARRAY_SIZE(mode_table)) {
|
||||
hal.console->printf("RCOutput: invalid BRD_PWM_COUNT %u\n", mode_parm);
|
||||
} else {
|
||||
int fd = open("/dev/px4fmu", 0);
|
||||
if (fd == -1) {
|
||||
AP_HAL::panic("Unable to open /dev/px4fmu");
|
||||
}
|
||||
if (ioctl(fd, PWM_SERVO_SET_MODE, mode_table[i].mode_value) != 0) {
|
||||
hal.console->printf("RCOutput: unable to setup AUX PWM with BRD_PWM_COUNT %u\n", mode_parm);
|
||||
}
|
||||
close(fd);
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||
if (mode_table[i].num_gpios < 2) {
|
||||
// reduce change of config mistake where relay and PWM interfere
|
||||
AP_Param::set_default_by_name("RELAY_PIN", -1);
|
||||
AP_Param::set_default_by_name("RELAY_PIN2", -1);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||
// setup channels to ignore the armed state
|
||||
int px4io_fd = open("/dev/px4io", 0);
|
||||
if (px4io_fd != -1) {
|
||||
if (ioctl(px4io_fd, PWM_SERVO_IGNORE_SAFETY, (uint16_t)(0x0000FFFF & px4.ignore_safety_channels)) != 0) {
|
||||
hal.console->printf("IGNORE_SAFETY failed\n");
|
||||
}
|
||||
close(px4io_fd);
|
||||
}
|
||||
|
||||
hal.uartC->set_flow_control((AP_HAL::UARTDriver::flow_control)px4.ser1_rtscts.get());
|
||||
if (hal.uartD != NULL) {
|
||||
hal.uartD->set_flow_control((AP_HAL::UARTDriver::flow_control)px4.ser2_rtscts.get());
|
||||
}
|
||||
#endif
|
||||
|
||||
if (px4.safety_enable.get() == 0) {
|
||||
hal.rcout->force_safety_off();
|
||||
}
|
||||
#endif
|
||||
|
||||
if (px4.sbus_out_rate.get() >= 1) {
|
||||
static const struct {
|
||||
uint8_t value;
|
||||
uint16_t rate;
|
||||
} rates[] = {
|
||||
{ 1, 50 },
|
||||
{ 2, 75 },
|
||||
{ 3, 100 },
|
||||
{ 4, 150 },
|
||||
{ 5, 200 },
|
||||
{ 6, 250 },
|
||||
{ 7, 300 }
|
||||
};
|
||||
uint16_t rate = 300;
|
||||
for (i=0; i<ARRAY_SIZE(rates); i++) {
|
||||
if (rates[i].value == px4.sbus_out_rate) {
|
||||
rate = rates[i].rate;
|
||||
}
|
||||
}
|
||||
if (!hal.rcout->enable_sbus_out(rate)) {
|
||||
hal.console->printf("Failed to enable SBUS out\n");
|
||||
}
|
||||
}
|
||||
|
||||
#if !defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
|
||||
if (px4.can_enable >= 1) {
|
||||
const char *args[] = { "uavcan", "start", NULL };
|
||||
int ret = uavcan_main(3, args);
|
||||
if (ret != 0) {
|
||||
hal.console->printf("UAVCAN: failed to start\n");
|
||||
} else {
|
||||
hal.console->printf("UAVCAN: started\n");
|
||||
// give some time for CAN bus initialisation
|
||||
hal.scheduler->delay(2000);
|
||||
}
|
||||
}
|
||||
if (px4.can_enable >= 2) {
|
||||
const char *args[] = { "uavcan", "start", "fw", NULL };
|
||||
int ret = uavcan_main(4, args);
|
||||
if (ret != 0) {
|
||||
hal.console->printf("UAVCAN: failed to start servers\n");
|
||||
} else {
|
||||
uint32_t start_wait_ms = AP_HAL::millis();
|
||||
int fd = open(UAVCAN_NODE_FILE, 0);
|
||||
if (fd == -1) {
|
||||
AP_HAL::panic("Configuration invalid - unable to open " UAVCAN_NODE_FILE);
|
||||
}
|
||||
|
||||
// delay startup, UAVCAN still discovering nodes
|
||||
while (ioctl(fd, UAVCAN_IOCG_NODEID_INPROGRESS,0) == OK &&
|
||||
AP_HAL::millis() - start_wait_ms < 7000) {
|
||||
hal.scheduler->delay(500);
|
||||
}
|
||||
hal.console->printf("UAVCAN: node discovery complete\n");
|
||||
close(fd);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
|
||||
// let the HAL know the target temperature. We pass a pointer as
|
||||
// we want the user to be able to change the parameter without
|
||||
// rebooting
|
||||
|
||||
@ -18,6 +18,8 @@
|
||||
|
||||
#endif
|
||||
|
||||
extern "C" typedef int (*main_fn_t)(int argc, char **);
|
||||
|
||||
class AP_BoardConfig
|
||||
{
|
||||
public:
|
||||
@ -34,6 +36,13 @@ public:
|
||||
private:
|
||||
AP_Int16 vehicleSerialNumber;
|
||||
|
||||
enum px4_board_type {
|
||||
PX4_BOARD_PX4V1,
|
||||
PX4_BOARD_PIXHAWK,
|
||||
PX4_BOARD_PIXHAWK2,
|
||||
PX4_BOARD_PIXRACER
|
||||
};
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
||||
struct {
|
||||
AP_Int8 pwm_count;
|
||||
@ -47,6 +56,7 @@ private:
|
||||
AP_Int8 ser2_rtscts;
|
||||
AP_Int8 sbus_out_rate;
|
||||
#endif
|
||||
enum px4_board_type board_type;
|
||||
} px4;
|
||||
void px4_drivers_start(void);
|
||||
void px4_setup(void);
|
||||
@ -55,6 +65,15 @@ private:
|
||||
void px4_setup_uart(void);
|
||||
void px4_setup_sbus(void);
|
||||
void px4_setup_canbus(void);
|
||||
void px4_setup_drivers(void);
|
||||
void px4_sensor_error(const char *reason);
|
||||
|
||||
bool px4_start_driver(main_fn_t main_function, const char *name, const char *arguments);
|
||||
void px4_start_common_sensors(void);
|
||||
void px4_start_fmuv1_sensors(void);
|
||||
void px4_start_fmuv2_sensors(void);
|
||||
void px4_start_fmuv4_sensors(void);
|
||||
void px4_start_optional_sensors(void);
|
||||
#endif // HAL_BOARD_PX4 || HAL_BOARD_VRBRAIN
|
||||
|
||||
// target temperarure for IMU in Celsius, or -1 to disable
|
||||
|
||||
@ -28,6 +28,8 @@
|
||||
#include <unistd.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <drivers/drv_sbus.h>
|
||||
#include <nuttx/arch.h>
|
||||
#include <spawn.h>
|
||||
|
||||
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
|
||||
#define BOARD_PWM_COUNT_DEFAULT 2
|
||||
@ -42,16 +44,37 @@
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
#if !defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
|
||||
extern "C" int uavcan_main(int argc, const char *argv[]);
|
||||
/*
|
||||
declare driver main entry points
|
||||
*/
|
||||
extern "C" {
|
||||
int mpu6000_main(int , char **);
|
||||
int mpu9250_main(int , char **);
|
||||
int ms5611_main(int , char **);
|
||||
int l3gd20_main(int , char **);
|
||||
int lsm303d_main(int , char **);
|
||||
int hmc5883_main(int , char **);
|
||||
int ets_airspeed_main(int, char **);
|
||||
int meas_airspeed_main(int, char **);
|
||||
int ll40ls_main(int, char **);
|
||||
int trone_main(int, char **);
|
||||
int mb12xx_main(int, char **);
|
||||
int pwm_input_main(int, char **);
|
||||
int oreoled_main(int, char **);
|
||||
int batt_smbus_main(int, char **);
|
||||
int irlock_main(int, char **);
|
||||
int px4flow_main(int, char **);
|
||||
int uavcan_main(int, char **);
|
||||
};
|
||||
|
||||
|
||||
#if !defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
|
||||
#define _UAVCAN_IOCBASE (0x4000) // IOCTL base for module UAVCAN
|
||||
#define _UAVCAN_IOC(_n) (_IOC(_UAVCAN_IOCBASE, _n))
|
||||
|
||||
#define UAVCAN_IOCG_NODEID_INPROGRESS _UAVCAN_IOC(1) // query if node identification is in progress
|
||||
#endif
|
||||
|
||||
|
||||
/*
|
||||
setup PWM pins
|
||||
*/
|
||||
@ -69,6 +92,9 @@ void AP_BoardConfig::px4_setup_pwm()
|
||||
{ 4, PWM_SERVO_MODE_4PWM, 2 },
|
||||
{ 6, PWM_SERVO_MODE_6PWM, 0 },
|
||||
{ 7, PWM_SERVO_MODE_3PWM1CAP, 2 },
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
||||
{ 8, PWM_SERVO_MODE_12PWM, 0 },
|
||||
#endif
|
||||
};
|
||||
uint8_t mode_parm = (uint8_t)px4.pwm_count.get();
|
||||
uint8_t i;
|
||||
@ -166,22 +192,16 @@ void AP_BoardConfig::px4_setup_canbus(void)
|
||||
{
|
||||
#if !defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
|
||||
if (px4.can_enable >= 1) {
|
||||
const char *args[] = { "uavcan", "start", NULL };
|
||||
int ret = uavcan_main(3, args);
|
||||
if (ret != 0) {
|
||||
hal.console->printf("UAVCAN: failed to start\n");
|
||||
} else {
|
||||
if (px4_start_driver(uavcan_main, "uavcan", "start")) {
|
||||
hal.console->printf("UAVCAN: started\n");
|
||||
// give some time for CAN bus initialisation
|
||||
hal.scheduler->delay(2000);
|
||||
} else {
|
||||
hal.console->printf("UAVCAN: failed to start\n");
|
||||
}
|
||||
}
|
||||
if (px4.can_enable >= 2) {
|
||||
const char *args[] = { "uavcan", "start", "fw", NULL };
|
||||
int ret = uavcan_main(4, args);
|
||||
if (ret != 0) {
|
||||
hal.console->printf("UAVCAN: failed to start servers\n");
|
||||
} else {
|
||||
if (px4_start_driver(uavcan_main, "uavcan", "start fw")) {
|
||||
uint32_t start_wait_ms = AP_HAL::millis();
|
||||
int fd = open("/dev/uavcan/esc", 0); // design flaw of uavcan driver, this should be /dev/uavcan/node one day
|
||||
if (fd == -1) {
|
||||
@ -196,10 +216,286 @@ void AP_BoardConfig::px4_setup_canbus(void)
|
||||
hal.console->printf("UAVCAN: node discovery complete\n");
|
||||
close(fd);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
}
|
||||
|
||||
extern "C" int waitpid(pid_t, int *, int);
|
||||
|
||||
/*
|
||||
start one px4 driver
|
||||
*/
|
||||
bool AP_BoardConfig::px4_start_driver(main_fn_t main_function, const char *name, const char *arguments)
|
||||
{
|
||||
char *s = strdup(arguments);
|
||||
char *args[10];
|
||||
uint8_t nargs = 0;
|
||||
char *saveptr = nullptr;
|
||||
|
||||
// parse into separate arguments
|
||||
for (char *tok=strtok_r(s, " ", &saveptr); tok; tok=strtok_r(nullptr, " ", &saveptr)) {
|
||||
args[nargs++] = tok;
|
||||
if (nargs == ARRAY_SIZE(args)-1) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
args[nargs++] = nullptr;
|
||||
|
||||
printf("Starting driver %s %s\n", name, arguments);
|
||||
pid_t pid;
|
||||
|
||||
if (task_spawn(&pid, name, main_function, nullptr, nullptr,
|
||||
args, nullptr) != 0) {
|
||||
free(s);
|
||||
printf("Failed to spawn %s\n", name);
|
||||
return false;
|
||||
}
|
||||
|
||||
// wait for task to exit and gather status
|
||||
int status = -1;
|
||||
if (waitpid(pid, &status, 0) != pid) {
|
||||
printf("waitpid failed for %s\n", name);
|
||||
free(s);
|
||||
return false;
|
||||
}
|
||||
free(s);
|
||||
return (status >> 8) == 0;
|
||||
}
|
||||
|
||||
/*
|
||||
setup sensors for PX4v2
|
||||
*/
|
||||
void AP_BoardConfig::px4_start_fmuv2_sensors(void)
|
||||
{
|
||||
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
|
||||
bool have_FMUV3 = false;
|
||||
|
||||
printf("Starting FMUv2 sensors\n");
|
||||
if (px4_start_driver(hmc5883_main, "hmc5883", "-C -T -X start")) {
|
||||
printf("Have external hmc5883\n");
|
||||
} else {
|
||||
printf("No external hmc5883\n");
|
||||
}
|
||||
if (px4_start_driver(hmc5883_main, "hmc5883", "-C -T -I -R 4 start")) {
|
||||
printf("Have internal hmc5883\n");
|
||||
} else {
|
||||
printf("No internal hmc5883\n");
|
||||
}
|
||||
|
||||
// external MPU6000 is rotated YAW_180 from standard
|
||||
if (px4_start_driver(mpu6000_main, "mpu6000", "-X -R 4 start")) {
|
||||
printf("Found MPU6000 external\n");
|
||||
have_FMUV3 = true;
|
||||
} else {
|
||||
if (px4_start_driver(mpu9250_main, "mpu9250", "-X -R 4 start")) {
|
||||
printf("Found MPU9250 external\n");
|
||||
have_FMUV3 = true;
|
||||
} else {
|
||||
printf("No MPU6000 or MPU9250 external\n");
|
||||
}
|
||||
}
|
||||
if (have_FMUV3) {
|
||||
// external L3GD20 is rotated YAW_180 from standard
|
||||
if (px4_start_driver(l3gd20_main, "l3gd20", "-X -R 4 start")) {
|
||||
printf("l3gd20 external started OK\n");
|
||||
} else {
|
||||
px4_sensor_error("No l3gd20");
|
||||
}
|
||||
// external LSM303D is rotated YAW_270 from standard
|
||||
if (px4_start_driver(lsm303d_main, "lsm303d", "-a 16 -X -R 6 start")) {
|
||||
printf("lsm303d external started OK\n");
|
||||
} else {
|
||||
px4_sensor_error("No lsm303d");
|
||||
}
|
||||
// internal MPU6000 is rotated ROLL_180_YAW_270 from standard
|
||||
if (px4_start_driver(mpu6000_main, "mpu6000", "-R 14 start")) {
|
||||
printf("Found MPU6000 internal\n");
|
||||
} else {
|
||||
if (px4_start_driver(mpu9250_main, "mpu9250", "-R 14 start")) {
|
||||
printf("Found MPU9250 internal\n");
|
||||
} else {
|
||||
px4_sensor_error("No MPU6000 or MPU9250");
|
||||
}
|
||||
}
|
||||
if (px4_start_driver(hmc5883_main, "hmc5883", "-C -T -S -R 8 start")) {
|
||||
printf("Found SPI hmc5883\n");
|
||||
}
|
||||
} else {
|
||||
// not FMUV3 (ie. not a pixhawk2)
|
||||
if (px4_start_driver(mpu6000_main, "mpu6000", "start")) {
|
||||
printf("Found MPU6000\n");
|
||||
} else {
|
||||
if (px4_start_driver(mpu9250_main, "mpu9250", "start")) {
|
||||
printf("Found MPU9250\n");
|
||||
} else {
|
||||
printf("No MPU6000 or MPU9250\n");
|
||||
}
|
||||
}
|
||||
if (px4_start_driver(l3gd20_main, "l3gd20", "start")) {
|
||||
printf("l3gd20 started OK\n");
|
||||
} else {
|
||||
px4_sensor_error("no l3gd20 found");
|
||||
}
|
||||
if (px4_start_driver(lsm303d_main, "lsm303d", "-a 16 start")) {
|
||||
printf("lsm303d started OK\n");
|
||||
} else {
|
||||
px4_sensor_error("no lsm303d found");
|
||||
}
|
||||
}
|
||||
printf("FMUv2 sensors started\n");
|
||||
#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
}
|
||||
|
||||
/*
|
||||
setup sensors for PX4v1
|
||||
*/
|
||||
void AP_BoardConfig::px4_start_fmuv1_sensors(void)
|
||||
{
|
||||
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
|
||||
printf("Starting FMUv1 sensors\n");
|
||||
if (px4_start_driver(hmc5883_main, "hmc5883", "-C -T -X start")) {
|
||||
printf("Have external hmc5883\n");
|
||||
} else {
|
||||
printf("No external hmc5883\n");
|
||||
}
|
||||
if (px4_start_driver(hmc5883_main, "hmc5883", "-C -T -I start")) {
|
||||
printf("Have internal hmc5883\n");
|
||||
} else {
|
||||
printf("No internal hmc5883\n");
|
||||
}
|
||||
if (px4_start_driver(mpu6000_main, "mpu6000", "start")) {
|
||||
printf("mpu6000 started OK\n");
|
||||
} else {
|
||||
px4_sensor_error("mpu6000");
|
||||
}
|
||||
if (px4_start_driver(l3gd20_main, "l3gd20", "start")) {
|
||||
printf("l3gd20 started OK\n");
|
||||
} else {
|
||||
printf("No l3gd20\n");
|
||||
}
|
||||
#endif // CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
}
|
||||
|
||||
/*
|
||||
setup sensors for FMUv4
|
||||
*/
|
||||
void AP_BoardConfig::px4_start_fmuv4_sensors(void)
|
||||
{
|
||||
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
|
||||
printf("Starting FMUv4 sensors\n");
|
||||
if (px4_start_driver(hmc5883_main, "hmc5883", "-C -T -X start")) {
|
||||
printf("Have external hmc5883\n");
|
||||
} else {
|
||||
printf("No external hmc5883\n");
|
||||
}
|
||||
if (px4_start_driver(hmc5883_main, "hmc5883", "-C -T -S -R 2 start")) {
|
||||
printf("Have SPI hmc5883\n");
|
||||
} else {
|
||||
printf("No SPI hmc5883\n");
|
||||
}
|
||||
|
||||
if (px4_start_driver(mpu6000_main, "mpu6000", "-R 2 -T 20608 start")) {
|
||||
printf("Found ICM-20608 internal\n");
|
||||
}
|
||||
|
||||
if (px4_start_driver(mpu9250_main, "mpu9250", "-R 2 start")) {
|
||||
printf("Found mpu9250 internal\n");
|
||||
}
|
||||
#endif // CONFIG_ARCH_BOARD_PX4FMU_V4
|
||||
}
|
||||
|
||||
/*
|
||||
setup common sensors
|
||||
*/
|
||||
void AP_BoardConfig::px4_start_common_sensors(void)
|
||||
{
|
||||
printf("Starting APM sensors\n");
|
||||
if (px4_start_driver(ms5611_main, "ms5611", "start")) {
|
||||
printf("ms5611 started OK\n");
|
||||
} else {
|
||||
px4_sensor_error("no ms5611 found");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
setup optional sensors
|
||||
*/
|
||||
void AP_BoardConfig::px4_start_optional_sensors(void)
|
||||
{
|
||||
if (px4_start_driver(ets_airspeed_main, "ets_airspeed", "start")) {
|
||||
printf("Found ETS airspeed sensor\n");
|
||||
}
|
||||
|
||||
if (px4_start_driver(meas_airspeed_main, "meas_airspeed", "start")) {
|
||||
printf("Found MEAS airspeed sensor\n");
|
||||
} else if (px4_start_driver(meas_airspeed_main, "meas_airspeed", "start -b 2")) {
|
||||
printf("Found MEAS airspeed sensor (bus2)\n");
|
||||
}
|
||||
|
||||
if (px4_start_driver(ll40ls_main, "ll40ls", "-X start")) {
|
||||
printf("Found external ll40ls sensor\n");
|
||||
}
|
||||
if (px4_start_driver(ll40ls_main, "ll40ls", "-I start")) {
|
||||
printf("Found internal ll40ls sensor\n");
|
||||
}
|
||||
if (px4_start_driver(trone_main, "trone", "start")) {
|
||||
printf("Found trone sensor\n");
|
||||
}
|
||||
if (px4_start_driver(mb12xx_main, "mb12xx", "start")) {
|
||||
printf("Found mb12xx sensor\n");
|
||||
}
|
||||
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
|
||||
if (px4_start_driver(px4flow_main, "px4flow", "start")) {
|
||||
printf("Found px4flow sensor\n");
|
||||
}
|
||||
#endif
|
||||
|
||||
if (px4_start_driver(pwm_input_main, "pwm_input", "start")) {
|
||||
printf("started pwm_input driver\n");
|
||||
}
|
||||
|
||||
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
|
||||
if (px4_start_driver(oreoled_main, "oreoled", "start autoupdate")) {
|
||||
printf("oreoled started OK\n");
|
||||
}
|
||||
#endif
|
||||
|
||||
if (px4_start_driver(batt_smbus_main, "batt_smbus", "-b 2 start")) {
|
||||
printf("Found batt_smbus\n");
|
||||
}
|
||||
|
||||
if (px4_start_driver(irlock_main, "irlock", "start")) {
|
||||
printf("irlock started\n");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void AP_BoardConfig::px4_setup_drivers(void)
|
||||
{
|
||||
px4_start_common_sensors();
|
||||
px4_start_fmuv1_sensors();
|
||||
px4_start_fmuv2_sensors();
|
||||
px4_start_fmuv4_sensors();
|
||||
px4_start_optional_sensors();
|
||||
}
|
||||
|
||||
/*
|
||||
fail startup of a required sensor
|
||||
*/
|
||||
void AP_BoardConfig::px4_sensor_error(const char *reason)
|
||||
{
|
||||
while (true) {
|
||||
printf("Sensor failure: %s\n", reason);
|
||||
hal.console->printf("Sensor failure: %s\n", reason);
|
||||
// need to force LED red
|
||||
hal.scheduler->delay(1000);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
setup px4 peripherals and drivers
|
||||
*/
|
||||
void AP_BoardConfig::px4_setup()
|
||||
{
|
||||
px4_setup_pwm();
|
||||
@ -207,6 +503,10 @@ void AP_BoardConfig::px4_setup()
|
||||
px4_setup_uart();
|
||||
px4_setup_sbus();
|
||||
px4_setup_canbus();
|
||||
px4_setup_drivers();
|
||||
|
||||
// delay for 1 second to give time for drivers to initialise
|
||||
hal.scheduler->delay(1000);
|
||||
}
|
||||
|
||||
#endif // HAL_BOARD_PX4
|
||||
|
||||
Loading…
Reference in New Issue
Block a user