From eb56a010b3e646f2a092aa8afc2a620cacacc6d8 Mon Sep 17 00:00:00 2001 From: Mateusz Sadowski Date: Fri, 28 Jul 2017 14:20:10 +0200 Subject: [PATCH] AP_RangeFinder: TeraRangerI2C: use address from parameter instead of hardcoded --- .../AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp | 13 ++++++------- .../AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.h | 7 ++++--- libraries/AP_RangeFinder/RangeFinder.cpp | 8 ++++++-- 3 files changed, 16 insertions(+), 12 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp index 9d297b2850..2145f8f463 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp @@ -23,8 +23,6 @@ extern const AP_HAL::HAL& hal; -#define TRONE_I2C_ADDR 0x30 - // registers #define TR_MEASURE 0x00 #define TR_WHOAMI 0x01 @@ -35,9 +33,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_TeraRangerI2C::AP_RangeFinder_TeraRangerI2C(uint8_t bus, RangeFinder::RangeFinder_State &_state) +AP_RangeFinder_TeraRangerI2C::AP_RangeFinder_TeraRangerI2C(RangeFinder::RangeFinder_State &_state, + AP_HAL::OwnPtr i2c_dev) : AP_RangeFinder_Backend(_state) - , dev(hal.i2c_mgr->get_device(bus, TRONE_I2C_ADDR)) + , dev(std::move(i2c_dev)) { } @@ -46,10 +45,10 @@ AP_RangeFinder_TeraRangerI2C::AP_RangeFinder_TeraRangerI2C(uint8_t bus, RangeFin trying to take a reading on I2C. If we get a result the sensor is there. */ -AP_RangeFinder_Backend *AP_RangeFinder_TeraRangerI2C::detect(uint8_t bus, - RangeFinder::RangeFinder_State &_state) +AP_RangeFinder_Backend *AP_RangeFinder_TeraRangerI2C::detect(RangeFinder::RangeFinder_State &_state, + AP_HAL::OwnPtr i2c_dev) { - AP_RangeFinder_TeraRangerI2C *sensor = new AP_RangeFinder_TeraRangerI2C(bus, _state); + AP_RangeFinder_TeraRangerI2C *sensor = new AP_RangeFinder_TeraRangerI2C(_state, std::move(i2c_dev)); if (!sensor) { return nullptr; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.h b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.h index 39b12addea..987875b7f8 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.h @@ -8,8 +8,8 @@ class AP_RangeFinder_TeraRangerI2C : public AP_RangeFinder_Backend { public: // static detection function - static AP_RangeFinder_Backend *detect(uint8_t bus, - RangeFinder::RangeFinder_State &_state); + static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state, + AP_HAL::OwnPtr i2c_dev); // update state void update(void); @@ -22,7 +22,8 @@ protected: private: // constructor - AP_RangeFinder_TeraRangerI2C(uint8_t bus, RangeFinder::RangeFinder_State &_state); + AP_RangeFinder_TeraRangerI2C(RangeFinder::RangeFinder_State &_state, + AP_HAL::OwnPtr i2c_dev); bool measure(void); bool collect(uint16_t &distance_cm); diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 3cd1d31950..7530615327 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -623,8 +623,12 @@ void RangeFinder::detect_instance(uint8_t instance) } break; case RangeFinder_TYPE_TRI2C: - if (!_add_backend(AP_RangeFinder_TeraRangerI2C::detect(0, state[instance]))) { - _add_backend(AP_RangeFinder_TeraRangerI2C::detect(1, state[instance])); + if (state[instance].address) { + if (!_add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance], + hal.i2c_mgr->get_device(1, state[instance].address)))) { + _add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance], + hal.i2c_mgr->get_device(0, state[instance].address))); + } } break; case RangeFinder_TYPE_VL53L0X: