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:
parent
a4b9b989b7
commit
05d4f5fb5a
libraries/AP_RangeFinder
@ -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
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user