diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp
new file mode 100644
index 0000000000..e2622cfb42
--- /dev/null
+++ b/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 .
+ */
+
+#include "AP_RangeFinder_LightWareI2C.h"
+#include
+
+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);
+ }
+}
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h
new file mode 100644
index 0000000000..27203e96e1
--- /dev/null
+++ b/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__
diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp
index 3fd1da2c6c..72e779fee6 100644
--- a/libraries/AP_RangeFinder/RangeFinder.cpp
+++ b/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)) {
diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h
index e1de23ff6b..9e103c1ec6 100644
--- a/libraries/AP_RangeFinder/RangeFinder.h
+++ b/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[];