From 066c641d6111c164be3ce73132dcb6948b7a6e90 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Mon, 26 Jun 2017 20:16:53 +0200 Subject: [PATCH] range aid: switch to baro/gps even if rng aid conditions met and rng faulty --- EKF/control.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index a4e2a3d9be..140484d0f7 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -730,9 +730,11 @@ void Ekf::controlHeightFusion() } } - } else if (_baro_data_ready && !_baro_hgt_faulty && !_in_range_aid_mode) { + } else if (_baro_data_ready && !_baro_hgt_faulty && + !(_in_range_aid_mode && !_range_data_ready && !_rng_hgt_faulty)) { setControlBaroHeight(); _fuse_height = true; + _in_range_aid_mode = false; // we have just switched to using baro height, we don't need to set a height sensor offset // since we track a separate _baro_hgt_offset @@ -785,9 +787,11 @@ void Ekf::controlHeightFusion() } } - } else if (_gps_data_ready && !_gps_hgt_faulty && !_in_range_aid_mode) { + } else if (_gps_data_ready && !_gps_hgt_faulty && + !(_in_range_aid_mode && !_range_data_ready && !_rng_hgt_faulty)) { setControlGPSHeight(); _fuse_height = true; + _in_range_aid_mode = false; // we have just switched to using gps height, calculate height sensor offset such that current // measurment matches our current height estimate