aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_RangeFinder: convert the PulsedLight driver to new API

Browse Source 
not tested yet
This commit is contained in:
Andrew Tridgell 2014-06-27 13:39:52 +10:00
parent 22b9059647
commit 0063d83dbc
3 changed files with 77 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

124
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
* 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 <AP_HAL.h>
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

25
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h
View File

@ -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__

10
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -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]);