From 5b5bddebea9b3dd4caf2334f667596e7ae522f6b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 24 Feb 2016 20:17:22 +1100 Subject: [PATCH] EKF: Always reset yaw and mag field states on entry into 3-axis mag fusion mode --- EKF/control.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/EKF/control.cpp b/EKF/control.cpp index 7674d66d46..0b482fa325 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -112,6 +112,11 @@ void Ekf::controlFusionModes() } else { if (_control_status.flags.in_air) { + // if transitioning from a non-3D fusion mode, we need to initialise the yaw angle and field states + if (!_control_status.flags.mag_3D) { + _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + } + // always use 3D mag fusion when airborne _control_status.flags.mag_hdg = false; _control_status.flags.mag_2D = false; @@ -145,6 +150,11 @@ void Ekf::controlFusionModes() _control_status.flags.mag_3D = false; } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) { + // if transitioning from a non-3D fusion mode, we need to initialise the yaw angle and field states + if (!_control_status.flags.mag_3D) { + _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + } + // always use 3-axis mag fusion _control_status.flags.mag_hdg = false; _control_status.flags.mag_2D = false;