AP_RangeFinder: TeraRangerI2C: use address from parameter instead of hardcoded

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<AP_HAL::I2CDevice> 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<AP_HAL::I2CDevice> 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;
     }

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<AP_HAL::I2CDevice> 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<AP_HAL::I2CDevice> i2c_dev);
 
     bool measure(void);
     bool collect(uint16_t &distance_cm);

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: