diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp index 911f170a7c..a4af72ceac 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp @@ -35,9 +35,10 @@ extern const AP_HAL::HAL& hal; constructor is not called until detect() returns true, so we already know that we should setup the rangefinder */ -AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder::RangeFinder_State &_state) +AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder::RangeFinder_State &_state, + AP_HAL::OwnPtr dev) : AP_RangeFinder_Backend(_state) - , _dev(hal.i2c_mgr->get_device(1, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR)) + , _dev(std::move(dev)) { } @@ -46,10 +47,11 @@ AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder::RangeFin trying to take a reading on I2C. If we get a result the sensor is there. */ -AP_RangeFinder_Backend *AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder::RangeFinder_State &_state) +AP_RangeFinder_Backend *AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder::RangeFinder_State &_state, + AP_HAL::OwnPtr dev) { AP_RangeFinder_MaxsonarI2CXL *sensor - = new AP_RangeFinder_MaxsonarI2CXL(_state); + = new AP_RangeFinder_MaxsonarI2CXL(_state, std::move(dev)); if (!sensor) { return nullptr; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h index 35250c6c5e..dc34d0bd27 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h @@ -17,7 +17,8 @@ class AP_RangeFinder_MaxsonarI2CXL : public AP_RangeFinder_Backend { public: // static detection function - static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state); + static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state, + AP_HAL::OwnPtr dev); // update state void update(void); @@ -30,7 +31,8 @@ protected: private: // constructor - AP_RangeFinder_MaxsonarI2CXL(RangeFinder::RangeFinder_State &_state); + AP_RangeFinder_MaxsonarI2CXL(RangeFinder::RangeFinder_State &_state, + AP_HAL::OwnPtr dev); bool _init(void); void _timer(void); diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 35199f246d..e274e5cb07 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -614,7 +614,11 @@ void RangeFinder::detect_instance(uint8_t instance) } break; case RangeFinder_TYPE_MBI2C: - _add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(state[instance])); + if (!_add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(state[instance], + hal.i2c_mgr->get_device(1, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR)))) { + _add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(state[instance], + hal.i2c_mgr->get_device(0, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR))); + } break; case RangeFinder_TYPE_LWI2C: if (state[instance].address) {