AP_RangeFinder: convert the PulsedLight driver to new API

not tested yet

libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp

@@ -1,4 +1,3 @@
-#if 0
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 /*
    This program is free software: you can redistribute it and/or modify
@@ -15,116 +14,107 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-/*
- *       AP_RangeFinder_PulsedLightLRF.cpp - Arduino Library for Pulsed Light's Laser Range Finder
- *       Code by Randy Mackay. DIYDrones.com
- *
- *       Sensor should be connected to the I2C port
- *
- *       Variables:
- *               bool healthy : indicates whether last communication with sensor was successful
- *
- *       Methods:
- *               take_reading(): ask the sonar to take a new distance measurement
- *               read() : read last distance measured (in cm)
- *
- */
-
 #include "AP_RangeFinder_PulsedLightLRF.h"
 #include <AP_HAL.h>
 
 extern const AP_HAL::HAL& hal;
 
-// Constructor //////////////////////////////////////////////////////////////
-
-AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(FilterInt16 *filter) :
-    RangeFinder(NULL, filter),
-    healthy(true),
-    _addr(AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR)
+/* 
+   The constructor also initialises the rangefinder. Note that this
+   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_Backend(_ranger, instance, _state)
 {
-    min_distance = AP_RANGEFINDER_PULSEDLIGHTLRF_MIN_DISTANCE;
-    max_distance = AP_RANGEFINDER_PULSEDLIGHTLRF_MAX_DISTANCE;
 }
 
-// Public Methods //////////////////////////////////////////////////////////////
-
-// init - simply sets the i2c address
-void AP_RangeFinder_PulsedLightLRF::init(uint8_t address)
+/* 
+   detect if a PulsedLight rangefinder is connected. We'll detect by
+   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)
 {
-    // set sensor i2c address
-    _addr = address;
+    if (!start_reading()) {
+        return false;
+    }
+    // give time for the sensor to process the request
+    hal.scheduler->delay(50);
+    uint16_t reading_cm;
+    return get_reading(reading_cm);
 }
 
-// take_reading - ask sensor to make a range reading
-bool AP_RangeFinder_PulsedLightLRF::take_reading()
+// start_reading() - ask sensor to make a range reading
+bool AP_RangeFinder_PulsedLightLRF::start_reading()
 {
     // 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(5)) {
-        healthy = false;
-        return healthy;
+    if (i2c_sem == NULL || !i2c_sem->take(1)) {
+        return false;
     }
 
     // send command to take reading
-    if (hal.i2c->writeRegister(_addr, AP_RANGEFINDER_PULSEDLIGHTLRF_MEASURE_REG, AP_RANGEFINDER_PULSEDLIGHTLRF_MSRREG_ACQUIRE) != 0) {
-        healthy = false;
-    }else{
-        healthy = true;
+    if (hal.i2c->writeRegister(AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR, 
+                               AP_RANGEFINDER_PULSEDLIGHTLRF_MEASURE_REG, 
+                               AP_RANGEFINDER_PULSEDLIGHTLRF_MSRREG_ACQUIRE) != 0) {
+        i2c_sem->give();
+        return false;
     }
 
-    hal.scheduler->delay_microseconds(200);
-    
     // return semaphore
     i2c_sem->give();
 
-    return healthy;
+    return true;
 }
 
 // read - return last value measured by sensor
-int16_t AP_RangeFinder_PulsedLightLRF::read()
+bool AP_RangeFinder_PulsedLightLRF::get_reading(uint16_t &reading_cm)
 {
     uint8_t buff[2];
-    int16_t ret_value = 0;
 
     // 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(5)) {
-        healthy = false;
-        return healthy;
+    if (i2c_sem == NULL || !i2c_sem->take(1)) {
+        return false;
     }
 
-    // assume the worst
-    healthy = false;
-
     // read the high byte
-    if (hal.i2c->readRegisters(_addr, AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG, 1, &buff[0]) == 0) {
-        hal.scheduler->delay_microseconds(200);
-        // read the low byte
-        if (hal.i2c->readRegisters(_addr, AP_RANGEFINDER_PULSEDLIGHTLRF_DISTLOW_REG, 1, &buff[1]) == 0) {
-            healthy = true;
-            // combine results into distance
-            ret_value = buff[0] << 8 | buff[1];
-        }
+    if (hal.i2c->readRegisters(AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR, 
+                               AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG, 1, &buff[0]) != 0) {
+        i2c_sem->give();
+        return false;
+    }
+    hal.scheduler->delay_microseconds(200);
+    // read the low byte
+    if (hal.i2c->readRegisters(AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR, 
+                               AP_RANGEFINDER_PULSEDLIGHTLRF_DISTLOW_REG, 1, &buff[1]) != 0) {
+        i2c_sem->give();
+        return false;
     }
 
-    // ensure distance is within min and max
-    ret_value = constrain_int16(ret_value, min_distance, max_distance);
-    ret_value = _mode_filter->apply(ret_value);
+    // combine results into distance
+    reading_cm = ((uint16_t)buff[0]) << 8 | buff[1];
 
-    hal.scheduler->delay_microseconds(200);
-    
     // return semaphore
     i2c_sem->give();
 
     // kick off another reading for next time
     // To-Do: replace this with continuous mode
-    take_reading();
+    hal.scheduler->delay_microseconds(200);
+    start_reading();
 
-    // to-do: do we really want to return 0 if reading the distance fails?
-    return ret_value;
+    return true;
+}
+
+/* 
+   update the state of the sensor
+*/
+void AP_RangeFinder_PulsedLightLRF::update(void)
+{
+    state.healthy = get_reading(state.distance_cm);
 }
