AP_RangeFinder: convert MaxbotixI2C driver to new API

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp

@@ -21,71 +21,97 @@
  *       datasheet: http://www.maxbotix.com/documents/I2CXL-MaxSonar-EZ_Datasheet.pdf
  *
  *       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)
- *
  */
 
-#if 0
-// AVR LibC Includes
 #include "AP_RangeFinder_MaxsonarI2CXL.h"
 #include <AP_HAL.h>
 
 extern const AP_HAL::HAL& hal;
 
-// Constructor //////////////////////////////////////////////////////////////
-
-AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL( FilterInt16 *filter ) :
-    RangeFinder(NULL, filter),
-    healthy(true),
-    _addr(AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_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_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
+    AP_RangeFinder_Backend(_ranger, instance, _state)
 {
-    min_distance = AP_RANGE_FINDER_MAXSONARI2CXL_MIN_DISTANCE;
-    max_distance = AP_RANGE_FINDER_MAXSONARI2CXL_MAX_DISTANCE;
 }
 
-// Public Methods //////////////////////////////////////////////////////////////
-
-// take_reading - ask sensor to make a range reading
-bool AP_RangeFinder_MaxsonarI2CXL::take_reading()
+/* 
+   detect if a Maxbotix 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_MaxsonarI2CXL::detect(RangeFinder &_ranger, uint8_t instance)
 {
-    // take range reading and read back results
+    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);
+}
+
+// start_reading() - ask sensor to make a range reading
+bool AP_RangeFinder_MaxsonarI2CXL::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(1)) {
+        return false;
+    }
+
     uint8_t tosend[1] = 
         { AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING };
-    if (hal.i2c->write(_addr, 1, tosend) != 0) {
-        healthy = false;
+
+    // send command to take reading
+    if (hal.i2c->write(AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR,
+                       1, tosend) != 0) {
+        i2c_sem->give();
         return false;
-    }else{
-        healthy = true;
-        return true;
     }
+
+    // return semaphore
+    i2c_sem->give();
+
+    return true;
 }
 
 // read - return last value measured by sensor
-int16_t AP_RangeFinder_MaxsonarI2CXL::read()
+bool AP_RangeFinder_MaxsonarI2CXL::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(1)) {
+        return false;
+    }
 
     // take range reading and read back results
-    if (hal.i2c->read(_addr, 2, buff) != 0) {
-        healthy = false;
-    }else{
-        // combine results into distance
-        ret_value = buff[0] << 8 | buff[1];
-        healthy = true;
+    if (hal.i2c->read(AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR, 2, buff) != 0) {
+        i2c_sem->give();
+        return false;
     }
-    
-    // ensure distance is within min and max
-    ret_value = constrain_float(ret_value, min_distance, max_distance);
-    
-    ret_value = _mode_filter->apply(ret_value);
-    
-    return ret_value;
+    i2c_sem->give();
+
+    // combine results into distance
+    reading_cm = ((uint16_t)buff[0]) << 8 | buff[1];
+
+    return true;
+}
+
+
+/* 
+   update the state of the sensor
+*/
+void AP_RangeFinder_MaxsonarI2CXL::update(void)
+{
+    state.healthy = get_reading(state.distance_cm);
 }
-#endif

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h

@@ -4,6 +4,7 @@
 #define __AP_RANGEFINDER_MAXSONARI2CXL_H__
 
 #include "RangeFinder.h"
+#include "RangeFinder_Backend.h"
 
 #define AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR   0x70
 
@@ -14,28 +15,21 @@
 
 #define AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING 0x51
 
-class AP_RangeFinder_MaxsonarI2CXL : public RangeFinder
+class AP_RangeFinder_MaxsonarI2CXL : public AP_RangeFinder_Backend
 {
-
 public:
-
     // constructor
-    AP_RangeFinder_MaxsonarI2CXL(FilterInt16 *filter);
+    AP_RangeFinder_MaxsonarI2CXL(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state);
 
-    // init - simply sets the i2c address
-    void init(uint8_t address = AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR) { _addr = address; }
+    // static detection function
+    static bool detect(RangeFinder &ranger, uint8_t instance);
 
-    // take_reading - ask sensor to make a range reading
-    bool            take_reading();
-
-    // read value from sensor and return distance in cm
-    int16_t         read();
-
-    // heath
-    bool            healthy;
-
-protected:
-    uint8_t _addr;
+    // update state
+    void update(void);
 
+private:
+    // start a reading
+    static bool start_reading(void);
+    static bool get_reading(uint16_t &reading_cm);
 };
 #endif  // __AP_RANGEFINDER_MAXSONARI2CXL_H__

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -17,6 +17,7 @@
 #include "RangeFinder.h"
 #include "AP_RangeFinder_analog.h"
 #include "AP_RangeFinder_PulsedLightLRF.h"
+#include "AP_RangeFinder_MaxsonarI2CXL.h"
 
 // table of user settable parameters
 const AP_Param::GroupInfo RangeFinder::var_info[] PROGMEM = {
@@ -203,6 +204,11 @@ void RangeFinder::detect_instance(uint8_t 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_MBI2C) {
+        if (AP_RangeFinder_MaxsonarI2CXL::detect(*this, instance)) {
+            state[instance].instance = instance;
+            drivers[instance] = new AP_RangeFinder_MaxsonarI2CXL(*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