AP_RangeFinder: provide access to signal_quality_pct

ArduPilot · Oct 17, 2023 · 18999a8 · 18999a8
1 parent fe2ce0e
commit 18999a8
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 1 deletion.
diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp
@@ -31,7 +31,13 @@ void Sub::read_rangefinder()
 #if RANGEFINDER_ENABLED == ENABLED
     rangefinder.update();
 
-    rangefinder_state.alt_healthy = ((rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good) && (rangefinder.range_valid_count_orient(ROTATION_PITCH_270) >= RANGEFINDER_HEALTH_MAX));
+    // Signal quality ranges from 0 (worst) to 100 (perfect), -1 means n/a
+    auto signal_quality_pct = rangefinder.signal_quality_pct(ROTATION_PITCH_270);
+
+    rangefinder_state.alt_healthy =
+            (rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good) &&
+            (rangefinder.range_valid_count_orient(ROTATION_PITCH_270) >= RANGEFINDER_HEALTH_MAX) &&
+            (signal_quality_pct == -1 || signal_quality_pct > 90);
 
     int16_t temp_alt = rangefinder.distance_cm_orient(ROTATION_PITCH_270);
 

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp
@@ -740,6 +740,17 @@ MAV_DISTANCE_SENSOR RangeFinder::get_mav_distance_sensor_type_orient(enum Rotati
     return backend->get_mav_distance_sensor_type();
 }
 
+int8_t RangeFinder::signal_quality_pct(enum Rotation orientation) const
+{
+    AP_RangeFinder_Backend *backend = find_instance(orientation);
+    int8_t signal_quality;
+    if (backend != nullptr && backend->get_signal_quality_pct(signal_quality)) {
+        return signal_quality;
+    } else {
+        return -1;
+    }
+}
+
 // get temperature reading in C.  returns true on success and populates temp argument
 bool RangeFinder::get_temp(enum Rotation orientation, float &temp) const
 {

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h
@@ -189,6 +189,7 @@ class RangeFinder
     uint8_t range_valid_count_orient(enum Rotation orientation) const;
     const Vector3f &get_pos_offset_orient(enum Rotation orientation) const;
     uint32_t last_reading_ms(enum Rotation orientation) const;
+    int8_t signal_quality_pct(enum Rotation orientation) const;
 
     // get temperature reading in C.  returns true on success and populates temp argument
     bool get_temp(enum Rotation orientation, float &temp) const;