Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: don't panic on too many compasses

Browse Source 
just use the first ones found. Panicing because you have too many
isn't useful
This commit is contained in:
Andrew Tridgell 2016-11-24 17:48:45 +11:00
parent 40ef68f104
commit b6b2919126
1 changed files with 51 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
libraries/AP_Compass/AP_Compass.cpp
View File

@ -489,8 +489,20 @@ void Compass::_detect_backends(void)
return; return;
} }
/*
macro to add a backend with check for too many backends or compass
instances. We don't try to start more than the maximum allowed
*/
#define ADD_BACKEND(backend, name, external) \
do { _add_backend(backend, name, external); \
if (_backend_count == COMPASS_MAX_BACKEND || \
_compass_count == COMPASS_MAX_INSTANCES) { \
return; \
} \
} while (0)
#if HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL #if HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL
_add_backend(AP_Compass_HIL::detect(*this), nullptr, false); ADD_BACKEND(AP_Compass_HIL::detect(*this), nullptr, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_PX4 || HAL_COMPASS_DEFAULT == HAL_COMPASS_VRBRAIN #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_PX4 || HAL_COMPASS_DEFAULT == HAL_COMPASS_VRBRAIN
if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PX4V1 || if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PX4V1 ||
AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK || AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK ||
@ -499,73 +511,73 @@ void Compass::_detect_backends(void)
AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2 || AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2 ||
AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXRACER) { AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXRACER) {
// external i2c bus // external i2c bus
_add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_HMC5843_I2C_ADDR), ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_HMC5843_I2C_ADDR),
true, ROTATION_ROLL_180), true, ROTATION_ROLL_180),
AP_Compass_HMC5843::name, true); AP_Compass_HMC5843::name, true);
// internal i2c bus // internal i2c bus
_add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_HMC5843_I2C_ADDR), false), ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_HMC5843_I2C_ADDR), false),
AP_Compass_HMC5843::name, false); AP_Compass_HMC5843::name, false);
} }
if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK) { if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK) {
_add_backend(AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME), ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME),
false, ROTATION_PITCH_180), false, ROTATION_PITCH_180),
AP_Compass_HMC5843::name, false); AP_Compass_HMC5843::name, false);
_add_backend(AP_Compass_LSM303D::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME)), ADD_BACKEND(AP_Compass_LSM303D::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME)),
AP_Compass_LSM303D::name, false); AP_Compass_LSM303D::name, false);
} }
if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2) { if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2) {
_add_backend(AP_Compass_LSM303D::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME), ROTATION_YAW_270), ADD_BACKEND(AP_Compass_LSM303D::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME), ROTATION_YAW_270),
AP_Compass_LSM303D::name, false); AP_Compass_LSM303D::name, false);
// we run the AK8963 only on the 2nd MPU9250, which leaves the // we run the AK8963 only on the 2nd MPU9250, which leaves the
// first MPU9250 to run without disturbance at high rate // first MPU9250 to run without disturbance at high rate
_add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 1, ROTATION_YAW_270), ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 1, ROTATION_YAW_270),
AP_Compass_AK8963::name, false); AP_Compass_AK8963::name, false);
} }
if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXRACER) { if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXRACER) {
_add_backend(AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME), ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME),
false, ROTATION_PITCH_180), false, ROTATION_PITCH_180),
AP_Compass_HMC5843::name, false); AP_Compass_HMC5843::name, false);
_add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90), ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90),
AP_Compass_AK8963::name, false); AP_Compass_AK8963::name, false);
} }
if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PHMINI) { if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PHMINI) {
_add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180), ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180),
AP_Compass_AK8963::name, false); AP_Compass_AK8963::name, false);
} }
if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PH2SLIM) { if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PH2SLIM) {
_add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_YAW_270), ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_YAW_270),
AP_Compass_AK8963::name, false); AP_Compass_AK8963::name, false);
} }
// also add any px4 level drivers (for canbus magnetometers) // also add any px4 level drivers (for canbus magnetometers)
_add_backend(AP_Compass_PX4::detect(*this), nullptr, false); ADD_BACKEND(AP_Compass_PX4::detect(*this), nullptr, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QURT #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QURT
_add_backend(AP_Compass_QURT::detect(*this), nullptr, false); ADD_BACKEND(AP_Compass_QURT::detect(*this), nullptr, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_RASPILOT #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_RASPILOT
_add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true), ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true); AP_Compass_HMC5843::name, true);
_add_backend(AP_Compass_LSM303D::probe(*this, hal.spi->get_device("lsm9ds0_am")), ADD_BACKEND(AP_Compass_LSM303D::probe(*this, hal.spi->get_device("lsm9ds0_am")),
AP_Compass_LSM303D::name, false); AP_Compass_LSM303D::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BH #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BH
// In BH, only one compass should be detected // In BH, only one compass should be detected
bool ret = _add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)), bool ret = ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
AP_Compass_HMC5843::name, false); AP_Compass_HMC5843::name, false);
if (!ret) { if (!ret) {
_add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0), ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false); AP_Compass_AK8963::name, false);
} }
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QFLIGHT #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QFLIGHT
_add_backend(AP_Compass_QFLIGHT::detect(*this)); ADD_BACKEND(AP_Compass_QFLIGHT::detect(*this));
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BBBMINI #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BBBMINI
_add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true), ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true); AP_Compass_HMC5843::name, true);
_add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0), ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false); AP_Compass_AK8963::name, false);
_add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 1), ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 1),
AP_Compass_AK8963::name, true); AP_Compass_AK8963::name, true);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
_add_backend(AP_Compass_HMC5843::probe_mpu6000(*this), ADD_BACKEND(AP_Compass_HMC5843::probe_mpu6000(*this),
AP_Compass_HMC5843::name, false); AP_Compass_HMC5843::name, false);
_add_backend(AP_Compass_HMC5843::probe(*this, ADD_BACKEND(AP_Compass_HMC5843::probe(*this,
Linux::I2CDeviceManager::from(hal.i2c_mgr)->get_device( Linux::I2CDeviceManager::from(hal.i2c_mgr)->get_device(
{ /* UEFI with lpss set to ACPI */ { /* UEFI with lpss set to ACPI */
"platform/80860F41:05", "platform/80860F41:05",
@ -575,48 +587,48 @@ void Compass::_detect_backends(void)
true), true),
AP_Compass_HMC5843::name, true); AP_Compass_HMC5843::name, true);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NAVIO2 #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NAVIO2
_add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0), ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false); AP_Compass_AK8963::name, false);
_add_backend(AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m")), ADD_BACKEND(AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m")),
AP_Compass_LSM9DS1::name, false); AP_Compass_LSM9DS1::name, false);
_add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true), ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true); AP_Compass_HMC5843::name, true);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NAVIO #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NAVIO
_add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0), ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false); AP_Compass_AK8963::name, false);
_add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true), ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true); AP_Compass_HMC5843::name, true);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || \ #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
_add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0), ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false); AP_Compass_AK8963::name, false);
_add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true), ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true); AP_Compass_HMC5843::name, true);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250 #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250
_add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0), ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false); AP_Compass_AK8963::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843 #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843
_add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)), ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
AP_Compass_HMC5843::name, false); AP_Compass_HMC5843::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843_MPU6000 #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843_MPU6000
_add_backend(AP_Compass_HMC5843::probe_mpu6000(*this), ADD_BACKEND(AP_Compass_HMC5843::probe_mpu6000(*this),
AP_Compass_HMC5843::name, false); AP_Compass_HMC5843::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_I2C #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_I2C
_add_backend(AP_Compass_AK8963::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)), ADD_BACKEND(AP_Compass_AK8963::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)),
AP_Compass_AK8963::name, false); AP_Compass_AK8963::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250_I2C #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250_I2C
_add_backend(AP_Compass_AK8963::probe_mpu9250(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)), ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)),
AP_Compass_AK8963::name, false); AP_Compass_AK8963::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AERO #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AERO
_add_backend(AP_Compass_BMM150::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_BMM150_I2C_BUS, HAL_COMPASS_BMM150_I2C_ADDR)), ADD_BACKEND(AP_Compass_BMM150::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_BMM150_I2C_BUS, HAL_COMPASS_BMM150_I2C_ADDR)),
AP_Compass_BMM150::name, false); AP_Compass_BMM150::name, false);
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE
_add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)), ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
AP_Compass_HMC5843::name, false); AP_Compass_HMC5843::name, false);
_add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0), ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false); AP_Compass_AK8963::name, false);
_add_backend(AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m")), ADD_BACKEND(AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m")),
AP_Compass_LSM9DS1::name, false); AP_Compass_LSM9DS1::name, false);
#else #else
#error Unrecognised HAL_COMPASS_TYPE setting #error Unrecognised HAL_COMPASS_TYPE setting