diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp
index 56934b148a..b9a929860f 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp
+++ b/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 .
*/
-/*
- * 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
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
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h
index b242bbfb8b..171e3d1bca 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h
+++ b/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);
+ AP_RangeFinder_PulsedLightLRF(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state);
- // init - simply sets the i2c address
- void init(uint8_t address = AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR);
+ // static detection function
+ static bool detect(RangeFinder &ranger, uint8_t instance);
- // take_reading - ask sensor to make a range reading
- bool take_reading();
+ // update state
+ void update(void);
- // read value from sensor and return distance in cm
- int16_t read();
- // 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__
diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp
index ddcd9519ed..a9c85fa4b1 100644
--- a/libraries/AP_RangeFinder/RangeFinder.cpp
+++ b/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]);