From 7c83ac4b6d6685444675765506973eea19cbf59e Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 1 Mar 2016 15:35:17 +1100 Subject: [PATCH] EKF: Use heading fusion as default for startup and ground operation --- EKF/control.cpp | 25 +++++++++---------------- 1 file changed, 9 insertions(+), 16 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 0b482fa325..6a53301c5f 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -105,9 +105,9 @@ void Ekf::controlFusionModes() // or the more accurate 3-axis fusion if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) { if (!_control_status.flags.armed) { - // always use 2D mag fusion for initial startup - _control_status.flags.mag_hdg = false; - _control_status.flags.mag_2D = true; + // use heading fusion for initial startup + _control_status.flags.mag_hdg = true; + _control_status.flags.mag_2D = false; _control_status.flags.mag_3D = false; } else { @@ -123,24 +123,17 @@ void Ekf::controlFusionModes() _control_status.flags.mag_3D = true; } else { - // always use 2D mag fusion when on the ground - _control_status.flags.mag_hdg = false; - _control_status.flags.mag_2D = true; + // use heading fusion when on the ground + _control_status.flags.mag_hdg = true; + _control_status.flags.mag_2D = false; _control_status.flags.mag_3D = false; } } } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) { - // always use yaw fusion unless tilt is over 45 deg, otherwise use 2D fusion - if (_R_prev(2, 2) > 0.7071f) { - _control_status.flags.mag_hdg = true; - _control_status.flags.mag_2D = false; - - } else { - _control_status.flags.mag_hdg = false; - _control_status.flags.mag_2D = true; - } - + // always use heading fusion + _control_status.flags.mag_hdg = true; + _control_status.flags.mag_2D = false; _control_status.flags.mag_3D = false; } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_2D) {