Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: move rangefinder backend data accessors to backend

This commit is contained in:
Peter Barker 2017-08-08 15:54:09 +10:00 committed by Francisco Ferreira
parent 1b6c3aeea1
commit c0aa10d84b
10 changed files with 144 additions and 160 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp
View File

@ -117,10 +117,10 @@ void AP_RangeFinder_PX4_PWM::update(void)
_last_pulse_time_ms = now;
// setup for scaling in meters per millisecond
float distance_cm = pwm.pulse_width * 0.1f * scaling + state.offset;
float _distance_cm = pwm.pulse_width * 0.1f * scaling + state.offset;
float distance_delta_cm = fabsf(distance_cm - _last_sample_distance_cm);
_last_sample_distance_cm = distance_cm;
float distance_delta_cm = fabsf(_distance_cm - _last_sample_distance_cm);
_last_sample_distance_cm = _distance_cm;
if (distance_delta_cm > 100) {
// varying by more than 1m in a single sample, which means
@ -131,7 +131,7 @@ void AP_RangeFinder_PX4_PWM::update(void)
if (_good_sample_count > 1) {
count++;
sum_cm += distance_cm;
sum_cm += _distance_cm;
_last_timestamp = pwm.timestamp;
} else {
_good_sample_count++;

8
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp
View File

@ -96,13 +96,13 @@ void AP_RangeFinder_PulsedLightLRF::timer(void)
be16_t val;
// read the high and low byte distance registers
if (_dev->read_registers(LL40LS_DISTHIGH_REG | LL40LS_AUTO_INCREMENT, (uint8_t*)&val, sizeof(val))) {
uint16_t distance_cm = be16toh(val);
uint16_t _distance_cm = be16toh(val);
// remove momentary spikes
if (abs(distance_cm - last_distance_cm) < 100) {
state.distance_cm = distance_cm;
if (abs(_distance_cm - last_distance_cm) < 100) {
state.distance_cm = _distance_cm;
update_status();
}
last_distance_cm = distance_cm;
last_distance_cm = _distance_cm;
} else {
set_status(RangeFinder::RangeFinder_NoData);
}

6
libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp
View File

@ -326,12 +326,12 @@ void AP_RangeFinder_VL53L0X::getSequenceStepEnables(SequenceStepEnables * enable
// Get the VCSEL pulse period in PCLKs for the given period type.
// based on VL53L0X_get_vcsel_pulse_period()
uint8_t AP_RangeFinder_VL53L0X::getVcselPulsePeriod(vcselPeriodType type)
uint8_t AP_RangeFinder_VL53L0X::getVcselPulsePeriod(vcselPeriodType _type)
{
#define decodeVcselPeriod(reg_val) (((reg_val) + 1) << 1)
if (type == VcselPeriodPreRange) {
if (_type == VcselPeriodPreRange) {
return decodeVcselPeriod(read_register(PRE_RANGE_CONFIG_VCSEL_PERIOD));
} else if (type == VcselPeriodFinalRange) {
} else if (_type == VcselPeriodFinalRange) {
return decodeVcselPeriod(read_register(FINAL_RANGE_CONFIG_VCSEL_PERIOD));
}
return 255;

6
libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
View File

@ -90,7 +90,7 @@ void AP_RangeFinder_analog::update(void)
float scaling = state.scaling;
float offset = state.offset;
RangeFinder::RangeFinder_Function function = (RangeFinder::RangeFinder_Function)state.function.get();
int16_t max_distance_cm = state.max_distance_cm;
int16_t _max_distance_cm = state.max_distance_cm;
switch (function) {
case RangeFinder::FUNCTION_LINEAR:
@ -106,8 +106,8 @@ void AP_RangeFinder_analog::update(void)
dist_m = 0;
}
dist_m = scaling / (v - offset);
if (isinf(dist_m) || dist_m > max_distance_cm * 0.01f) {
dist_m = max_distance_cm * 0.01f;
if (isinf(dist_m) || dist_m > _max_distance_cm * 0.01f) {
dist_m = _max_distance_cm * 0.01f;
}
break;
}

14
libraries/AP_RangeFinder/AP_RangeFinder_trone.cpp
View File

@ -88,8 +88,8 @@ bool AP_RangeFinder_trone::init(void)
// give time for the sensor to process the request
hal.scheduler->delay(70);
uint16_t distance_cm;
if (!collect(distance_cm)) {
uint16_t _distance_cm;
if (!collect(_distance_cm)) {
dev->get_semaphore()->give();
return false;
}
@ -112,7 +112,7 @@ bool AP_RangeFinder_trone::measure()
}
// collect - return last value measured by sensor
bool AP_RangeFinder_trone::collect(uint16_t &distance_cm)
bool AP_RangeFinder_trone::collect(uint16_t &_distance_cm)
{
uint8_t d[3];
@ -126,7 +126,7 @@ bool AP_RangeFinder_trone::collect(uint16_t &distance_cm)
return false;
}
distance_cm = ((uint16_t(d[0]) << 8) | d[1]) / 10;
_distance_cm = ((uint16_t(d[0]) << 8) | d[1]) / 10;
return true;
}
@ -137,9 +137,9 @@ bool AP_RangeFinder_trone::collect(uint16_t &distance_cm)
void AP_RangeFinder_trone::timer(void)
{
// take a reading
uint16_t distance_cm;
if (collect(distance_cm) && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
accum.sum += distance_cm;
uint16_t _distance_cm;
if (collect(_distance_cm) && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
accum.sum += _distance_cm;
accum.count++;
_sem->give();
}

141
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -701,28 +701,26 @@ void RangeFinder::detect_instance(uint8_t instance)
}
}
// query status
RangeFinder::RangeFinder_Status RangeFinder::status(uint8_t instance) const
{
// sanity check instance
if (instance >= RANGEFINDER_MAX_INSTANCES) {
return RangeFinder_NotConnected;
AP_RangeFinder_Backend *RangeFinder::get_backend(uint8_t id) const {
if (id >= num_instances) {
return nullptr;
}
if (drivers[instance] == nullptr || state[instance].type == RangeFinder_TYPE_NONE) {
return RangeFinder_NotConnected;
if (drivers[id] != nullptr) {
if (drivers[id]->type() == RangeFinder_TYPE_NONE) {
// pretend it isn't here; disabled at runtime?
return nullptr;
}
}
return state[instance].status;
}
return drivers[id];
};
RangeFinder::RangeFinder_Status RangeFinder::status_orient(enum Rotation orientation) const
{
uint8_t i;
if (find_instance(orientation, i)) {
return status(i);
AP_RangeFinder_Backend *backend = find_instance(orientation);
if (backend == nullptr) {
return RangeFinder_NotConnected;
}
return RangeFinder_NotConnected;
return backend->status();
}
void RangeFinder::handle_msg(mavlink_message_t *msg)
@ -738,93 +736,86 @@ void RangeFinder::handle_msg(mavlink_message_t *msg)
// return true if we have a range finder with the specified orientation
bool RangeFinder::has_orientation(enum Rotation orientation) const
{
uint8_t i;
return find_instance(orientation, i);
return (find_instance(orientation) != nullptr);
}
// find first range finder instance with the specified orientation
bool RangeFinder::find_instance(enum Rotation orientation, uint8_t &instance) const
AP_RangeFinder_Backend *RangeFinder::find_instance(enum Rotation orientation) const
{
for (uint8_t i=0; i<num_instances; i++) {
if (state[i].orientation == orientation) {
instance = i;
return true;
AP_RangeFinder_Backend *backend = get_backend(i);
if (backend == nullptr) {
continue;
}
if (backend->orientation() != orientation) {
continue;
}
return backend;
}
return false;
return nullptr;
}
uint16_t RangeFinder::distance_cm_orient(enum Rotation orientation) const
{
uint8_t i;
if (find_instance(orientation, i)) {
return distance_cm(i);
AP_RangeFinder_Backend *backend = find_instance(orientation);
if (backend == nullptr) {
return 0;
}
return 0;
return backend->distance_cm();
}
uint16_t RangeFinder::voltage_mv_orient(enum Rotation orientation) const
{
uint8_t i;
if (find_instance(orientation, i)) {
return voltage_mv(i);
AP_RangeFinder_Backend *backend = find_instance(orientation);
if (backend == nullptr) {
return 0;
}
return 0;
return backend->voltage_mv();
}
int16_t RangeFinder::max_distance_cm_orient(enum Rotation orientation) const
{
uint8_t i;
if (find_instance(orientation, i)) {
return max_distance_cm(i);
AP_RangeFinder_Backend *backend = find_instance(orientation);
if (backend == nullptr) {
return 0;
}
return 0;
return backend->max_distance_cm();
}
int16_t RangeFinder::min_distance_cm_orient(enum Rotation orientation) const
{
uint8_t i;
if (find_instance(orientation, i)) {
return min_distance_cm(i);
AP_RangeFinder_Backend *backend = find_instance(orientation);
if (backend == nullptr) {
return 0;
}
return 0;
return backend->min_distance_cm();
}
int16_t RangeFinder::ground_clearance_cm_orient(enum Rotation orientation) const
{
uint8_t i;
if (find_instance(orientation, i)) {
return ground_clearance_cm(i);
AP_RangeFinder_Backend *backend = find_instance(orientation);
if (backend == nullptr) {
return 0;
}
return 0;
}
// true if sensor is returning data
bool RangeFinder::has_data(uint8_t instance) const
{
// sanity check instance
if (instance >= RANGEFINDER_MAX_INSTANCES) {
return RangeFinder_NotConnected;
}
return ((state[instance].status != RangeFinder_NotConnected) && (state[instance].status != RangeFinder_NoData));
return backend->ground_clearance_cm();
}
bool RangeFinder::has_data_orient(enum Rotation orientation) const
{
uint8_t i;
if (find_instance(orientation, i)) {
return has_data(i);
AP_RangeFinder_Backend *backend = find_instance(orientation);
if (backend == nullptr) {
return false;
}
return false;
return backend->has_data();
}
uint8_t RangeFinder::range_valid_count_orient(enum Rotation orientation) const
{
uint8_t i;
if (find_instance(orientation, i)) {
return range_valid_count(i);
AP_RangeFinder_Backend *backend = find_instance(orientation);
if (backend == nullptr) {
return 0;
}
return 0;
return backend->range_valid_count();
}
/*
@ -845,30 +836,18 @@ bool RangeFinder::pre_arm_check() const
const Vector3f &RangeFinder::get_pos_offset_orient(enum Rotation orientation) const
{
uint8_t i=0;
if (find_instance(orientation, i)) {
return get_pos_offset(i);
AP_RangeFinder_Backend *backend = find_instance(orientation);
if (backend == nullptr) {
return pos_offset_zero;
}
return pos_offset_zero;
}
MAV_DISTANCE_SENSOR RangeFinder::get_sensor_type(uint8_t instance) const {
// sanity check instance
if (instance >= RANGEFINDER_MAX_INSTANCES) {
return MAV_DISTANCE_SENSOR_UNKNOWN;
}
if (drivers[instance] == nullptr || state[instance].type == RangeFinder_TYPE_NONE) {
return MAV_DISTANCE_SENSOR_UNKNOWN;
}
return drivers[instance]->get_sensor_type();
return backend->get_pos_offset();
}
MAV_DISTANCE_SENSOR RangeFinder::get_sensor_type_orient(enum Rotation orientation) const
{
uint8_t i;
if (find_instance(orientation, i)) {
return get_sensor_type(i);
AP_RangeFinder_Backend *backend = find_instance(orientation);
if (backend == nullptr) {
return MAV_DISTANCE_SENSOR_UNKNOWN;
}
return MAV_DISTANCE_SENSOR_UNKNOWN;
return backend->get_sensor_type();
}

53
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -118,64 +118,26 @@ public:
// Handle an incoming DISTANCE_SENSOR message (from a MAVLink enabled range finder)
void handle_msg(mavlink_message_t *msg);
#define _RangeFinder_STATE(instance) state[instance]
// return true if we have a range finder with the specified orientation
bool has_orientation(enum Rotation orientation) const;
// find first range finder instance with the specified orientation
bool find_instance(enum Rotation orientation, uint8_t &instance) const;
AP_RangeFinder_Backend *find_instance(enum Rotation orientation) const;
// get orientation of a given range finder
enum Rotation get_orientation(uint8_t instance) const {
return (instance<num_instances? (enum Rotation)_RangeFinder_STATE(instance).orientation.get() : ROTATION_NONE);
}
AP_RangeFinder_Backend *get_backend(uint8_t id) const;
uint16_t distance_cm(uint8_t instance) const {
return (instance<num_instances? _RangeFinder_STATE(instance).distance_cm : 0);
}
// methods to return a distance on a particular orientation from
// any sensor which can current supply it
uint16_t distance_cm_orient(enum Rotation orientation) const;
uint16_t voltage_mv(uint8_t instance) const {
return _RangeFinder_STATE(instance).voltage_mv;
}
uint16_t voltage_mv_orient(enum Rotation orientation) const;
int16_t max_distance_cm(uint8_t instance) const {
return _RangeFinder_STATE(instance).max_distance_cm;
}
int16_t max_distance_cm_orient(enum Rotation orientation) const;
int16_t min_distance_cm(uint8_t instance) const {
return _RangeFinder_STATE(instance).min_distance_cm;
}
int16_t min_distance_cm_orient(enum Rotation orientation) const;
int16_t ground_clearance_cm(uint8_t instance) const {
return _RangeFinder_STATE(instance).ground_clearance_cm;
}
int16_t ground_clearance_cm_orient(enum Rotation orientation) const;
MAV_DISTANCE_SENSOR get_sensor_type(uint8_t instance) const;
MAV_DISTANCE_SENSOR get_sensor_type_orient(enum Rotation orientation) const;
// query status
RangeFinder_Status status(uint8_t instance) const;
RangeFinder_Status status_orient(enum Rotation orientation) const;
// query type
RangeFinder_Type type(uint8_t instance) const { return (RangeFinder_Type)_RangeFinder_STATE(instance).type.get(); }
// true if sensor is returning data
bool has_data(uint8_t instance) const;
bool has_data_orient(enum Rotation orientation) const;
// returns count of consecutive good readings
uint8_t range_valid_count(uint8_t instance) const {
return _RangeFinder_STATE(instance).range_valid_count;
}
uint8_t range_valid_count_orient(enum Rotation orientation) const;
const Vector3f &get_pos_offset_orient(enum Rotation orientation) const;
/*
set an externally estimated terrain height. Used to enable power
@ -192,11 +154,6 @@ public:
*/
bool pre_arm_check() const;
// return a 3D vector defining the position offset of the sensor in metres relative to the body frame origin
const Vector3f &get_pos_offset(uint8_t instance) const {
return _RangeFinder_STATE(instance).pos_offset;
}
const Vector3f &get_pos_offset_orient(enum Rotation orientation) const;
private:
RangeFinder_State state[RANGEFINDER_MAX_INSTANCES];

6
libraries/AP_RangeFinder/RangeFinder_Backend.cpp
View File

@ -45,12 +45,12 @@ void AP_RangeFinder_Backend::update_status()
}
// set status and update valid count
void AP_RangeFinder_Backend::set_status(RangeFinder::RangeFinder_Status status)
void AP_RangeFinder_Backend::set_status(RangeFinder::RangeFinder_Status _status)
{
state.status = status;
state.status = _status;
// update valid count
if (status == RangeFinder::RangeFinder_Good) {
if (_status == RangeFinder::RangeFinder_Good) {
if (state.range_valid_count < 10) {
state.range_valid_count++;
}

39
libraries/AP_RangeFinder/RangeFinder_Backend.h
View File

@ -32,14 +32,45 @@ public:
// update the state structure
virtual void update() = 0;
MAV_DISTANCE_SENSOR get_sensor_type() const {
return sensor_type;
}
virtual void handle_msg(mavlink_message_t *msg) { return; }
void update_pre_arm_check();
uint8_t instance() const { return state.instance; }
enum Rotation orientation() const { return (Rotation)state.orientation.get(); }
uint16_t distance_cm() const { return state.distance_cm; }
uint16_t voltage_mv() const { return state.voltage_mv; }
int16_t max_distance_cm() const { return state.max_distance_cm; }
int16_t min_distance_cm() const { return state.min_distance_cm; }
int16_t ground_clearance_cm() const { return state.ground_clearance_cm; }
MAV_DISTANCE_SENSOR get_sensor_type() const {
if (state.type == RangeFinder::RangeFinder_TYPE_NONE) {
return MAV_DISTANCE_SENSOR_UNKNOWN;
}
return sensor_type;
}
RangeFinder::RangeFinder_Status status() const {
if (state.type == RangeFinder::RangeFinder_TYPE_NONE) {
// turned off at runtime?
return RangeFinder::RangeFinder_NotConnected;
}
return state.status;
}
RangeFinder::RangeFinder_Type type() const { return (RangeFinder::RangeFinder_Type)state.type.get(); }
// true if sensor is returning data
bool has_data() const {
return ((state.status != RangeFinder::RangeFinder_NotConnected) &&
(state.status != RangeFinder::RangeFinder_NoData));
}
// returns count of consecutive good readings
uint8_t range_valid_count() const { return state.range_valid_count; }
// return a 3D vector defining the position offset of the sensor
// in metres relative to the body frame origin
const Vector3f &get_pos_offset() const { return state.pos_offset; }
protected:
// update status based on distance measurement

23
libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp
View File

@ -3,7 +3,7 @@
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <AP_RangeFinder/RangeFinder_Backend.h>
void setup();
void loop();
@ -35,8 +35,25 @@ void loop()
hal.scheduler->delay(100);
sonar.update();
hal.console->printf("All: device_0 type %d status %d distance_cm %d, device_1 type %d status %d distance_cm %d\n",
(int)sonar.type(0), (int)sonar.status(0), sonar.distance_cm(0), (int)sonar.type(1), (int)sonar.status(1), sonar.distance_cm(1));
bool had_data = false;
for (uint8_t i=0; i<sonar.num_sensors(); i++) {
AP_RangeFinder_Backend *sensor = sonar.get_backend(i);
if (sensor == nullptr) {
continue;
}
if (!sensor->has_data()) {
continue;
}
hal.console->printf("All: device_%u type %d status %d distance_cm %d\n",
i,
(int)sensor->type(),
(int)sensor->status(),
sensor->distance_cm());
had_data = true;
}
if (!had_data) {
hal.console->printf("All: no data on any sensor\n");
}
}
AP_HAL_MAIN();