From 732dc5984a6a052c0865e3759f3a4c3ab08b358a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 18 Jun 2017 17:26:25 +1000 Subject: [PATCH] AP_Compass: added COMPASS_TYPEMASK parameter this allows driver type to be disabled, to avoid probing them and wasting CPU on sensors that are not being used This also moves UAVCAN compass to be detected last, as discussed in the dev call --- libraries/AP_Compass/AP_Compass.cpp | 260 +++++++++++++++------------- libraries/AP_Compass/AP_Compass.h | 23 +++ 2 files changed, 165 insertions(+), 118 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 37da3759ff..987c3479da 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -421,6 +421,13 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Increment: 1 // @User: Advanced AP_GROUPINFO("OFFS_MAX", 31, Compass, _offset_max, AP_COMPASS_OFFSETS_MAX_DEFAULT), + + // @Param: TYPEMASK + // @DisplayName: Compass disable driver type mask + // @Description: This is a bitmask of driver types to disable. If a driver type is set in this mask then that driver will not try to find a sensor at startup + // @Bitmask: 0:HMC5883,1:LSM303D,2:AK8963,3:BMM150,4:LSM9DS1,5:LIS3MDL,6:AK09916,7:IST8310,8:ICM20948,9:MMC3416,10:QFLIGHT,11:UAVCAN,12:QMC5883 + // @User: Advanced + AP_GROUPINFO("TYPEMASK", 33, Compass, _driver_type_mask, 0), AP_GROUPEND }; @@ -494,6 +501,15 @@ bool Compass::_add_backend(AP_Compass_Backend *backend, const char *name, bool e return true; } +/* + return true if a driver type is enabled + */ +bool Compass::_driver_enabled(enum DriverType driver_type) +{ + uint32_t mask = (1U<get_device(1, HAL_COMPASS_HMC5843_I2C_ADDR), - true, ROTATION_ROLL_180), - AP_Compass_HMC5843::name, true); + ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_HMC5843_I2C_ADDR), + true, ROTATION_ROLL_180), + AP_Compass_HMC5843::name, true); // internal i2c bus - ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_HMC5843_I2C_ADDR), + ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_HMC5843_I2C_ADDR), both_i2c_external, both_i2c_external?ROTATION_ROLL_180:ROTATION_YAW_270), AP_Compass_HMC5843::name, both_i2c_external); + //external i2c bus - ADD_BACKEND(AP_Compass_QMC5883L::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_QMC5883L_I2C_ADDR), + ADD_BACKEND(DRIVER_QMC5883, AP_Compass_QMC5883L::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_QMC5883L_I2C_ADDR), true,ROTATION_ROLL_180), AP_Compass_QMC5883L::name, true); //internal i2c bus - ADD_BACKEND(AP_Compass_QMC5883L::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_QMC5883L_I2C_ADDR), + ADD_BACKEND(DRIVER_QMC5883, AP_Compass_QMC5883L::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_QMC5883L_I2C_ADDR), both_i2c_external, both_i2c_external?ROTATION_ROLL_180:ROTATION_YAW_270), AP_Compass_QMC5883L::name,both_i2c_external); #if !HAL_MINIMIZE_FEATURES // AK09916 on ICM20948 - ADD_BACKEND(AP_Compass_AK09916::probe_ICM20948(*this, - hal.i2c_mgr->get_device(1, HAL_COMPASS_AK09916_I2C_ADDR), - hal.i2c_mgr->get_device(1, HAL_COMPASS_ICM20948_I2C_ADDR), - true, ROTATION_NONE), + ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(*this, + hal.i2c_mgr->get_device(1, HAL_COMPASS_AK09916_I2C_ADDR), + hal.i2c_mgr->get_device(1, HAL_COMPASS_ICM20948_I2C_ADDR), + true, ROTATION_NONE), AP_Compass_AK09916::name, true); - ADD_BACKEND(AP_Compass_AK09916::probe_ICM20948(*this, - hal.i2c_mgr->get_device(0, HAL_COMPASS_AK09916_I2C_ADDR), - hal.i2c_mgr->get_device(0, HAL_COMPASS_ICM20948_I2C_ADDR), - both_i2c_external, ROTATION_NONE), + ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(*this, + hal.i2c_mgr->get_device(0, HAL_COMPASS_AK09916_I2C_ADDR), + hal.i2c_mgr->get_device(0, HAL_COMPASS_ICM20948_I2C_ADDR), + both_i2c_external, ROTATION_NONE), AP_Compass_AK09916::name, true); -#if 0 - // lis3mdl - this is disabled for now due to an errata on pixhawk2 GPS unit, pending investigation - ADD_BACKEND(AP_Compass_LIS3MDL::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_LIS3MDL_I2C_ADDR), - true, ROTATION_YAW_90), - AP_Compass_LIS3MDL::name, true); - ADD_BACKEND(AP_Compass_LIS3MDL::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_LIS3MDL_I2C_ADDR), - both_i2c_external, both_i2c_external?ROTATION_YAW_90:ROTATION_NONE), - AP_Compass_LIS3MDL::name, both_i2c_external); -#endif - + // lis3mdl + ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_LIS3MDL_I2C_ADDR), + true, ROTATION_YAW_90), + AP_Compass_LIS3MDL::name, true); + ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_LIS3MDL_I2C_ADDR), + both_i2c_external, both_i2c_external?ROTATION_YAW_90:ROTATION_NONE), + AP_Compass_LIS3MDL::name, both_i2c_external); + // AK09916 - ADD_BACKEND(AP_Compass_AK09916::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_AK09916_I2C_ADDR), - true, ROTATION_YAW_270), - AP_Compass_AK09916::name, true); - ADD_BACKEND(AP_Compass_AK09916::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_AK09916_I2C_ADDR), - both_i2c_external, both_i2c_external?ROTATION_YAW_270:ROTATION_NONE), - AP_Compass_AK09916::name, both_i2c_external); + ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_AK09916_I2C_ADDR), + true, ROTATION_YAW_270), + AP_Compass_AK09916::name, true); + ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_AK09916_I2C_ADDR), + both_i2c_external, both_i2c_external?ROTATION_YAW_270:ROTATION_NONE), + AP_Compass_AK09916::name, both_i2c_external); #endif // HAL_MINIMIZE_FEATURES } break; case AP_BoardConfig::PX4_BOARD_AEROFC: #ifdef HAL_COMPASS_IST8310_I2C_BUS - ADD_BACKEND(AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_IST8310_I2C_BUS, HAL_COMPASS_IST8310_I2C_ADDR), - ROTATION_PITCH_180_YAW_90), AP_Compass_IST8310::name, true); + ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_IST8310_I2C_BUS, HAL_COMPASS_IST8310_I2C_ADDR), + ROTATION_PITCH_180_YAW_90), AP_Compass_IST8310::name, true); #endif break; default: @@ -606,43 +618,43 @@ void Compass::_detect_backends(void) } switch (AP_BoardConfig::get_board_type()) { case AP_BoardConfig::PX4_BOARD_PIXHAWK: - ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME), - false, ROTATION_PITCH_180), - AP_Compass_HMC5843::name, false); - ADD_BACKEND(AP_Compass_LSM303D::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME)), - AP_Compass_LSM303D::name, false); + ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME), + false, ROTATION_PITCH_180), + AP_Compass_HMC5843::name, false); + ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME)), + AP_Compass_LSM303D::name, false); break; case 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), - AP_Compass_LSM303D::name, false); + ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME), ROTATION_YAW_270), + AP_Compass_LSM303D::name, false); // we run the AK8963 only on the 2nd MPU9250, which leaves the // first MPU9250 to run without disturbance at high rate - ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 1, ROTATION_YAW_270), - AP_Compass_AK8963::name, false); + ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 1, ROTATION_YAW_270), + AP_Compass_AK8963::name, false); break; case AP_BoardConfig::PX4_BOARD_PIXRACER: - ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME), - false, ROTATION_PITCH_180), - AP_Compass_HMC5843::name, false); - ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90), - AP_Compass_AK8963::name, false); + ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME), + false, ROTATION_PITCH_180), + AP_Compass_HMC5843::name, false); + ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90), + AP_Compass_AK8963::name, false); break; case AP_BoardConfig::PX4_BOARD_PHMINI: - ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180), - AP_Compass_AK8963::name, false); + ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180), + AP_Compass_AK8963::name, false); break; case AP_BoardConfig::PX4_BOARD_AUAV21: - ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90), - AP_Compass_AK8963::name, false); + ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90), + AP_Compass_AK8963::name, false); break; case AP_BoardConfig::PX4_BOARD_PH2SLIM: - ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_YAW_270), - AP_Compass_AK8963::name, false); + ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_YAW_270), + AP_Compass_AK8963::name, false); break; default: @@ -650,29 +662,29 @@ void Compass::_detect_backends(void) } #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QURT - ADD_BACKEND(AP_Compass_QURT::detect(*this), nullptr, false); + ADD_BACKEND(DRIVER_QFLIGHT, AP_Compass_QURT::detect(*this), nullptr, false); #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), - AP_Compass_HMC5843::name, true); - ADD_BACKEND(AP_Compass_LSM303D::probe(*this, hal.spi->get_device("lsm9ds0_am")), - AP_Compass_LSM303D::name, false); + ADD_BACKEND(DRIVER_HMC5883, 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); + ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(*this, hal.spi->get_device("lsm9ds0_am")), + AP_Compass_LSM303D::name, false); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BH - ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)), + ADD_BACKEND(DRIVER_HMC5883, 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); - ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0), AP_Compass_AK8963::name, false); + ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0), AP_Compass_AK8963::name, false); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QFLIGHT - ADD_BACKEND(AP_Compass_QFLIGHT::detect(*this)); + ADD_BACKEND(DRIVER_QFLIGHT, AP_Compass_QFLIGHT::detect(*this)); #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), - AP_Compass_HMC5843::name, true); - ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0), - AP_Compass_AK8963::name, false); - ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 1), - AP_Compass_AK8963::name, true); + ADD_BACKEND(DRIVER_HMC5883, 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); + ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0), + AP_Compass_AK8963::name, false); + ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 1), + AP_Compass_AK8963::name, true); #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE - ADD_BACKEND(AP_Compass_HMC5843::probe_mpu6000(*this), - AP_Compass_HMC5843::name, false); - ADD_BACKEND(AP_Compass_HMC5843::probe(*this, + ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe_mpu6000(*this), + AP_Compass_HMC5843::name, false); + ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, Linux::I2CDeviceManager::from(hal.i2c_mgr)->get_device( { /* UEFI with lpss set to ACPI */ "platform/80860F41:05", @@ -682,58 +694,70 @@ void Compass::_detect_backends(void) true), AP_Compass_HMC5843::name, true); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NAVIO2 - ADD_BACKEND(AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m"), ROTATION_ROLL_180), - AP_Compass_LSM9DS1::name, false); - ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0), - 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), - AP_Compass_HMC5843::name, true); + ADD_BACKEND(DRIVER_LSM9DS1, AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m"), ROTATION_ROLL_180), + AP_Compass_LSM9DS1::name, false); + ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0), + AP_Compass_AK8963::name, false); + ADD_BACKEND(DRIVER_HMC5883, 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); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NAVIO - ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0), - 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), - AP_Compass_HMC5843::name, true); + ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0), + AP_Compass_AK8963::name, false); + ADD_BACKEND(DRIVER_HMC5883, 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); #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_PXFMINI - ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0), - 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), - AP_Compass_HMC5843::name, true); + ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0), + AP_Compass_AK8963::name, false); + ADD_BACKEND(DRIVER_HMC5883, 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); #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE - 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); - 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(DRIVER_AK8963, 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); + ADD_BACKEND(DRIVER_HMC5883, 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); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250 - ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0), - AP_Compass_AK8963::name, false); + ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0), + AP_Compass_AK8963::name, false); #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)), - AP_Compass_HMC5843::name, false); + ADD_BACKEND(DRIVER_HMC5883, 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); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843_MPU6000 - ADD_BACKEND(AP_Compass_HMC5843::probe_mpu6000(*this), - AP_Compass_HMC5843::name, false); + ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe_mpu6000(*this), + AP_Compass_HMC5843::name, false); #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)), - AP_Compass_AK8963::name, false); + ADD_BACKEND(DRIVER_AK8963, 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); #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)), - AP_Compass_AK8963::name, false); + ADD_BACKEND(DRIVER_AK8963, 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); #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)), - AP_Compass_BMM150::name, false); + ADD_BACKEND(DRIVER_BMM150, 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); #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)), - AP_Compass_HMC5843::name, false); - ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0), - AP_Compass_AK8963::name, false); - ADD_BACKEND(AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m")), - AP_Compass_LSM9DS1::name, false); + ADD_BACKEND(DRIVER_HMC5883, 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); + ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0), + AP_Compass_AK8963::name, false); + ADD_BACKEND(DRIVER_LSM9DS1, AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m")), + AP_Compass_LSM9DS1::name, false); #else #error Unrecognised HAL_COMPASS_TYPE setting #endif +#if HAL_WITH_UAVCAN + if (_driver_enabled(DRIVER_UAVCAN)) { + bool added; + do { + added = _add_backend(AP_Compass_UAVCAN::probe(*this), "UAVCAN", true); + if (_backend_count == COMPASS_MAX_BACKEND || _compass_count == COMPASS_MAX_INSTANCES) { + return; + } + } while (added); + } +#endif + if (_backend_count == 0 || _compass_count == 0) { hal.console->printf("No Compass backends available\n"); diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 9c47cdb0cb..9277b7ef45 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -320,6 +320,26 @@ private: bool _cal_complete_requires_reboot; bool _cal_has_run; + // enum of drivers for COMPASS_TYPEMASK + enum DriverType { + DRIVER_HMC5883 =0, + DRIVER_LSM303D =1, + DRIVER_AK8963 =2, + DRIVER_BMM150 =3, + DRIVER_LSM9DS1 =4, + DRIVER_LIS3MDL =5, + DRIVER_AK09916 =6, + DRIVER_IST8310 =7, + DRIVER_ICM20948 =8, + DRIVER_MMC3416 =9, + DRIVER_QFLIGHT =10, + DRIVER_UAVCAN =11, + DRIVER_QMC5883 =12, + DRIVER_SITL =13, + }; + + bool _driver_enabled(enum DriverType driver_type); + // backend objects AP_Compass_Backend *_backends[COMPASS_MAX_BACKEND]; uint8_t _backend_count; @@ -398,4 +418,7 @@ private: bool _hil_mode:1; AP_Float _calibration_threshold; + + // mask of driver types to not load. Bit positions match DEVTYPE_ in backend + AP_Int32 _driver_type_mask; };