From a7b8afe420f438554ad90bcba0f1f4872325e75b Mon Sep 17 00:00:00 2001 From: Eike Date: Fri, 30 Apr 2021 20:17:39 +0200 Subject: [PATCH] Allow rangefinder fusion in vision height mode (Fix for #994) (#999) Co-authored-by: Daniel Agar --- EKF/control.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 06761d4d1c..2a696ee78b 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -788,8 +788,7 @@ void Ekf::controlHeightSensorTimeouts() // check if height has been inertial deadreckoning for too long // in vision hgt mode check for vision data - const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6) || - (_control_status.flags.ev_hgt && !isRecent(_time_last_ext_vision, 5 * EV_MAX_INTERVAL)); + const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6); if (hgt_fusion_timeout || continuous_bad_accel_hgt) { @@ -1079,15 +1078,13 @@ void Ekf::controlHeightFusion() } } - if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { - // switch to baro if there was a reset to baro - fuse_height = true; - } - - // determine if we should use the vertical position observation - if (_control_status.flags.ev_hgt) { - fuse_height = true; - } + if (_control_status.flags.ev_hgt && _ev_data_ready) { + fuse_height = true; + } else if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) { + fuse_height = true; + } else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { + fuse_height = true; + } break; }