From a5df93bf106691f5eeb49b74cf0f67d1017209df Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Wed, 5 Aug 2015 19:02:56 -0300 Subject: [PATCH] AP_Compass: AK8963: be agnostic to I2C bus/address This decision is better made by the caller rather than polluting the driver with board-specific details. --- libraries/AP_Compass/AP_Compass_AK8963.cpp | 14 ++++++-------- libraries/AP_Compass/AP_Compass_AK8963.h | 4 +++- libraries/AP_Compass/Compass.cpp | 3 ++- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index ef4afe7b5a..09f7562829 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -79,10 +79,6 @@ #endif #endif -#if !defined(HAL_COMPASS_AK8963_I2C_ADDR) -#define HAL_COMPASS_AK8963_I2C_ADDR 0xC -#endif - extern const AP_HAL::HAL& hal; AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_SerialBus *bus) : @@ -112,11 +108,13 @@ AP_Compass_Backend *AP_Compass_AK8963::detect_mpu9250(Compass &compass) return sensor; } -AP_Compass_Backend *AP_Compass_AK8963::detect_i2c1(Compass &compass) + +AP_Compass_Backend *AP_Compass_AK8963::detect_i2c(Compass &compass, + AP_HAL::I2CDriver *i2c, + uint8_t addr) { - AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, - new AP_AK8963_SerialBus_I2C( - hal.i2c1, HAL_COMPASS_AK8963_I2C_ADDR)); + AP_Compass_AK8963 *sensor = + new AP_Compass_AK8963(compass, new AP_AK8963_SerialBus_I2C(i2c, addr)); if (sensor == nullptr) { return nullptr; diff --git a/libraries/AP_Compass/AP_Compass_AK8963.h b/libraries/AP_Compass/AP_Compass_AK8963.h index a395e31e5d..96f4ac8421 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.h +++ b/libraries/AP_Compass/AP_Compass_AK8963.h @@ -37,7 +37,9 @@ public: AP_Compass_AK8963(Compass &compass, AP_AK8963_SerialBus *bus); static AP_Compass_Backend *detect_mpu9250(Compass &compass); - static AP_Compass_Backend *detect_i2c1(Compass &compass); + static AP_Compass_Backend *detect_i2c(Compass &compass, + AP_HAL::I2CDriver *i2c, + uint8_t addr); bool init(void); void read(void); diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 182e6631cd..83e2837b69 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -348,7 +348,8 @@ void Compass::_detect_backends(void) #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843 _add_backend(AP_Compass_HMC5843::detect(*this)); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_I2C && HAL_INS_AK8963_I2C_BUS == 1 - _add_backend(AP_Compass_AK8963::detect_i2c1(*this)); + _add_backend(AP_Compass_AK8963::detect_i2c(*this, hal.i2c1, + HAL_COMPASS_AK8963_I2C_ADDR)); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_PX4 || HAL_COMPASS_DEFAULT == HAL_COMPASS_VRBRAIN _add_backend(AP_Compass_PX4::detect(*this)); #else