From 8e095d4a8174f13cc69c283347726ba560abdb84 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 26 Jun 2019 11:16:30 +0900 Subject: [PATCH] Copter: avoid terrain failsafe from a single out-of-range rangefinder reading --- ArduCopter/sensors.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 40b0c120ec..07e899320d 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -40,8 +40,10 @@ void Copter::read_rangefinder(void) // filter rangefinder for use by AC_WPNav uint32_t now = AP_HAL::millis(); + const bool timed_out = now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS; + if (rangefinder_state.alt_healthy) { - if (now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS) { + if (timed_out) { // reset filter if we haven't used it within the last second rangefinder_state.alt_cm_filt.reset(rangefinder_state.alt_cm); } else { @@ -51,7 +53,9 @@ void Copter::read_rangefinder(void) } // send rangefinder altitude and health to waypoint navigation library - wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get()); + if (rangefinder_state.alt_healthy || timed_out) { + wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get()); + } #else rangefinder_state.enabled = false;