AP_RangeFinder: move rangefinder backend data accessors to backend

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);
+        float distance_delta_cm = fabsf(_distance_cm - _last_sample_distance_cm);
-        _last_sample_distance_cm = 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++;

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) {
+            if (abs(_distance_cm - last_distance_cm) < 100) {
-                state.distance_cm = distance_cm;
+                state.distance_cm = _distance_cm;
                 update_status();                
             }
-            last_distance_cm = distance_cm;
+            last_distance_cm = _distance_cm;
         } else {
             set_status(RangeFinder::RangeFinder_NoData);
         }

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;

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) {
+        if (isinf(dist_m) || dist_m > _max_distance_cm * 0.01f) {
-            dist_m = max_distance_cm * 0.01f;
+            dist_m = _max_distance_cm * 0.01f;
         }
         break;
     }

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;
+    uint16_t _distance_cm;
-    if (!collect(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;
+    uint16_t _distance_cm;
-    if (collect(distance_cm) && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+    if (collect(_distance_cm) && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        accum.sum += distance_cm;
+        accum.sum += _distance_cm;
         accum.count++;
         _sem->give();
     }

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -701,28 +701,26 @@ void RangeFinder::detect_instance(uint8_t instance)
     }
 }
 
-// query status
+AP_RangeFinder_Backend *RangeFinder::get_backend(uint8_t id) const {
-RangeFinder::RangeFinder_Status RangeFinder::status(uint8_t instance) const
+    if (id >= num_instances) {
-{
+        return nullptr;
-    // sanity check instance
-    if (instance >= RANGEFINDER_MAX_INSTANCES) {
-        return RangeFinder_NotConnected;
     }
-
+    if (drivers[id] != nullptr) {
-    if (drivers[instance] == nullptr || state[instance].type == RangeFinder_TYPE_NONE) {
+        if (drivers[id]->type() == RangeFinder_TYPE_NONE) {
-        return RangeFinder_NotConnected;
+            // 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;
+    AP_RangeFinder_Backend *backend = find_instance(orientation);
-    if (find_instance(orientation, i)) {
+    if (backend == nullptr) {
-        return status(i);
-    }
         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) != nullptr);
-    return find_instance(orientation, i);
 }
 
 // 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) {
+        AP_RangeFinder_Backend *backend = get_backend(i);
-            instance = i;
+        if (backend == nullptr) {
-            return true;
+            continue;
         }
+        if (backend->orientation() != orientation) {
+            continue;
         }
-    return false;
+        return backend;
+    }
+    return nullptr;
 }
 
 uint16_t RangeFinder::distance_cm_orient(enum Rotation orientation) const
 {
-    uint8_t i;
+    AP_RangeFinder_Backend *backend = find_instance(orientation);
-    if (find_instance(orientation, i)) {
+    if (backend == nullptr) {
-        return distance_cm(i);
-    }
         return 0;
+    }
+    return backend->distance_cm();
 }
 
 uint16_t RangeFinder::voltage_mv_orient(enum Rotation orientation) const
 {
-    uint8_t i;
+    AP_RangeFinder_Backend *backend = find_instance(orientation);
-    if (find_instance(orientation, i)) {
+    if (backend == nullptr) {
-        return voltage_mv(i);
-    }
         return 0;
+    }
+    return backend->voltage_mv();
 }
 
 int16_t RangeFinder::max_distance_cm_orient(enum Rotation orientation) const
 {
-    uint8_t i;
+    AP_RangeFinder_Backend *backend = find_instance(orientation);
-    if (find_instance(orientation, i)) {
+    if (backend == nullptr) {
-        return max_distance_cm(i);
-    }
         return 0;
+    }
+    return backend->max_distance_cm();
 }
 
 int16_t RangeFinder::min_distance_cm_orient(enum Rotation orientation) const
 {
-    uint8_t i;
+    AP_RangeFinder_Backend *backend = find_instance(orientation);
-    if (find_instance(orientation, i)) {
+    if (backend == nullptr) {
-        return min_distance_cm(i);
-    }
         return 0;
+    }
+    return backend->min_distance_cm();
 }
 
 int16_t RangeFinder::ground_clearance_cm_orient(enum Rotation orientation) const
 {
-    uint8_t i;
+    AP_RangeFinder_Backend *backend = find_instance(orientation);
-    if (find_instance(orientation, i)) {
+    if (backend == nullptr) {
-        return ground_clearance_cm(i);
-    }
         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;
+    AP_RangeFinder_Backend *backend = find_instance(orientation);
-    if (find_instance(orientation, i)) {
+    if (backend == nullptr) {
-        return has_data(i);
-    }
         return false;
+    }
+    return backend->has_data();
 }
 
 uint8_t RangeFinder::range_valid_count_orient(enum Rotation orientation) const
 {
-    uint8_t i;
+    AP_RangeFinder_Backend *backend = find_instance(orientation);
-    if (find_instance(orientation, i)) {
+    if (backend == nullptr) {
-        return range_valid_count(i);
-    }
         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;
+    AP_RangeFinder_Backend *backend = find_instance(orientation);
-    if (find_instance(orientation, i)) {
+    if (backend == nullptr) {
-        return get_pos_offset(i);
-    }
         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;
     }
-
+    return backend->get_pos_offset();
-    if (drivers[instance] == nullptr || state[instance].type == RangeFinder_TYPE_NONE) {
-        return MAV_DISTANCE_SENSOR_UNKNOWN;
-    }
-    return drivers[instance]->get_sensor_type();
 }
 
 MAV_DISTANCE_SENSOR RangeFinder::get_sensor_type_orient(enum Rotation orientation) const
 {
-    uint8_t i;
+    AP_RangeFinder_Backend *backend = find_instance(orientation);
-    if (find_instance(orientation, i)) {
+    if (backend == nullptr) {
-        return get_sensor_type(i);
-    }
         return MAV_DISTANCE_SENSOR_UNKNOWN;
+    }
+    return backend->get_sensor_type();
 }

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
+    AP_RangeFinder_Backend *get_backend(uint8_t id) const;
-    enum Rotation get_orientation(uint8_t instance) const {
-        return (instance<num_instances? (enum Rotation)_RangeFinder_STATE(instance).orientation.get() : ROTATION_NONE);
-    }
 
-    uint16_t distance_cm(uint8_t instance) const {
+    // methods to return a distance on a particular orientation from
-        return (instance<num_instances? _RangeFinder_STATE(instance).distance_cm : 0);
+    // 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];

libraries/AP_RangeFinder/RangeFinder_Backend.cpp

@@ -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++;
         }

libraries/AP_RangeFinder/RangeFinder_Backend.h

@@ -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

libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp

@@ -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",
+    bool had_data = false;
-                        (int)sonar.type(0), (int)sonar.status(0), sonar.distance_cm(0), (int)sonar.type(1), (int)sonar.status(1), sonar.distance_cm(1));
+    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();