From c0aa10d84b25eb1c12ff7152be5421d9e37008aa Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 8 Aug 2017 15:54:09 +1000 Subject: [PATCH] AP_RangeFinder: move rangefinder backend data accessors to backend --- .../AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp | 8 +- .../AP_RangeFinder_PulsedLightLRF.cpp | 8 +- .../AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp | 6 +- .../AP_RangeFinder/AP_RangeFinder_analog.cpp | 6 +- .../AP_RangeFinder/AP_RangeFinder_trone.cpp | 14 +- libraries/AP_RangeFinder/RangeFinder.cpp | 141 ++++++++---------- libraries/AP_RangeFinder/RangeFinder.h | 53 +------ .../AP_RangeFinder/RangeFinder_Backend.cpp | 6 +- .../AP_RangeFinder/RangeFinder_Backend.h | 39 ++++- .../examples/RFIND_test/RFIND_test.cpp | 23 ++- 10 files changed, 144 insertions(+), 160 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp index 0af502fa85..0643557a9d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp @@ -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++; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp index 220fe2b97f..e299b00936 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp @@ -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); } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp index c8b609bce2..6311faefbe 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp @@ -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; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp index c31ae4d0c8..e455f37bcb 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp @@ -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; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_trone.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_trone.cpp index 84b606d5ff..140ecaac18 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_trone.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_trone.cpp @@ -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(); } diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 6951f6d8c4..bde1c9002c 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -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; iorientation() != 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(); } diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index 56d9cf81f3..d503e2b5f2 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -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 -#include +#include 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; ihas_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();