From 0eb25fcc74d4c8ab82983441857f135239182385 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 2 Oct 2017 18:23:57 +1100 Subject: [PATCH] AP_Compass: avoid double detection of the AK09916 with an ICM20948 we can double detect the compass due to the i2c pass-thru --- libraries/AP_Compass/AP_Compass.cpp | 30 +++++++++++++++++++++++------ libraries/AP_Compass/AP_Compass.h | 3 +++ 2 files changed, 27 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index f2cff2bf4c..0a9959ff9b 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -510,6 +510,20 @@ bool Compass::_driver_enabled(enum DriverType driver_type) return (mask & uint32_t(_driver_type_mask.get())) == 0; } +/* + see if we already have probed a driver by bus type + */ +bool Compass::_have_driver(AP_HAL::Device::BusType bus_type, uint8_t bus_num, uint8_t address, uint8_t devtype) const +{ + uint32_t id = AP_HAL::Device::make_bus_id(bus_type, bus_num, address, devtype); + for (uint8_t i=0; i<_compass_count; i++) { + if (id == uint32_t(_state[i].dev_id.get())) { + return true; + } + } + return false; +} + /* detect available backends for this board */ @@ -597,13 +611,17 @@ void Compass::_detect_backends(void) both_i2c_external, both_i2c_external?ROTATION_YAW_90:ROTATION_NONE), AP_Compass_LIS3MDL::name, both_i2c_external); - // AK09916 - ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_AK09916_I2C_ADDR), - true, ROTATION_YAW_270), + // AK09916. This can be found twice, due to the ICM20948 i2c bus pass-thru, so we need to be careful to avoid that + if (!_have_driver(AP_HAL::Device::BUS_TYPE_I2C, 1, HAL_COMPASS_AK09916_I2C_ADDR, AP_Compass_Backend::DEVTYPE_ICM20948)) { + 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); + } + if (!_have_driver(AP_HAL::Device::BUS_TYPE_I2C, 0, HAL_COMPASS_AK09916_I2C_ADDR, AP_Compass_Backend::DEVTYPE_ICM20948)) { + 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); + } // IST8310 on external and internal bus ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_IST8310_I2C_ADDR), diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 9277b7ef45..be4377b8be 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -310,6 +310,9 @@ private: bool _start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot=false); bool _auto_reboot() { return _compass_cal_autoreboot; } + // see if we already have probed a driver by bus type + bool _have_driver(AP_HAL::Device::BusType bus_type, uint8_t bus_num, uint8_t address, uint8_t devtype) const; + //keep track of which calibrators have been saved bool _cal_saved[COMPASS_MAX_INSTANCES];