AP_SurfaceDistance: Add Rangefinder configured check and out of range low timer

libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp

@@ -64,6 +64,20 @@ void AP_SurfaceDistance::update()
     // update health
     alt_healthy = (rangefinder->status_orient(rotation) == RangeFinder::Status::Good) &&
                             (rangefinder->range_valid_count_orient(rotation) >= RANGEFINDER_HEALTH_MIN);
+
+    // Keep track of whether we have ever had a healthy reading
+    has_been_healthy = has_been_healthy || alt_healthy;
+
+    // reset the out of range low timer if reading is reporting any other status
+    if (rangefinder->status_orient(rotation) != RangeFinder::Status::OutOfRangeLow) {
+        out_of_range_low_ms = 0;
+    }
+
+    // if we have just changed state to out of range low then keep track of the time this happened
+    if (out_of_range_low_ms == 0 && rangefinder->status_orient(rotation) == RangeFinder::Status::OutOfRangeLow) {
+        out_of_range_low_ms = now;
+    }
+
     if (!alt_healthy) {
         status |= (uint8_t)Surface_Distance_Status::Unhealthy;
     }
@@ -134,11 +148,37 @@ void AP_SurfaceDistance::update()
   difference between the inertial height at that time and the current
   inertial height to give us interpolation of height from rangefinder
  */
-bool AP_SurfaceDistance::get_rangefinder_height_interpolated_cm(int32_t& ret) const
+bool AP_SurfaceDistance::get_rangefinder_height_interpolated_cm(int32_t& ret, const uint32_t oor_low_timeout_ms)
 {
-    if (!enabled_and_healthy()) {
+    WITH_SEMAPHORE(sem);
+    // surface distance state not enabled
+    if (!enabled) {
         return false;
     }
+
+    // rangefinder has never been healthy so we cannot trust an interpolated result
+    if (!has_been_healthy) {
+        return false;
+    }
+
+    // rangefinder is unhealthy and we have not been told to temporarily accept out of range low values so we cannot use this measurement
+    if (!alt_healthy && oor_low_timeout_ms == 0) {
+        return false;
+    }
+
+    // we don't have an out of range low measurement and the rangefinder state is reporting unhealthy
+    if (!alt_healthy && out_of_range_low_ms == 0) {
+        return false;
+    }
+
+    // we are unhealthy because of an out of range low ready and the timer has run out on how long we are allowed to use a potentially out of range low reading
+    // TODO: handle timer wrap case?
+    if (!alt_healthy && (out_of_range_low_ms > 0) && (AP_HAL::millis() - out_of_range_low_ms > oor_low_timeout_ms)) {
+        return false;
+    }
+
+    // if we have got this far the rangefinder is enabled and is either healthy or we are within the permited
+    // grace time to be able to still use the rangefinder even though it is reporting out of range low
     ret = alt_cm_filt.get();
     ret += inertial_nav.get_position_z_up_cm() - inertial_alt_cm;
     return true;
@@ -185,3 +225,13 @@ bool AP_SurfaceDistance::enabled_and_healthy(void) const
 {
     return enabled && alt_healthy;
 }
+
+bool AP_SurfaceDistance::rangefinder_configured(void) const
+{
+    const RangeFinder *rangefinder = RangeFinder::get_singleton();
+    if (rangefinder == nullptr) {
+        return false;
+    }
+    return rangefinder->status_orient(rotation) != RangeFinder::Status::NotConnected &&
+           rangefinder->status_orient(rotation) != RangeFinder::Status::NoData;
+}

libraries/AP_SurfaceDistance/AP_SurfaceDistance.h

@@ -21,8 +21,12 @@ public:
     // helper to check that rangefinder was last reported as enabled and healthy
     bool enabled_and_healthy(void) const;
 
+    // Check that there is a rangefinder in the correct orientation configured, used for pre-arm checks
+    // when the rangefinder may not be reporting healthy (e.g. on ground reporting out of range low)
+    bool rangefinder_configured(void) const;
+
     // get inertially interpolated rangefinder height
-    bool get_rangefinder_height_interpolated_cm(int32_t& ret) const;
+    bool get_rangefinder_height_interpolated_cm(int32_t& ret, const uint32_t oor_low_timeout_ms = 0);
 
     bool enabled;                          // not to be confused with rangefinder enabled, this state is to be set by the vehicle.
     bool alt_healthy;                      // true if we can trust the altitude from the rangefinder
@@ -42,6 +46,9 @@ private:
     // multi-thread access support
     HAL_Semaphore sem;
 
+    bool rangefinder_is_config;
+    bool has_been_healthy;
+    uint32_t out_of_range_low_ms;           // keep track of the rangefinder state. When using the inertially interpolated rangefinder reading sometimes it is acceptable to use the rangefinder when it is reporting low
     const uint8_t instance;
     uint8_t status;
     uint32_t last_healthy_ms;