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 method returning zero 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

@@ -81,19 +81,27 @@ AP_RangeFinder_DroneCAN* AP_RangeFinder_DroneCAN::get_dronecan_backend(AP_DroneC
 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 ((AP_HAL::millis() - _last_reading_ms) > 500U) {
+        // if last read was more than 500ms, report NoData
         set_status(RangeFinder::Status::NoData);
-    } else if (_status == RangeFinder::Status::Good && new_data) {
-        //copy over states
-        state.distance_m = _distance_m;
-        state.last_reading_ms = _last_reading_ms;
+        return;
+    }
+
+    if (!new_data) {
+        return;
+    }
+
+    if (_status == RangeFinder::Status::Good) {
+        // copy over states
         update_status();
-        new_data = false;
-    } else if (_status != RangeFinder::Status::Good) {
-        //handle additional states received by measurement handler
+    } else {
+        // handle additional states received by measurement handler
         set_status(_status);
     }
+
+    state.distance_m = _distance_m;
+    state.last_reading_ms = _last_reading_ms;
+    new_data = false;
 }
 
 //RangeFinder message handler
@@ -118,14 +126,18 @@ void AP_RangeFinder_DroneCAN::handle_measurement(AP_DroneCAN *ap_dronecan, const
         //Additional states supported by RFND message
         case UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_CLOSE:
         {
+            driver->_distance_m = msg.range;
             driver->_last_reading_ms = AP_HAL::millis();
             driver->_status = RangeFinder::Status::OutOfRangeLow;
+            driver->new_data = true;
             break;
         }
         case UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_FAR:
         {
+            driver->_distance_m = msg.range;
             driver->_last_reading_ms = AP_HAL::millis();
             driver->_status = RangeFinder::Status::OutOfRangeHigh;
+            driver->new_data = true;
             break;
         }
         default:

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 - or "NoData" in case of timeout
     RangeFinder::Status _status;
     float _distance_m;
     uint32_t _last_reading_ms;