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:
parent
ae259bc563
commit
c167364fa0
libraries/AP_RangeFinder
@ -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];
|
||||
|
@ -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;
|
||||
};
|
||||
|
@ -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)) {
|
||||
|
Loading…
Reference in New Issue
Block a user