Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_BoardConfig: added board type auto-detection 

using SPI probing
This commit is contained in:
Andrew Tridgell 2016-11-10 15:29:26 +11:00
parent c565c9df35
commit 4f50d67790
2 changed files with 91 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_BoardConfig/AP_BoardConfig.h
View File

@ -104,8 +104,10 @@ private:
void px4_setup_px4io(void);
void px4_tone_alarm(const char *tone_string);
void px4_sensor_error(const char *reason);
bool spi_check_register(const char *devname, uint8_t regnum, uint8_t value, uint8_t read_flag = 0x80);
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
void px4_autodetect(void);
void px4_start_common_sensors(void);
void px4_start_fmuv1_sensors(void);
void px4_start_fmuv2_sensors(void);

98
libraries/AP_BoardConfig/px4_drivers.cpp
View File

@ -595,20 +595,19 @@ void AP_BoardConfig::px4_setup_drivers(void)
}
#endif
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
px4.board_type.set_and_notify(PX4_BOARD_PIXRACER);
#endif
// run board auto-detection
px4_autodetect();
if (px4.board_type == PX4_BOARD_PH2SLIM) {
if (px4.board_type == PX4_BOARD_PH2SLIM ||
px4.board_type == PX4_BOARD_PIXHAWK2) {
_imu_target_temperature.set_default(60);
}
if (px4.board_type == PX4_BOARD_TEST_V1 ||
px4.board_type == PX4_BOARD_TEST_V2 ||
px4.board_type == PX4_BOARD_TEST_V3 ||
px4.board_type == PX4_BOARD_PHMINI ||
if (px4.board_type == PX4_BOARD_PHMINI ||
px4.board_type == PX4_BOARD_PH2SLIM ||
px4.board_type == PX4_BOARD_PIXRACER) {
px4.board_type == PX4_BOARD_PIXRACER ||
px4.board_type == PX4_BOARD_PIXHAWK ||
px4.board_type == PX4_BOARD_PIXHAWK2) {
// use in-tree drivers
printf("Using in-tree drivers\n");
px4_configured_board = (enum px4_board_type)px4.board_type.get();
@ -765,6 +764,87 @@ void AP_BoardConfig::px4_setup_peripherals(void)
hal.rcout->init();
}
/*
check a SPI device for a register value
*/
bool AP_BoardConfig::spi_check_register(const char *devname, uint8_t regnum, uint8_t value, uint8_t read_flag)
{
auto dev = hal.spi->get_device(devname);
if (!dev) {
printf("%s: no device\n", devname);
return false;
}
dev->set_read_flag(read_flag);
uint8_t v;
if (!dev->read_registers(regnum, &v, 1)) {
printf("%s: reg %02x read fail\n", devname, (unsigned)regnum);
return false;
}
printf("%s: reg %02x %02x %02x\n", devname, (unsigned)regnum, (unsigned)value, (unsigned)v);
return v == value;
}
#define MPUREG_WHOAMI 0x75
#define MPU_WHOAMI_MPU60X0 0x68
#define MPU_WHOAMI_MPU9250 0x71
#define MPU_WHOAMI_ICM20608 0xaf
#define LSMREG_WHOAMI 0x0f
#define LSM_WHOAMI_LSM303D 0x49
/*
auto-detect board type
*/
void AP_BoardConfig::px4_autodetect(void)
{
if (px4.board_type == PX4_BOARD_TEST_V1 ||
px4.board_type == PX4_BOARD_TEST_V2 ||
px4.board_type == PX4_BOARD_TEST_V3) {
// these were old test values
px4.board_type.set(0);
}
if (px4.board_type != PX4_BOARD_AUTO) {
// user has chosen a board type
return;
}
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
// only one choice
px4.board_type.set(PX4_BOARD_PX4V1);
hal.console->printf("Detected PX4v1\n");
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
if ((spi_check_register(HAL_INS_MPU60x0_EXT_NAME, MPUREG_WHOAMI, MPU_WHOAMI_MPU60X0) ||
spi_check_register(HAL_INS_MPU9250_EXT_NAME, MPUREG_WHOAMI, MPU_WHOAMI_MPU9250) ||
spi_check_register(HAL_INS_ICM20608_EXT_NAME, MPUREG_WHOAMI, MPU_WHOAMI_ICM20608)) &&
spi_check_register(HAL_INS_LSM9DS0_EXT_A_NAME, LSMREG_WHOAMI, LSM_WHOAMI_LSM303D)) {
// Pixhawk2 has LSM303D and MPUxxxx on external bus
px4.board_type.set(PX4_BOARD_PIXHAWK2);
hal.console->printf("Detected PIXHAWK2\n");
} else if (spi_check_register(HAL_INS_ICM20608_AM_NAME, MPUREG_WHOAMI, MPU_WHOAMI_ICM20608) &&
spi_check_register(HAL_INS_MPU9250_NAME, MPUREG_WHOAMI, MPU_WHOAMI_MPU9250)) {
// PHMINI has an ICM20608 and MPU9250 on sensor bus
px4.board_type.set(PX4_BOARD_PHMINI);
hal.console->printf("Detected PixhawkMini\n");
} else if (spi_check_register(HAL_INS_LSM9DS0_A_NAME, LSMREG_WHOAMI, LSM_WHOAMI_LSM303D) &&
(spi_check_register(HAL_INS_MPU60x0_NAME, MPUREG_WHOAMI, MPU_WHOAMI_MPU60X0) ||
spi_check_register(HAL_INS_ICM20608_NAME, MPUREG_WHOAMI, MPU_WHOAMI_ICM20608) ||
spi_check_register(HAL_INS_MPU9250_NAME, MPUREG_WHOAMI, MPU_WHOAMI_MPU9250))) {
// classic or upgraded Pixhawk1
px4.board_type.set(PX4_BOARD_PIXHAWK);
hal.console->printf("Detected Pixhawk\n");
} else {
px4_sensor_error("Unable to detect board type");
}
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
// only one choice
px4.board_type.set_and_notify(PX4_BOARD_PIXRACER);
hal.console->printf("Detected Pixracer\n");
#endif
}
/*
fail startup of a required sensor
*/