Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: PulsedLightLRF: use I2CDevice interface

Browse Source
This commit is contained in:
Luiz Ywata 2016-04-14 17:00:38 -03:00 committed by Lucas De Marchi
parent a7fddc0594
commit e1342eb533
3 changed files with 44 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp
View File

@ -16,6 +16,7 @@
#include "AP_RangeFinder_PulsedLightLRF.h"
#include <AP_HAL/AP_HAL.h>
#include <utility>
extern const AP_HAL::HAL& hal;
@ -24,8 +25,10 @@ extern const AP_HAL::HAL& hal;
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)
AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(RangeFinder &_ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state) :
AP_RangeFinder_Backend(_ranger, instance, _state),
_dev(hal.i2c_mgr->get_device(0, AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR))
{
}
@ -34,38 +37,44 @@ AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(RangeFinder &_range
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)
AP_RangeFinder_Backend *AP_RangeFinder_PulsedLightLRF::detect(RangeFinder &_ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state)
{
if (!start_reading()) {
return false;
AP_RangeFinder_PulsedLightLRF *sensor
= new AP_RangeFinder_PulsedLightLRF(_ranger, instance, _state);
if (!sensor->start_reading()) {
delete sensor;
return nullptr;
}
// give time for the sensor to process the request
hal.scheduler->delay(50);
uint16_t reading_cm;
return get_reading(reading_cm);
if (!sensor->get_reading(reading_cm)) {
delete sensor;
return nullptr;
}
return sensor;
}
// 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(1)) {
if (!_dev->get_semaphore()->take(1)) {
return false;
}
// send command to take reading
if (hal.i2c->writeRegister(AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR,
AP_RANGEFINDER_PULSEDLIGHTLRF_MEASURE_REG,
AP_RANGEFINDER_PULSEDLIGHTLRF_MSRREG_ACQUIRE) != 0) {
i2c_sem->give();
if (!_dev->write_register(AP_RANGEFINDER_PULSEDLIGHTLRF_MEASURE_REG,
AP_RANGEFINDER_PULSEDLIGHTLRF_MSRREG_ACQUIRE)) {
_dev->get_semaphore()->give();
return false;
}
// return semaphore
i2c_sem->give();
_dev->get_semaphore()->give();
return true;
}
@ -73,20 +82,14 @@ bool AP_RangeFinder_PulsedLightLRF::start_reading()
// read - return last value measured by sensor
bool AP_RangeFinder_PulsedLightLRF::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)) {
if (_dev->get_semaphore()->take(1)) {
return false;
}
uint8_t buff[2];
// read the high and low byte distance registers
if (hal.i2c->readRegisters(AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR,
AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG, 2, &buff[0]) != 0) {
i2c_sem->give();
if (!_dev->read_registers(AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG, buff, sizeof(buff))) {
_dev->get_semaphore()->give();
return false;
}
@ -94,7 +97,7 @@ bool AP_RangeFinder_PulsedLightLRF::get_reading(uint16_t &reading_cm)
reading_cm = ((uint16_t)buff[0]) << 8 | buff[1];
// return semaphore
i2c_sem->give();
_dev->get_semaphore()->give();
// kick off another reading for next time
// To-Do: replace this with continuous mode

16
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h
View File

@ -3,6 +3,7 @@
#include "RangeFinder.h"
#include "RangeFinder_Backend.h"
#include <AP_HAL/I2CDevice.h>
/* Connection diagram
*
@ -32,18 +33,21 @@ class AP_RangeFinder_PulsedLightLRF : public AP_RangeFinder_Backend
{
public:
// constructor
AP_RangeFinder_PulsedLightLRF(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state);
// static detection function
static bool detect(RangeFinder &ranger, uint8_t instance);
static AP_RangeFinder_Backend *detect(RangeFinder &ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state);
// update state
void update(void);
private:
// constructor
AP_RangeFinder_PulsedLightLRF(RangeFinder &ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state);
// start a reading
static bool start_reading(void);
static bool get_reading(uint16_t &reading_cm);
bool start_reading(void);
bool get_reading(uint16_t &reading_cm);
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
};

10
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -26,6 +26,8 @@
#include "AP_RangeFinder_Bebop.h"
#include "AP_RangeFinder_MAVLink.h"
extern const AP_HAL::HAL& hal;
// table of user settable parameters
const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @Param: _TYPE
@ -490,12 +492,8 @@ void RangeFinder::detect_instance(uint8_t instance)
}
#endif
if (type == RangeFinder_TYPE_PLI2C) {
if (AP_RangeFinder_PulsedLightLRF::detect(*this, instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_RangeFinder_PulsedLightLRF(*this, instance, state[instance]);
return;
}
}
_add_backend(AP_RangeFinder_PulsedLightLRF::detect(*this, instance, state[instance]));
}
if (type == RangeFinder_TYPE_MBI2C) {
if (AP_RangeFinder_MaxsonarI2CXL::detect(*this, instance)) {
state[instance].instance = instance;