5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00

AP_RangeFinder: convert PX4 driver to new API

This commit is contained in:
Andrew Tridgell 2014-06-27 15:02:51 +10:00
parent a4b9b989b7
commit 05d4f5fb5a
3 changed files with 90 additions and 99 deletions

View File

@ -31,107 +31,92 @@
extern const AP_HAL::HAL& hal;
// Constructor ////////////////////////////////////////////////////////
AP_RangeFinder_PX4::AP_RangeFinder_PX4(FilterInt16 *filter) :
RangeFinder(NULL, filter),
_num_instances(0){ }
uint8_t AP_RangeFinder_PX4::num_px4_instances = 0;
bool AP_RangeFinder_PX4::init(void)
/*
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_PX4::AP_RangeFinder_PX4(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
AP_RangeFinder_Backend(_ranger, instance, _state)
{
_range_fd[0] = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
if (_range_fd[0] < 0) {
hal.console->printf("Unable to open " RANGE_FINDER_DEVICE_PATH "\n");
return false;
_fd = open_driver();
// consider this path used up
num_px4_instances++;
if (_fd == -1) {
hal.console->printf("Unable to open PX4 rangefinder %u\n", num_px4_instances);
state.healthy = false;
return;
}
_range_fd[1] = open(RANGE_FINDER_DEVICE_PATH "1", O_RDONLY);
if (_range_fd[1] >= 0) {
_num_instances = 2;
} else {
_num_instances = 1;
// average over up to 20 samples
if (ioctl(_fd, SENSORIOCSQUEUEDEPTH, 20) != 0) {
hal.console->printf("Failed to setup range finder queue\n");
state.healthy = false;
return;
}
for (uint8_t i=0; i<_num_instances; i++) {
// average over up to 20 samples
if (ioctl(_range_fd[i], SENSORIOCSQUEUEDEPTH, 20) != 0) {
hal.console->printf("Failed to setup range finder queue\n");
return false;
}
state.healthy = true;
}
_count[0] = 0;
_sum[i] = 0;
_healthy[i] = false;
/*
open the PX4 driver, returning the file descriptor
*/
int AP_RangeFinder_PX4::open_driver(void)
{
// work out the device path based on how many PX4 drivers we have loaded
char path[] = RANGE_FINDER_DEVICE_PATH "n";
if (num_px4_instances == 0) {
path[strlen(path)-1] = 0;
} else {
path[strlen(path)-1] = '1' + (num_px4_instances-1);
}
return open(path, O_RDONLY);
}
// give the driver a chance to run, and gather one sample
hal.scheduler->delay(40);
accumulate();
if (_count[0] == 0) {
hal.console->printf("Failed initial range finder accumulate\n");
/*
see if the PX4 driver is available
*/
bool AP_RangeFinder_PX4::detect(RangeFinder &_ranger, uint8_t instance)
{
int fd = open_driver();
if (fd == -1) {
return false;
}
close(fd);
return true;
}
bool AP_RangeFinder_PX4::take_reading(void)
void AP_RangeFinder_PX4::update(void)
{
// try to accumulate one more sample, so we have the latest data
accumulate();
// consider the range finder healthy if we got a reading in the last 0.2s
for (uint8_t i=0; i<_num_instances; i++) {
_healthy[i] = (hrt_absolute_time() - _last_timestamp[i] < 200000);
if (_fd == -1) {
state.healthy = false;
return;
}
for (uint8_t i=0; i<_num_instances; i++) {
// avoid division by zero if we haven't received any range reports
if (_count[i] == 0) continue;
_sum[i] /= _count[i];
_sum[i] *= 100.00f;
if (_mode_filter) {
_distance[i] = _mode_filter->apply(_sum[i]);
}
else {
_distance[i] = _sum[i];
}
_sum[i] = 0;
_count[i] = 0;
}
return _healthy[_get_primary()];
}
void AP_RangeFinder_PX4::accumulate(void)
{
struct range_finder_report range_report;
for (uint8_t i=0; i<_num_instances; i++) {
while (::read(_range_fd[i], &range_report, sizeof(range_report)) == sizeof(range_report) &&
range_report.timestamp != _last_timestamp[i]) {
float sum = 0;
uint16_t count = 0;
while (::read(_fd, &range_report, sizeof(range_report)) == sizeof(range_report) &&
range_report.timestamp != _last_timestamp) {
// Only take valid readings
if (range_report.valid == 1) {
_sum[i] += range_report.distance;
_count[i]++;
_last_timestamp[i] = range_report.timestamp;
sum += range_report.distance;
count++;
_last_timestamp = range_report.timestamp;
}
}
}
}
uint8_t AP_RangeFinder_PX4::_get_primary(void) const
{
for (uint8_t i=0; i<_num_instances; i++) {
if (_healthy[i]) return i;
}
return 0;
}
// consider the range finder healthy if we got a reading in the last 0.2s
state.healthy = (hrt_absolute_time() - _last_timestamp < 200000);
int16_t AP_RangeFinder_PX4::read()
{
take_reading();
return _distance[_get_primary()];
if (count != 0) {
state.distance_cm = sum / count * 100.0f;
}
}
#endif // CONFIG_HAL_BOARD

View File

@ -18,32 +18,30 @@
#define AP_RangeFinder_PX4_H
#include "RangeFinder.h"
#include "RangeFinder_Backend.h"
class AP_RangeFinder_PX4 : public RangeFinder
class AP_RangeFinder_PX4 : public AP_RangeFinder_Backend
{
public:
// constructor
AP_RangeFinder_PX4(FilterInt16 *);
// initialize all the range finder devices
bool init(void);
bool take_reading(void);
void accumulate(void);
// read value from primary sensor and return distance in cm
int16_t read();
// return the number of compass instances
uint8_t get_count(void) const { return _num_instances; }
AP_RangeFinder_PX4(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state);
// static detection function
static bool detect(RangeFinder &ranger, uint8_t instance);
// update state
void update(void);
private:
uint8_t _get_primary(void) const;
uint8_t _num_instances;
int _range_fd[RANGEFINDER_MAX_INSTANCES];
float _sum[RANGEFINDER_MAX_INSTANCES];
uint32_t _count[RANGEFINDER_MAX_INSTANCES];
uint64_t _last_timestamp[RANGEFINDER_MAX_INSTANCES];
int _fd;
uint64_t _last_timestamp;
// we need to keep track of how many PX4 drivers have been loaded
// so we can open the right device filename
static uint8_t num_px4_instances;
// try to open the PX4 driver and return its fd
static int open_driver(void);
};
#endif // AP_RangeFinder_PX4_H

View File

@ -18,6 +18,7 @@
#include "AP_RangeFinder_analog.h"
#include "AP_RangeFinder_PulsedLightLRF.h"
#include "AP_RangeFinder_MaxsonarI2CXL.h"
#include "AP_RangeFinder_PX4.h"
// table of user settable parameters
const AP_Param::GroupInfo RangeFinder::var_info[] PROGMEM = {
@ -209,6 +210,13 @@ void RangeFinder::detect_instance(uint8_t instance)
state[instance].instance = instance;
drivers[instance] = new AP_RangeFinder_MaxsonarI2CXL(*this, instance, state[instance]);
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
} else if (_type[instance] == RangeFinder_TYPE_AUTO || _type[instance] == RangeFinder_TYPE_PX4) {
if (AP_RangeFinder_PX4::detect(*this, instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_RangeFinder_PX4(*this, instance, state[instance]);
}
#endif
} 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