Copter: rangefinder glitch detection moved to read_rangefinder

ArduCopter/Copter.h

@@ -288,7 +288,9 @@ 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;
+        int16_t alt_cm_glitch_protected;    // last glitch protected altitude
+        int8_t glitch_count;    // non-zero number indicates rangefinder is glitching
+        uint32_t glitch_cleared_ms; // system time glitch cleared
     } rangefinder_state;
 
     class SurfaceTracking {
@@ -307,6 +309,7 @@ private:
     private:
         float target_alt_cm;        // desired altitude in cm above the ground
         uint32_t last_update_ms;    // system time of last update to target_alt_cm
+        uint32_t last_glitch_cleared_ms;    // system time of last handle glitch recovery
         bool valid_for_logging;     // true if target_alt_cm is valid for logging
     } surface_tracking;
 

ArduCopter/sensors.cpp

@@ -35,8 +35,29 @@ void Copter::read_rangefinder(void)
     temp_alt = (float)temp_alt * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
  #endif
 
+    // tilt corrected but unfiltered, not glitch protected alt
     rangefinder_state.alt_cm = temp_alt;
 
+    // glitch handling.  rangefinder readings more than RANGEFINDER_GLITCH_ALT_CM from the last good reading
+    // are considered a glitch and glitch_count becomes non-zero
+    // glitches clear after RANGEFINDER_GLITCH_NUM_SAMPLES samples in a row.
+    // glitch_cleared_ms is set so surface tracking (or other consumers) can trigger a target reset
+    const int32_t glitch_cm = rangefinder_state.alt_cm - rangefinder_state.alt_cm_glitch_protected;
+    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;
+        rangefinder_state.alt_cm_glitch_protected = rangefinder_state.alt_cm;
+    }
+    if (abs(rangefinder_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) {
+        // clear glitch and record time so consumers (i.e. surface tracking) can reset their target altitudes
+        rangefinder_state.glitch_count = 0;
+        rangefinder_state.alt_cm_glitch_protected = rangefinder_state.alt_cm;
+        rangefinder_state.glitch_cleared_ms = AP_HAL::millis();
+    }
+
     // filter rangefinder for use by AC_WPNav
     uint32_t now = AP_HAL::millis();
 

ArduCopter/surface_tracking.cpp

@@ -6,8 +6,9 @@
 float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
 {
 #if RANGEFINDER_ENABLED == ENABLED
-    if (!copter.rangefinder_alt_ok()) {
+    // if rangefinder alt is not ok or glitching, do not do surface tracking
-        // if rangefinder is not ok, do not use surface tracking
+    if (!copter.rangefinder_alt_ok() ||
+        (copter.rangefinder_state.glitch_count != 0)) {
         return target_rate;
     }
 
@@ -18,6 +19,7 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
     const uint32_t now = millis();
     if (now - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
         target_alt_cm = copter.rangefinder_state.alt_cm + current_alt_target - current_alt;
+        last_glitch_cleared_ms = copter.rangefinder_state.glitch_cleared_ms;
     }
     last_update_ms = now;
 
@@ -27,30 +29,11 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
     }
     valid_for_logging = true;
 
-    /*
+    // handle glitch recovery by resetting target
-      handle rangefinder glitches. When we get a rangefinder reading
+    if (copter.rangefinder_state.glitch_cleared_ms != last_glitch_cleared_ms) {
-      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
-     */
-    const int32_t glitch_cm = copter.rangefinder_state.alt_cm - target_alt_cm;
-    if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) {
-        copter.rangefinder_state.glitch_count = MAX(copter.rangefinder_state.glitch_count+1,1);
-    } else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) {
-        copter.rangefinder_state.glitch_count = MIN(copter.rangefinder_state.glitch_count-1,-1);
-    } else {
-        copter.rangefinder_state.glitch_count = 0;
-    }
-    if (abs(copter.rangefinder_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) {
         // shift to the new rangefinder reading
         target_alt_cm = copter.rangefinder_state.alt_cm;
-        copter.rangefinder_state.glitch_count = 0;
+        last_glitch_cleared_ms = copter.rangefinder_state.glitch_cleared_ms;
-    }
-    if (copter.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)