From d5d7fbe7614b10e2a6741be28d649bb1d16fb67d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 22 Aug 2019 21:05:45 +0900 Subject: [PATCH] Copter: rangefinder glitch detection moved to read_rangefinder --- ArduCopter/Copter.h | 5 ++++- ArduCopter/sensors.cpp | 21 +++++++++++++++++++++ ArduCopter/surface_tracking.cpp | 31 +++++++------------------------ 3 files changed, 32 insertions(+), 25 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 5084c22cc1..5b12a6028f 100644 --- a/ArduCopter/Copter.h +++ b/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; diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 451a28196d..e1de784f09 100644 --- a/ArduCopter/sensors.cpp +++ b/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(); diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp index a7f77ce15e..da90cfb160 100644 --- a/ArduCopter/surface_tracking.cpp +++ b/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 is not ok, do not use surface tracking + // if rangefinder alt is not ok or glitching, do not do 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 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 - */ - 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) { + // handle glitch recovery by resetting target + if (copter.rangefinder_state.glitch_cleared_ms != last_glitch_cleared_ms) { // shift to the new rangefinder reading target_alt_cm = copter.rangefinder_state.alt_cm; - copter.rangefinder_state.glitch_count = 0; - } - if (copter.rangefinder_state.glitch_count != 0) { - // we are currently glitching, just use the target rate - return target_rate; + last_glitch_cleared_ms = copter.rangefinder_state.glitch_cleared_ms; } // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)