-#endif

libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h

@@ -4,6 +4,7 @@
 #define __AP_RANGEFINDER_PULSEDLIGHTLRF_H__
 
 #include "RangeFinder.h"
+#include "RangeFinder_Backend.h"
 
 /* Connection diagram
  *
@@ -38,27 +39,23 @@
 // command register values
 #define AP_RANGEFINDER_PULSEDLIGHTLRF_MSRREG_ACQUIRE        0x04    // Varies based on sensor revision, 0x04 is newest, 0x61 is older
 
-class AP_RangeFinder_PulsedLightLRF : public RangeFinder
+class AP_RangeFinder_PulsedLightLRF : public AP_RangeFinder_Backend
 {
 
 public:
-
     // constructor
-    AP_RangeFinder_PulsedLightLRF(FilterInt16 *filter);
-
-    // init - simply sets the i2c address
-    void init(uint8_t address = AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR);
+    AP_RangeFinder_PulsedLightLRF(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state);
 
-    // take_reading - ask sensor to make a range reading
-    bool            take_reading();
+    // static detection function
+    static bool detect(RangeFinder &ranger, uint8_t instance);
 
-    // read value from sensor and return distance in cm
-    int16_t         read();
+    // update state
+    void update(void);
 
-    // heath
-    bool            healthy;
 
-protected:
-    uint8_t _addr;
+private:
+    // start a reading
+    static bool start_reading(void);
+    static bool get_reading(uint16_t &reading_cm);
 };
 #endif  // __AP_RANGEFINDER_PULSEDLIGHTLRF_H__

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -16,6 +16,7 @@
 
 #include "RangeFinder.h"
 #include "AP_RangeFinder_analog.h"
+#include "AP_RangeFinder_PulsedLightLRF.h"
 
 // table of user settable parameters
 const AP_Param::GroupInfo RangeFinder::var_info[] PROGMEM = {
@@ -197,7 +198,14 @@ void RangeFinder::update(void)
  */
 void RangeFinder::detect_instance(uint8_t instance)
 {
-    if (_type[instance] == RangeFinder_TYPE_AUTO || _type[instance] == RangeFinder_TYPE_ANALOG) {
+    if (_type[instance] == RangeFinder_TYPE_AUTO || _type[instance] == RangeFinder_TYPE_PLI2C) {
+        if (AP_RangeFinder_PulsedLightLRF::detect(*this, instance)) {
+            state[instance].instance = instance;
+            drivers[instance] = new AP_RangeFinder_PulsedLightLRF(*this, instance, state[instance]);
+        }
+    } else if (_type[instance] == RangeFinder_TYPE_AUTO || _type[instance] == RangeFinder_TYPE_ANALOG) {
+        // note that analog must be the last to be checked, as it will
+        // always come back as present if the pin is valid
         if (AP_RangeFinder_analog::detect(*this, instance)) {
             state[instance].instance = instance;
             drivers[instance] = new AP_RangeFinder_analog(*this, instance, state[instance]);