From a74b7ef60bc3b6c7fc081c051eeeb9e2f5e6f863 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 7 Nov 2018 11:55:45 +0100 Subject: [PATCH] 3D only mag fusion: make sure _flt_mag_align_complete is set - _flt_mag_align_complete was not set when choosing pure 3D mag fusion. one effect of this was that the declination used in the filter was not the one calculated from the magnetic field states. Signed-off-by: Roman --- EKF/control.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 0db174f680..0aeae63a99 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1494,13 +1494,14 @@ void Ekf::controlMagFusion() } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) { // if transitioning into 3-axis 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) || _control_status.flags.yaw_align; + if (!_control_status.flags.mag_3D || !_flt_mag_align_complete) { + _flt_mag_align_complete= resetMagHeading(_mag_sample_delayed.mag); + _control_status.flags.yaw_align = _control_status.flags.yaw_align || _flt_mag_align_complete; } // always use 3-axis mag fusion + _control_status.flags.mag_3D = _flt_mag_align_complete; _control_status.flags.mag_hdg = false; - _control_status.flags.mag_3D = true; } else { // do no magnetometer fusion at all