AP_RangeFinder: support LightWare I2C laser rangefinders

this has been tested with the SF10/C

libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp

@@ -0,0 +1,85 @@
+// -*- 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
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "AP_RangeFinder_LightWareI2C.h"
+#include <AP_HAL/AP_HAL.h>
+
+extern const AP_HAL::HAL& hal;
+
+/* 
+   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_LightWareI2C::AP_RangeFinder_LightWareI2C(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
+    AP_RangeFinder_Backend(_ranger, instance, _state)
+{
+}
+
+/* 
+   detect if a Lightware 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_LightWareI2C::detect(RangeFinder &_ranger, uint8_t instance)
+{
+    uint8_t buff[2];
+    if (_ranger._address[instance] == 0) {
+        return false;
+    }
+    return hal.i2c->read(_ranger._address[instance], 2, &buff[0]) == 0;
+}
+
+// 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)) {
+        return false;
+    }
+
+    // read the high and low byte distance registers
+    if (hal.i2c->read(ranger._address[state.instance], 2, &buff[0]) != 0) {
+        i2c_sem->give();
+        return false;
+    }
+
+    // combine results into distance
+    reading_cm = ((uint16_t)buff[0]) << 8 | buff[1];
+
+    // return semaphore
+    i2c_sem->give();
+
+    return true;
+}
+
+/* 
+   update the state of the sensor
+*/
+void AP_RangeFinder_LightWareI2C::update(void)
+{
+    if (get_reading(state.distance_cm)) {
+        // update range_valid state based on distance measured
+        update_status();
+    } else {
+        set_status(RangeFinder::RangeFinder_NoData);
+    }
+}

libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h

@@ -0,0 +1,26 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef __AP_RANGEFINDER_LIGHTWARELRF_H__
+#define __AP_RANGEFINDER_LIGHTWARELRF_H__
+
+#include "RangeFinder.h"
+#include "RangeFinder_Backend.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);
+
+    // update state
+    void update(void);
+
+private:
+    // get a reading
+    bool get_reading(uint16_t &reading_cm);
+};
+#endif  // __AP_RANGEFINDER_LIGHTWARELRF_H__

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -21,6 +21,7 @@
 #include "AP_RangeFinder_PX4.h"
 #include "AP_RangeFinder_PX4_PWM.h"
 #include "AP_RangeFinder_BBB_PRU.h"
+#include "AP_RangeFinder_LightWareI2C.h"
 
 // table of user settable parameters
 const AP_Param::GroupInfo RangeFinder::var_info[] PROGMEM = {
@@ -183,6 +184,24 @@ const AP_Param::GroupInfo RangeFinder::var_info[] PROGMEM = {
     AP_GROUPINFO("2_GNDCLEAR", 22, RangeFinder, _ground_clearance_cm[1], RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT),
 #endif
 
+    // @Param: _ADDR
+    // @DisplayName: Bus address of sensor
+    // @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor to allow for multiple sensors on different addresses. A value of 0 disables the sensor.
+    // @Range: 0 127
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("_ADDR", 23, RangeFinder, _address[0], 0),
+
+#if RANGEFINDER_MAX_INSTANCES > 1
+    // @Param: 2_ADDR
+    // @DisplayName: Bus address of 2nd rangefinder
+    // @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor to allow for multiple sensors on different addresses. A value of 0 disables the sensor.
+    // @Range: 0 127
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("2_ADDR", 24, RangeFinder, _address[1], 0),
+#endif
+    
     AP_GROUPEND
 };
 
@@ -281,6 +300,13 @@ void RangeFinder::detect_instance(uint8_t instance)
             return;
         }
     }
+    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 CONFIG_HAL_BOARD == HAL_BOARD_PX4  || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
     if (type == RangeFinder_TYPE_PX4) {
         if (AP_RangeFinder_PX4::detect(*this, instance)) {

libraries/AP_RangeFinder/RangeFinder.h

@@ -45,7 +45,8 @@ public:
         RangeFinder_TYPE_PLI2C  = 3,
         RangeFinder_TYPE_PX4    = 4,
         RangeFinder_TYPE_PX4_PWM= 5,
-        RangeFinder_TYPE_BBB_PRU= 6
+        RangeFinder_TYPE_BBB_PRU= 6,
+        RangeFinder_TYPE_LWI2C  = 7
     };
 
     enum RangeFinder_Function {
@@ -87,6 +88,7 @@ public:
     AP_Int16 _min_distance_cm[RANGEFINDER_MAX_INSTANCES];
     AP_Int16 _max_distance_cm[RANGEFINDER_MAX_INSTANCES];
     AP_Int8  _ground_clearance_cm[RANGEFINDER_MAX_INSTANCES];
+    AP_Int8  _address[RANGEFINDER_MAX_INSTANCES];
     AP_Int16 _powersave_range;
 
     static const struct AP_Param::GroupInfo var_info[];