AP_RangeFinder: ensure that too low and high far states are propagated correctly in DroneCAN

Author: andyp1per

libraries/AP_RangeFinder/AP_RangeFinder_Backend.h

@@ -66,6 +66,7 @@ class AP_RangeFinder_Backend
     bool has_data() const;
 
     // returns count of consecutive good readings
+    // note that this does not mean that the device is unhealthy
     uint8_t range_valid_count() const { return state.range_valid_count; }
 
     // return a 3D vector defining the position offset of the sensor

libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.cpp

@@ -82,17 +82,29 @@ void AP_RangeFinder_DroneCAN::update()
 {
     WITH_SEMAPHORE(_sem);
     if ((AP_HAL::millis() - _last_reading_ms) > 500) {
-        //if data is older than 500ms, report NoData
+        // if last read was more than 500ms, report NoData
         set_status(RangeFinder::Status::NoData);
-    } else if (_status == RangeFinder::Status::Good && new_data) {
+        return;
+    }
+
+    if (_status != RangeFinder::Status::Good) {
+        // handle additional states received by measurement handler
+        if (_status == RangeFinder::Status::OutOfRangeLow) {
+            // distance_m starts out as 0 when we are initially on the ground, we want it to return
+            // to this value upon landing
+            state.distance_m = 0;
+        }
+        state.last_reading_ms = _last_reading_ms;
+        set_status(_status);
+        return;
+    }
+
+    if (_status == RangeFinder::Status::Good && new_data) {
         //copy over states
         state.distance_m = _distance_cm * 0.01f;
         state.last_reading_ms = _last_reading_ms;
         update_status();
         new_data = false;
-    } else if (_status != RangeFinder::Status::Good) {
-        //handle additional states received by measurement handler
-        set_status(_status);
     }
 }
 

libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.h

@@ -28,6 +28,7 @@ class AP_RangeFinder_DroneCAN : public AP_RangeFinder_Backend {
     }
 private:
     uint8_t _instance;
+    // _status is the state received from the peripheral
     RangeFinder::Status _status;
     uint16_t _distance_cm;
     uint32_t _last_reading_ms;