Rover: use rangefinder backend accessors

APMrover2/GCS_Mavlink.cpp

@@ -3,6 +3,8 @@
 
 #include "GCS_Mavlink.h"
 
+#include <AP_RangeFinder/RangeFinder_Backend.h>
+
 void Rover::send_heartbeat(mavlink_channel_t chan)
 {
     uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
@@ -184,35 +186,26 @@ void Rover::send_simstate(mavlink_channel_t chan)
 
 void Rover::send_rangefinder(mavlink_channel_t chan)
 {
-    if (!rangefinder.has_data(0) && !rangefinder.has_data(1)) {
+    float distance_cm;
-        // no rangefinder to report
+    float voltage;
-        return;
+    bool got_one = false;
+
+    // report smaller distance of all rangefinders
+    for (uint8_t i=0; i<rangefinder.num_sensors(); i++) {
+        AP_RangeFinder_Backend *s = rangefinder.get_backend(i);
+        if (s == nullptr) {
+            continue;
+        }
+        if (!got_one ||
+            s->distance_cm() < distance_cm) {
+            distance_cm = s->distance_cm();
+            voltage = s->voltage_mv();
+            got_one = true;
+        }
     }
-
+    if (!got_one) {
-    float distance_cm = 0.0f;
+        // no relevant data found
-    float voltage = 0.0f;
+        return;
-
-    /*
-      report smaller distance of two rangefinders
-     */
-    if (rangefinder.has_data(0) && rangefinder.has_data(1)) {
-        if (rangefinder.distance_cm(0) <= rangefinder.distance_cm(1)) {
-            distance_cm = rangefinder.distance_cm(0);
-            voltage = rangefinder.voltage_mv(0);
-        } else {
-            distance_cm = rangefinder.distance_cm(1);
-            voltage = rangefinder.voltage_mv(1);
-        }
-    } else {
-        // only rangefinder 0 or rangefinder 1 has data
-        if (rangefinder.has_data(0)) {
-            distance_cm = rangefinder.distance_cm(0);
-            voltage = rangefinder.voltage_mv(0) * 0.001f;
-        }
-        if (rangefinder.has_data(1)) {
-            distance_cm = rangefinder.distance_cm(1);
-            voltage = rangefinder.voltage_mv(1) * 0.001f;
-        }
     }
 
     mavlink_msg_rangefinder_send(

APMrover2/Log.cpp

@@ -1,6 +1,8 @@
 #include "Rover.h"
 #include "version.h"
 
+#include <AP_RangeFinder/RangeFinder_Backend.h>
+
 #if LOGGING_ENABLED == ENABLED
 
 struct PACKED log_Performance {
@@ -178,12 +180,14 @@ void Rover::Log_Write_Rangefinder()
     if (!is_zero(obstacle.turn_angle)) {
         turn_time = AP_HAL::millis() - obstacle.detected_time_ms;
     }
+    AP_RangeFinder_Backend *s0 = rangefinder.get_backend(0);
+    AP_RangeFinder_Backend *s1 = rangefinder.get_backend(1);
     struct log_Rangefinder pkt = {
         LOG_PACKET_HEADER_INIT(LOG_RANGEFINDER_MSG),
         time_us               : AP_HAL::micros64(),
         lateral_accel         : control_mode->lateral_acceleration,
-        rangefinder1_distance : rangefinder.distance_cm(0),
+        rangefinder1_distance : s0 ? s0->distance_cm() : (uint16_t)0,
-        rangefinder2_distance : rangefinder.distance_cm(1),
+        rangefinder2_distance : s1 ? s1->distance_cm() : (uint16_t)0,
         detected_count        : obstacle.detected_count,
         turn_angle            : static_cast<int8_t>(obstacle.turn_angle),
         turn_time             : turn_time,

APMrover2/sensors.cpp

@@ -1,5 +1,7 @@
 #include "Rover.h"
 
+#include <AP_RangeFinder/RangeFinder_Backend.h>
+
 // initialise compass
 void Rover::init_compass()
 {
@@ -200,15 +202,18 @@ void Rover::read_rangefinders(void)
 {
     rangefinder.update();
 
-    if (rangefinder.status(0) == RangeFinder::RangeFinder_NotConnected) {
+    AP_RangeFinder_Backend *s0 = rangefinder.get_backend(0);
+    AP_RangeFinder_Backend *s1 = rangefinder.get_backend(1);
+
+    if (s0 == nullptr || s0->status() == RangeFinder::RangeFinder_NotConnected) {
         // this makes it possible to disable rangefinder at runtime
         return;
     }
 
-    if (rangefinder.has_data(1)) {
+    if (s1 != nullptr && s1->has_data()) {
         // we have two rangefinders
-        obstacle.rangefinder1_distance_cm = rangefinder.distance_cm(0);
+        obstacle.rangefinder1_distance_cm = s0->distance_cm();
-        obstacle.rangefinder2_distance_cm = rangefinder.distance_cm(1);
+        obstacle.rangefinder2_distance_cm = s1->distance_cm();
         if (obstacle.rangefinder1_distance_cm < static_cast<uint16_t>(g.rangefinder_trigger_cm) &&
             obstacle.rangefinder1_distance_cm < static_cast<uint16_t>(obstacle.rangefinder2_distance_cm))  {
             // we have an object on the left
@@ -235,7 +240,7 @@ void Rover::read_rangefinders(void)
         }
     } else {
         // we have a single rangefinder
-        obstacle.rangefinder1_distance_cm = rangefinder.distance_cm(0);
+        obstacle.rangefinder1_distance_cm = s0->distance_cm();
         obstacle.rangefinder2_distance_cm = 0;
         if (obstacle.rangefinder1_distance_cm < static_cast<uint16_t>(g.rangefinder_trigger_cm))  {
             // obstacle detected in front
@@ -347,7 +352,8 @@ void Rover::update_sensor_status_flags(void)
         if (g.rangefinder_trigger_cm > 0) {
             control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
         }
-        if (rangefinder.has_data(0)) {
+        AP_RangeFinder_Backend *s = rangefinder.get_backend(0);
+        if (s != nullptr && s->has_data()) {
             control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
         }
     }