From 938c8ad9ad63600240cffc7b394ad1abbe3d58e2 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Mon, 4 Feb 2019 09:57:23 +0100 Subject: [PATCH] EKF: use hagl estimate if valid for when to trigger yaw reset on takeoff --- EKF/control.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 2ef23f0a5c..13f403c3b1 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1385,7 +1385,8 @@ void Ekf::controlMagFusion() if (!_control_status.flags.mag_align_complete) { // Check if height has increased sufficiently to be away from ground magnetic anomalies // and request a yaw reset if not already requested. - _mag_yaw_reset_req |= ((_last_on_ground_posD - _state.pos(2)) > 1.5f); + float terrain_vpos_estimate = get_terrain_valid() ? _terrain_vpos : _last_on_ground_posD; + _mag_yaw_reset_req |= (terrain_vpos_estimate - _state.pos(2)) > 1.5f; } // perform a yaw reset if requested by other functions