From bcea3ac8d4e224661acdcd80c9318b77df30d342 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 20 Nov 2016 17:26:51 +1100
Subject: [PATCH] Copter: handle rangefinder glitches in alt tracking

this handles glitches from the rangefinder when tracking the
ground. It requires 3 readings in a row to settle on a new target
altitude

thanks to discussions with Leonard and Randy
---
 ArduCopter/Attitude.cpp | 28 +++++++++++++++++++++++++---
 ArduCopter/Copter.h     |  1 +
 ArduCopter/config.h     |  8 ++++++++
 3 files changed, 34 insertions(+), 3 deletions(-)

diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp
index e3d7a16bb4..8ecea48481 100644
--- a/ArduCopter/Attitude.cpp
+++ b/ArduCopter/Attitude.cpp
@@ -244,9 +244,31 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current
         target_rangefinder_alt += target_rate * dt;
     }
 
-    // do not let target altitude get too far from current altitude above ground
-    // Note: the 750cm limit is perhaps too wide but is consistent with the regular althold limits and helps ensure a smooth transition
-    target_rangefinder_alt = constrain_float(target_rangefinder_alt,rangefinder_state.alt_cm-pos_control.get_leash_down_z(),rangefinder_state.alt_cm+pos_control.get_leash_up_z());
+    /*
+      handle rangefinder glitches. When we get a rangefinder reading
+      more than RANGEFINDER_GLITCH_ALT_CM different from the current
+      rangefinder reading then we consider it a glitch and reject
+      until we get RANGEFINDER_GLITCH_NUM_SAMPLES samples in a
+      row. When that happens we reset the target altitude to the new
+      reading
+     */
+    int32_t glitch_cm = rangefinder_state.alt_cm - target_rangefinder_alt;
+    if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) {
+        rangefinder_state.glitch_count = MAX(rangefinder_state.glitch_count+1,1);
+    } else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) {
+        rangefinder_state.glitch_count = MIN(rangefinder_state.glitch_count-1,-1);
+    } else {
+        rangefinder_state.glitch_count = 0;
+    }
+    if (abs(rangefinder_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) {
+        // shift to the new rangefinder reading
+        target_rangefinder_alt = rangefinder_state.alt_cm;
+        rangefinder_state.glitch_count = 0;
+    }
+    if (rangefinder_state.glitch_count != 0) {
+        // we are currently glitching, just use the target rate
+        return target_rate;
+    }
 
     // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
     distance_error = (target_rangefinder_alt - rangefinder_state.alt_cm) - (current_alt_target - current_alt);
diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index be2e81b3fa..93e7c465a5 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -195,6 +195,7 @@ private:
         int16_t alt_cm;     // tilt compensated altitude (in cm) from rangefinder
         uint32_t last_healthy_ms;
         LowPassFilterFloat alt_cm_filt; // altitude filter
+        int8_t glitch_count;
     } rangefinder_state = { false, false, 0, 0 };
 
     AP_RPM rpm_sensor;
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index 7c157ad519..2ce2aa8502 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -151,6 +151,14 @@
  # define RANGEFINDER_TILT_CORRECTION ENABLED
 #endif
 
+#ifndef RANGEFINDER_GLITCH_ALT_CM
+ # define RANGEFINDER_GLITCH_ALT_CM  200      // amount of rangefinder change to be considered a glitch
+#endif
+
+#ifndef RANGEFINDER_GLITCH_NUM_SAMPLES
+ # define RANGEFINDER_GLITCH_NUM_SAMPLES  3   // number of rangefinder glitches in a row to take new reading
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 // Proximity sensor
 //