Copter: provide filtered range finder altitude to AC_WPNav

ArduCopter/Copter.h

@@ -175,7 +175,9 @@ private:
         bool enabled:1;
         bool alt_healthy:1; // true if we can trust the altitude from the rangefinder
         int16_t alt_cm;     // tilt compensated altitude (in cm) from rangefinder
-    } rangefinder_state = { true, false, 0 };
+        uint32_t last_healthy_ms;
+        LowPassFilterFloat alt_cm_filt; // altitude filter
+    } rangefinder_state = { true, false, 0, 0 };
 #endif
 
     AP_RPM rpm_sensor;

ArduCopter/config.h

@@ -140,6 +140,10 @@
  # define RANGEFINDER_TIMEOUT_MS  1000      // desired rangefinder alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt
 #endif
 
+#ifndef RANGEFINDER_WPNAV_FILT_HZ
+ # define RANGEFINDER_WPNAV_FILT_HZ   0.25f // filter frequency for rangefinder altitude provided to waypoint navigation class
+#endif
+
 #ifndef RANGEFINDER_TILT_CORRECTION         // by disable tilt correction for use of range finder data by EKF
  # define RANGEFINDER_TILT_CORRECTION ENABLED
 #endif

ArduCopter/sensors.cpp

@@ -30,6 +30,7 @@ void Copter::init_rangefinder(void)
 {
 #if RANGEFINDER_ENABLED == ENABLED
    rangefinder.init();
+   rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
 #endif
 }
 
@@ -39,13 +40,7 @@ void Copter::read_rangefinder(void)
 #if RANGEFINDER_ENABLED == ENABLED
     rangefinder.update();
 
-    // exit immediately if rangefinder is disabled
-    if (rangefinder.status() != RangeFinder::RangeFinder_Good) {
-        rangefinder_state.alt_healthy = false;
-        return;
-    }
-
-    rangefinder_state.alt_healthy = (rangefinder.range_valid_count() >= RANGEFINDER_HEALTH_MAX);
+    rangefinder_state.alt_healthy = ((rangefinder.status() == RangeFinder::RangeFinder_Good) && (rangefinder.range_valid_count() >= RANGEFINDER_HEALTH_MAX));
 
     int16_t temp_alt = rangefinder.distance_cm();
 
@@ -55,6 +50,23 @@ void Copter::read_rangefinder(void)
  #endif
 
     rangefinder_state.alt_cm = temp_alt;
+
+    // filter rangefinder for use by AC_WPNav
+    uint32_t now = AP_HAL::millis();
+
+    if (rangefinder_state.alt_healthy) {
+        if (now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS) {
+            // reset filter if we haven't used it within the last second
+            rangefinder_state.alt_cm_filt.reset(rangefinder_state.alt_cm);
+        } else {
+            rangefinder_state.alt_cm_filt.apply(rangefinder_state.alt_cm, 0.1f);
+        }
+        rangefinder_state.last_healthy_ms = now;
+    }
+
+    // send rangefinder altitude and health to waypoint navigation library
+    wp_nav.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
+
 #else
     rangefinder_state.alt_healthy = false;
     rangefinder_state.alt_cm = 0;