From 628079719f9afdc54b8cdc54a05df3eebaf5401d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 17 Jul 2018 08:03:12 +1000 Subject: [PATCH] EKF: Fix inconsistency in timeout for terrain validity Terrain validity is determined solely by successful range finder fusion and terrain state initialisation. A range finder that has been declared faulty requires continuous range finder data fusion requires data to be continuous before the fault status _rng_hgt_faulty can be cleared. This will enforce the requirement for continuous data before fusion can commence. --- EKF/control.cpp | 5 +++++ EKF/terrain_estimator.cpp | 3 +-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 7c49d5ca3f..b7c8c41fda 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1144,6 +1144,11 @@ void Ekf::checkRangeDataValidity() return; } + // Don't allow faulty flag to clear unless range data is continuous + if (_rng_hgt_faulty && !_range_data_continuous) { + return; + } + // Don't run the checks after this unless we have retrieved new data from the buffer if (!_range_data_ready) { return; diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index b080093205..16cb9a29ee 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -171,8 +171,7 @@ bool Ekf::get_terrain_valid() // determine terrain validity void Ekf::update_terrain_valid() { - if (_terrain_initialised && _range_data_continuous && !_control_status.flags.rng_stuck && - (_time_last_imu - _time_last_hagl_fuse < (uint64_t)5e6)) { + if (_terrain_initialised && (_time_last_imu - _time_last_hagl_fuse < (uint64_t)5e6)) { _hagl_valid = true;