AP_RangeFinder: MaxsonarI2CXL: use I2CDevice interface

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp

@@ -25,6 +25,7 @@
 
 #include "AP_RangeFinder_MaxsonarI2CXL.h"
 #include <AP_HAL/AP_HAL.h>
+#include <utility>
 
 extern const AP_HAL::HAL& hal;
 
@@ -34,7 +35,8 @@ extern const AP_HAL::HAL& hal;
    already know that we should setup the rangefinder
 */
 AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
-    AP_RangeFinder_Backend(_ranger, instance, _state)
+    AP_RangeFinder_Backend(_ranger, instance, _state),
+    _dev(hal.i2c_mgr->get_device(0, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR))
 {
 }
 
@@ -43,40 +45,43 @@ AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder &_ranger,
    trying to take a reading on I2C. If we get a result the sensor is
    there.
 */
-bool AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder &_ranger, uint8_t instance)
+AP_RangeFinder_Backend *AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder &_ranger, uint8_t instance,
+                                                             RangeFinder::RangeFinder_State &_state)
 {
-    if (!start_reading()) {
+    AP_RangeFinder_MaxsonarI2CXL *sensor
-        return false;
+        = new AP_RangeFinder_MaxsonarI2CXL(_ranger, instance, _state);
+    if (!sensor || !sensor->start_reading()) {
+        delete sensor;
+        return nullptr;
     }
     // give time for the sensor to process the request
     hal.scheduler->delay(50);
     uint16_t reading_cm;
-    return get_reading(reading_cm);
+    if (!sensor->get_reading(reading_cm)) {
+        delete sensor;
+        return nullptr;
+    }
+
+    return sensor;
 }
 
 // start_reading() - ask sensor to make a range reading
 bool AP_RangeFinder_MaxsonarI2CXL::start_reading()
 {
-    // get pointer to i2c bus semaphore
+    if (!_dev->get_semaphore()->take(1)) {
-    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)) {
         return false;
     }
 
-    uint8_t tosend[1] = 
+    uint8_t tosend[] = {AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING};
-        { AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING };
 
     // send command to take reading
-    if (hal.i2c->write(AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR,
+    if (!_dev->transfer(tosend, sizeof(tosend), nullptr, 0)) {
-                       1, tosend) != 0) {
+        _dev->get_semaphore()->give();
-        i2c_sem->give();
         return false;
     }
 
     // return semaphore
-    i2c_sem->give();
+    _dev->get_semaphore()->give();
 
     return true;
 }
@@ -84,22 +89,18 @@ bool AP_RangeFinder_MaxsonarI2CXL::start_reading()
 // read - return last value measured by sensor
 bool AP_RangeFinder_MaxsonarI2CXL::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;
     }
 
+    uint8_t buff[2];
     // take range reading and read back results
-    if (hal.i2c->read(AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR, 2, buff) != 0) {
+    if (!_dev->transfer(nullptr, 0, buff, sizeof(buff))) {
-        i2c_sem->give();
+        _dev->get_semaphore()->give();
         return false;
     }
-    i2c_sem->give();
+    _dev->get_semaphore()->give();
 
     // combine results into distance
     reading_cm = ((uint16_t)buff[0]) << 8 | buff[1];

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h

@@ -3,6 +3,7 @@
 
 #include "RangeFinder.h"
 #include "RangeFinder_Backend.h"
+#include <AP_HAL/I2CDevice.h>
 
 #define AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR   0x70
 
@@ -16,17 +17,20 @@
 class AP_RangeFinder_MaxsonarI2CXL : public AP_RangeFinder_Backend
 {
 public:
-    // constructor
-    AP_RangeFinder_MaxsonarI2CXL(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);
 
     // update state
     void update(void);
 
 private:
+    // constructor
+    AP_RangeFinder_MaxsonarI2CXL(RangeFinder &ranger, uint8_t instance,
+                                 RangeFinder::RangeFinder_State &_state);
+
     // start a reading
-    static bool start_reading(void);
+    bool start_reading(void);
-    static bool get_reading(uint16_t &reading_cm);
+    bool get_reading(uint16_t &reading_cm);
+    AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
 };

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -26,7 +26,7 @@
 #include "AP_RangeFinder_Bebop.h"
 #include "AP_RangeFinder_MAVLink.h"
 
-extern const AP_HAL::HAL& hal;
+extern const AP_HAL::HAL &hal;
 
 // table of user settable parameters
 const AP_Param::GroupInfo RangeFinder::var_info[] = {
@@ -495,18 +495,14 @@ void RangeFinder::detect_instance(uint8_t instance)
         _add_backend(AP_RangeFinder_PulsedLightLRF::detect(*this, instance, state[instance]));
     }
     if (type == RangeFinder_TYPE_MBI2C) {
-        if (AP_RangeFinder_MaxsonarI2CXL::detect(*this, instance)) {
+        _add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(*this, instance, state[instance]));
-            state[instance].instance = instance;
-            drivers[instance] = new AP_RangeFinder_MaxsonarI2CXL(*this, instance, state[instance]);
-            return;
-        }
     }
     if (type == RangeFinder_TYPE_LWI2C) {
         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
     if (type == RangeFinder_TYPE_PX4) {
         if (AP_RangeFinder_PX4::detect(*this, instance)) {