AP_RangeFinder: LightWareI2C: use I2CDevice interface

libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp

@@ -16,6 +16,7 @@
 
 #include "AP_RangeFinder_LightWareI2C.h"
 #include <AP_HAL/AP_HAL.h>
+#include <utility>
 
 extern const AP_HAL::HAL& hal;
 
@@ -24,8 +25,9 @@ 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_LightWareI2C::AP_RangeFinder_LightWareI2C(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
-    AP_RangeFinder_Backend(_ranger, instance, _state)
+AP_RangeFinder_LightWareI2C::AP_RangeFinder_LightWareI2C(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev) :
+    AP_RangeFinder_Backend(_ranger, instance, _state),
+    _dev(std::move(dev))
 {
 }
 
@@ -34,31 +36,37 @@ AP_RangeFinder_LightWareI2C::AP_RangeFinder_LightWareI2C(RangeFinder &_ranger, u
    trying to take a reading on I2C. If we get a result the sensor is
    there.
 */
-bool AP_RangeFinder_LightWareI2C::detect(RangeFinder &_ranger, uint8_t instance)
+AP_RangeFinder_Backend *AP_RangeFinder_LightWareI2C::detect(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
 {
-    uint8_t buff[2];
-    if (_ranger._address[instance] == 0) {
-        return false;
+    AP_RangeFinder_LightWareI2C *sensor
+        = new AP_RangeFinder_LightWareI2C(_ranger, instance, _state, std::move(dev));
+
+    uint16_t reading_cm;
+
+    if (!sensor->get_reading(reading_cm)) {
+        delete sensor;
+        return nullptr;
     }
-    return hal.i2c->read(_ranger._address[instance], 2, &buff[0]) == 0;
+
+    return sensor;
 }
 
 // read - return last value measured by sensor
 bool AP_RangeFinder_LightWareI2C::get_reading(uint16_t &reading_cm)
 {
-    uint8_t buff[2];
-
-    // get pointer to i2c bus semaphore
-    AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
-
     // exit immediately if we can't take the semaphore
-    if (i2c_sem == NULL || !i2c_sem->take(1)) {
+    if (!_dev->get_semaphore()->take(1)) {
+        return false;
+    }
+    
+    if (ranger._address[state.instance] == 0) {
         return false;
     }
 
+    uint8_t buff[2];
     // read the high and low byte distance registers
-    if (hal.i2c->read(ranger._address[state.instance], 2, &buff[0]) != 0) {
-        i2c_sem->give();
+    if (!_dev->read(buff, sizeof(buff))) {
+        _dev->get_semaphore()->give();
         return false;
     }
 
@@ -66,7 +74,7 @@ bool AP_RangeFinder_LightWareI2C::get_reading(uint16_t &reading_cm)
     reading_cm = ((uint16_t)buff[0]) << 8 | buff[1];
 
     // return semaphore
-    i2c_sem->give();
+    _dev->get_semaphore()->give();
 
     return true;
 }

libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h

@@ -3,21 +3,23 @@
 
 #include "RangeFinder.h"
 #include "RangeFinder_Backend.h"
+#include <AP_HAL/I2CDevice.h>
 
 class AP_RangeFinder_LightWareI2C : public AP_RangeFinder_Backend
 {
 
 public:
-    // constructor
-    AP_RangeFinder_LightWareI2C(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state);
-
     // static detection function
-    static bool detect(RangeFinder &ranger, uint8_t instance);
+    static AP_RangeFinder_Backend *detect(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
 
     // update state
     void update(void);
 
 private:
+    // constructor
+    AP_RangeFinder_LightWareI2C(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
+
     // get a reading
     bool get_reading(uint16_t &reading_cm);
+    AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
 };

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -502,10 +502,9 @@ void RangeFinder::detect_instance(uint8_t instance)
         }
     }
     if (type == RangeFinder_TYPE_LWI2C) {
-        if (AP_RangeFinder_LightWareI2C::detect(*this, instance)) {
-            state[instance].instance = instance;
-            drivers[instance] = new AP_RangeFinder_LightWareI2C(*this, instance, state[instance]);
-            return;
+        if (_address[instance]) {
+            _add_backend(AP_RangeFinder_LightWareI2C::detect(*this, instance, state[instance],
+                hal.i2c_mgr->get_device(0, _address[instance])));
         }
     } 
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4  || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN