AP_RangeFinder: PulsedLightLRF: use I2CDevice interface

libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp

@@ -16,6 +16,7 @@
 
 #include "AP_RangeFinder_PulsedLightLRF.h"
 #include <AP_HAL/AP_HAL.h>
+#include <utility>
 
 extern const AP_HAL::HAL& hal;
 
@@ -24,8 +25,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_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
+AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(RangeFinder &_ranger, uint8_t instance,
-    AP_RangeFinder_Backend(_ranger, instance, _state)
+                                                             RangeFinder::RangeFinder_State &_state) :
+    AP_RangeFinder_Backend(_ranger, instance, _state),
+    _dev(hal.i2c_mgr->get_device(0, AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR))
 {
 }
 
@@ -34,38 +37,44 @@ AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(RangeFinder &_range
    trying to take a reading on I2C. If we get a result the sensor is
    there.
 */
-bool AP_RangeFinder_PulsedLightLRF::detect(RangeFinder &_ranger, uint8_t instance)
+AP_RangeFinder_Backend *AP_RangeFinder_PulsedLightLRF::detect(RangeFinder &_ranger, uint8_t instance,
+                                                              RangeFinder::RangeFinder_State &_state)
 {
-    if (!start_reading()) {
+    AP_RangeFinder_PulsedLightLRF *sensor
-        return false;
+        = new AP_RangeFinder_PulsedLightLRF(_ranger, instance, _state);
+
+    if (!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_PulsedLightLRF::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;
     }
 
     // send command to take reading
-    if (hal.i2c->writeRegister(AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR, 
+    if (!_dev->write_register(AP_RANGEFINDER_PULSEDLIGHTLRF_MEASURE_REG,
-                               AP_RANGEFINDER_PULSEDLIGHTLRF_MEASURE_REG, 
+                              AP_RANGEFINDER_PULSEDLIGHTLRF_MSRREG_ACQUIRE)) {
-                               AP_RANGEFINDER_PULSEDLIGHTLRF_MSRREG_ACQUIRE) != 0) {
+        _dev->get_semaphore()->give();
-        i2c_sem->give();
         return false;
     }
 
     // return semaphore
-    i2c_sem->give();
+    _dev->get_semaphore()->give();
 
     return true;
 }
@@ -73,20 +82,14 @@ bool AP_RangeFinder_PulsedLightLRF::start_reading()
 // read - return last value measured by sensor
 bool AP_RangeFinder_PulsedLightLRF::get_reading(uint16_t &reading_cm)
 {
-    uint8_t buff[2];
+    if (_dev->get_semaphore()->take(1)) {
-
-    // 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)) {
         return false;
     }
 
+    uint8_t buff[2];
     // read the high and low byte distance registers
-    if (hal.i2c->readRegisters(AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR, 
+    if (!_dev->read_registers(AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG, buff, sizeof(buff))) {
-                               AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG, 2, &buff[0]) != 0) {
+        _dev->get_semaphore()->give();
-        i2c_sem->give();
         return false;
     }
 
@@ -94,7 +97,7 @@ bool AP_RangeFinder_PulsedLightLRF::get_reading(uint16_t &reading_cm)
     reading_cm = ((uint16_t)buff[0]) << 8 | buff[1];
 
     // return semaphore
-    i2c_sem->give();
+    _dev->get_semaphore()->give();
 
     // kick off another reading for next time
     // To-Do: replace this with continuous mode

libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h

@@ -3,6 +3,7 @@
 
 #include "RangeFinder.h"
 #include "RangeFinder_Backend.h"
+#include <AP_HAL/I2CDevice.h>
 
 /* Connection diagram
  *
@@ -32,18 +33,21 @@ class AP_RangeFinder_PulsedLightLRF : public AP_RangeFinder_Backend
 {
 
 public:
-    // constructor
-    AP_RangeFinder_PulsedLightLRF(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_PulsedLightLRF(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,6 +26,8 @@
 #include "AP_RangeFinder_Bebop.h"
 #include "AP_RangeFinder_MAVLink.h"
 
+extern const AP_HAL::HAL& hal;
+
 // table of user settable parameters
 const AP_Param::GroupInfo RangeFinder::var_info[] = {
     // @Param: _TYPE
@@ -490,12 +492,8 @@ void RangeFinder::detect_instance(uint8_t instance)
     }
 #endif
     if (type == RangeFinder_TYPE_PLI2C) {
-        if (AP_RangeFinder_PulsedLightLRF::detect(*this, instance)) {
+        _add_backend(AP_RangeFinder_PulsedLightLRF::detect(*this, instance, state[instance]));
-            state[instance].instance = instance;
+    }
-            drivers[instance] = new AP_RangeFinder_PulsedLightLRF(*this, instance, state[instance]);
-            return;
-        }
-    } 
     if (type == RangeFinder_TYPE_MBI2C) {
         if (AP_RangeFinder_MaxsonarI2CXL::detect(*this, instance)) {
             state[instance].instance = instance;