From dff235dc36a607ae810c093fe096ad9fe485e30a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 28 Aug 2015 19:52:34 +1000 Subject: [PATCH] AP_RangeFinder: support LightWare I2C laser rangefinders this has been tested with the SF10/C --- .../AP_RangeFinder_LightWareI2C.cpp | 85 +++++++++++++++++++ .../AP_RangeFinder_LightWareI2C.h | 26 ++++++ libraries/AP_RangeFinder/RangeFinder.cpp | 26 ++++++ libraries/AP_RangeFinder/RangeFinder.h | 4 +- 4 files changed, 140 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h 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[];