From 521b9f5dccff3c6f1f5cb408d9a3c71892457d63 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 27 Jul 2021 10:15:46 +0200 Subject: [PATCH] control: do not constantly ask for mag reset if yaw not aligned This also prevents triggering the `_mag_yaw_reset_req` flag in magless mode; preventing the GNSS fustion from starting. --- src/modules/ekf2/EKF/control.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 3413991f41..462af77631 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -529,8 +529,7 @@ void Ekf::controlGpsFusion() // If the heading is not aligned, reset the yaw and magnetic field states // Do not use external vision for yaw if using GPS because yaw needs to be // defined relative to an NED reference frame - if (!_control_status.flags.yaw_align - || _control_status.flags.ev_yaw + if (_control_status.flags.ev_yaw || _mag_inhibit_yaw_reset_req || _mag_yaw_reset_req) { @@ -540,7 +539,7 @@ void Ekf::controlGpsFusion() stopEvYawFusion(); _inhibit_ev_yaw_use = true; - } else { + } else if (_control_status.flags.yaw_align) { // If the heading is valid start using gps aiding startGpsFusion(); }