diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp
index 40b0c120ec..07e899320d 100644
--- a/ArduCopter/sensors.cpp
+++ b/ArduCopter/sensors.cpp
@@ -40,8 +40,10 @@ void Copter::read_rangefinder(void)
     // filter rangefinder for use by AC_WPNav
     uint32_t now = AP_HAL::millis();
 
+    const bool timed_out = now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS;
+
     if (rangefinder_state.alt_healthy) {
-        if (now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS) {
+        if (timed_out) {
             // reset filter if we haven't used it within the last second
             rangefinder_state.alt_cm_filt.reset(rangefinder_state.alt_cm);
         } else {
@@ -51,7 +53,9 @@ void Copter::read_rangefinder(void)
     }
 
     // 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());
+    if (rangefinder_state.alt_healthy || timed_out) {
+        wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
+    }
 
 #else
     rangefinder_state.enabled = false;