Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: LightWareI2C: use I2CDevice interface

Browse Source
This commit is contained in:
Luiz Ywata 2016-05-24 15:51:54 -03:00 committed by Lucas De Marchi
parent a372f33cc0
commit 49d1520850
3 changed files with 33 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp
View File

@ -16,6 +16,7 @@
#include "AP_RangeFinder_LightWareI2C.h"
#include <AP_HAL/AP_HAL.h>
#include <utility>
extern const AP_HAL::HAL& hal;
@ -24,8 +25,9 @@ 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_LightWareI2C::AP_RangeFinder_LightWareI2C(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
AP_RangeFinder_Backend(_ranger, instance, _state)
AP_RangeFinder_LightWareI2C::AP_RangeFinder_LightWareI2C(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev) :
AP_RangeFinder_Backend(_ranger, instance, _state),
_dev(std::move(dev))
{
}
@ -34,31 +36,37 @@ AP_RangeFinder_LightWareI2C::AP_RangeFinder_LightWareI2C(RangeFinder &_ranger, u
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)
AP_RangeFinder_Backend *AP_RangeFinder_LightWareI2C::detect(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
{
uint8_t buff[2];
if (_ranger._address[instance] == 0) {
return false;
AP_RangeFinder_LightWareI2C *sensor
= new AP_RangeFinder_LightWareI2C(_ranger, instance, _state, std::move(dev));
uint16_t reading_cm;
if (!sensor->get_reading(reading_cm)) {
delete sensor;
return nullptr;
}
return hal.i2c->read(_ranger._address[instance], 2, &buff[0]) == 0;
return sensor;
}
// 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)) {
if (!_dev->get_semaphore()->take(1)) {
return false;
}
if (ranger._address[state.instance] == 0) {
return false;
}
uint8_t buff[2];
// read the high and low byte distance registers
if (hal.i2c->read(ranger._address[state.instance], 2, &buff[0]) != 0) {
i2c_sem->give();
if (!_dev->read(buff, sizeof(buff))) {
_dev->get_semaphore()->give();
return false;
}
@ -66,7 +74,7 @@ bool AP_RangeFinder_LightWareI2C::get_reading(uint16_t &reading_cm)
reading_cm = ((uint16_t)buff[0]) << 8 | buff[1];
// return semaphore
i2c_sem->give();
_dev->get_semaphore()->give();
return true;
}

10
libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h
View File

@ -3,21 +3,23 @@
#include "RangeFinder.h"
#include "RangeFinder_Backend.h"
#include <AP_HAL/I2CDevice.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);
static AP_RangeFinder_Backend *detect(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
// update state
void update(void);
private:
// constructor
AP_RangeFinder_LightWareI2C(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
// get a reading
bool get_reading(uint16_t &reading_cm);
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
};

7
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -502,10 +502,9 @@ void RangeFinder::detect_instance(uint8_t instance)
}
}
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 (_address[instance]) {
_add_backend(AP_RangeFinder_LightWareI2C::detect(*this, instance, state[instance],
hal.i2c_mgr->get_device(0, _address[instance])));
}
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN