From 350ed204600a93892c31fcd9715a72d2c65b341c Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Mon, 17 Oct 2016 14:34:00 -0700 Subject: [PATCH] Plane: ensure rangefinder last_time resets ensure rangefinder_state.last_correction_time_ms resets to zero via memset(&rangefinder_state, 0..) --- ArduPlane/altitude.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 53c9d4555a..240211ed90 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -659,8 +659,10 @@ void Plane::rangefinder_height_update(void) rangefinder_state.correction = correction; rangefinder_state.initial_correction = correction; auto_state.initial_land_slope = auto_state.land_slope; + rangefinder_state.last_correction_time_ms = now; } else { rangefinder_state.correction = 0.8f*rangefinder_state.correction + 0.2f*correction; + rangefinder_state.last_correction_time_ms = now; if (fabsf(rangefinder_state.correction - rangefinder_state.initial_correction) > 30) { // the correction has changed by more than 30m, reset use of Lidar. We may have a bad lidar if (rangefinder_state.in_use) { @@ -669,7 +671,7 @@ void Plane::rangefinder_height_update(void) memset(&rangefinder_state, 0, sizeof(rangefinder_state)); } } - rangefinder_state.last_correction_time_ms = now; + } } #endif