5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 23:48:31 -04:00

AP_RangeFinder: MaxsonarI2CXL: use I2CDevice interface

This commit is contained in:
Luiz Ywata 2016-04-14 13:42:06 -03:00 committed by Lucas De Marchi
parent ae259bc563
commit c167364fa0
3 changed files with 39 additions and 38 deletions

View File

@ -25,6 +25,7 @@
#include "AP_RangeFinder_MaxsonarI2CXL.h"
#include <AP_HAL/AP_HAL.h>
#include <utility>
extern const AP_HAL::HAL& hal;
@ -34,7 +35,8 @@ extern const AP_HAL::HAL& hal;
already know that we should setup the rangefinder
*/
AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
AP_RangeFinder_Backend(_ranger, instance, _state)
AP_RangeFinder_Backend(_ranger, instance, _state),
_dev(hal.i2c_mgr->get_device(0, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR))
{
}
@ -43,40 +45,43 @@ AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder &_ranger,
trying to take a reading on I2C. If we get a result the sensor is
there.
*/
bool AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder &_ranger, uint8_t instance)
AP_RangeFinder_Backend *AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder &_ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state)
{
if (!start_reading()) {
return false;
AP_RangeFinder_MaxsonarI2CXL *sensor
= new AP_RangeFinder_MaxsonarI2CXL(_ranger, instance, _state);
if (!sensor || !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_MaxsonarI2CXL::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;
}
uint8_t tosend[1] =
{ AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING };
uint8_t tosend[] = {AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING};
// send command to take reading
if (hal.i2c->write(AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR,
1, tosend) != 0) {
i2c_sem->give();
if (!_dev->transfer(tosend, sizeof(tosend), nullptr, 0)) {
_dev->get_semaphore()->give();
return false;
}
// return semaphore
i2c_sem->give();
_dev->get_semaphore()->give();
return true;
}
@ -84,22 +89,18 @@ bool AP_RangeFinder_MaxsonarI2CXL::start_reading()
// read - return last value measured by sensor
bool AP_RangeFinder_MaxsonarI2CXL::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];
// take range reading and read back results
if (hal.i2c->read(AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR, 2, buff) != 0) {
i2c_sem->give();
if (!_dev->transfer(nullptr, 0, buff, sizeof(buff))) {
_dev->get_semaphore()->give();
return false;
}
i2c_sem->give();
_dev->get_semaphore()->give();
// combine results into distance
reading_cm = ((uint16_t)buff[0]) << 8 | buff[1];

View File

@ -3,6 +3,7 @@
#include "RangeFinder.h"
#include "RangeFinder_Backend.h"
#include <AP_HAL/I2CDevice.h>
#define AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR 0x70
@ -16,17 +17,20 @@
class AP_RangeFinder_MaxsonarI2CXL : public AP_RangeFinder_Backend
{
public:
// constructor
AP_RangeFinder_MaxsonarI2CXL(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_MaxsonarI2CXL(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;
};

View File

@ -26,7 +26,7 @@
#include "AP_RangeFinder_Bebop.h"
#include "AP_RangeFinder_MAVLink.h"
extern const AP_HAL::HAL& hal;
extern const AP_HAL::HAL &hal;
// table of user settable parameters
const AP_Param::GroupInfo RangeFinder::var_info[] = {
@ -495,18 +495,14 @@ void RangeFinder::detect_instance(uint8_t instance)
_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;
drivers[instance] = new AP_RangeFinder_MaxsonarI2CXL(*this, instance, state[instance]);
return;
}
_add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(*this, instance, state[instance]));
}
if (type == RangeFinder_TYPE_LWI2C) {
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
if (type == RangeFinder_TYPE_PX4) {
if (AP_RangeFinder_PX4::detect(*this, instance)) {