diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index b5ea118e04..92a123d1f1 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -230,6 +230,7 @@ private:
         uint32_t last_correction_time_ms;
         uint8_t in_range_count;
         float height_estimate;
+        float last_distance;
     } rangefinder_state;
 #endif
 
diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp
index 240211ed90..71077d8be8 100644
--- a/ArduPlane/altitude.cpp
+++ b/ArduPlane/altitude.cpp
@@ -601,6 +601,7 @@ float Plane::rangefinder_correction(void)
 void Plane::rangefinder_height_update(void)
 {
     float distance = rangefinder.distance_cm()*0.01f;
+    
     if ((rangefinder.status() == RangeFinder::RangeFinder_Good) && home_is_set != HOME_UNSET) {
         if (!rangefinder_state.have_initial_reading) {
             rangefinder_state.have_initial_reading = true;
@@ -616,9 +617,14 @@ void Plane::rangefinder_height_update(void)
         // catch Lidars that are giving a constant range, either due
         // to misconfiguration or a faulty sensor
         if (rangefinder_state.in_range_count < 10) {
-            if (fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_cm()*0.01f) {
+            if (!is_equal(distance, rangefinder_state.last_distance) &&
+                fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_cm()*0.01f) {
                 rangefinder_state.in_range_count++;
             }
+            if (fabsf(rangefinder_state.last_distance - distance) > rangefinder.max_distance_cm()*0.01*0.2) {
+                // changes by more than 20% of full range will reset counter
+                rangefinder_state.in_range_count = 0;
+            }
         } else {
             rangefinder_state.in_range = true;
             if (!rangefinder_state.in_use &&
@@ -633,6 +639,7 @@ void Plane::rangefinder_height_update(void)
                 gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate);
             }
         }
+        rangefinder_state.last_distance = distance;
     } else {
         rangefinder_state.in_range_count = 0;
         rangefinder_state.in_range = false;