AP_RangeFinder: MAVLink: use larger of min ranges / smaller of max ra…

…nges
ArduCustom · Jan 6, 2021 · 4962671 · 4962671
1 parent 81ab322
commit 4962671
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 2 deletions.
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp
@@ -34,6 +34,30 @@ void AP_RangeFinder_MAVLink::handle_msg(const mavlink_message_t &msg)
     }
 }
 
+int16_t AP_RangeFinder_MAVLink::max_distance_cm() const
+{
+    if (_max_distance_cm == 0 && _min_distance_cm == 0) {
+        // we assume if both of these are zero that we ignore both
+        return params.max_distance_cm;
+    }
+
+    if (params.max_distance_cm < _max_distance_cm) {
+        return params.max_distance_cm;
+    }
+    return _max_distance_cm;
+}
+int16_t AP_RangeFinder_MAVLink::min_distance_cm() const
+{
+    if (_max_distance_cm == 0 && _min_distance_cm == 0) {
+        // we assume if both of these are zero that we ignore both
+        return params.min_distance_cm;
+    }
+    if (params.min_distance_cm > _min_distance_cm) {
+        return params.min_distance_cm;
+    }
+    return _min_distance_cm;
+}
+
 /*
    update the state of the sensor
 */

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h
@@ -23,8 +23,8 @@ class AP_RangeFinder_MAVLink : public AP_RangeFinder_Backend
     // Get update from mavlink
     void handle_msg(const mavlink_message_t &msg) override;
 
-    int16_t max_distance_cm() const override { return _max_distance_cm; }
-    int16_t min_distance_cm() const override { return _min_distance_cm; }
+    int16_t max_distance_cm() const override;
+    int16_t min_distance_cm() const override;
 
 protected: