From 264c8c4e8681704e4719d0a03b848df8617c0863 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 20 Aug 2020 16:25:41 +0200 Subject: [PATCH] mag_fusion: mag heading and 3d modes are mutually exclusive There is no need to check if 3d fusion is active when in mag heading fusion mode since this is impossible --- EKF/mag_fusion.cpp | 16 ++-------------- 1 file changed, 2 insertions(+), 14 deletions(-) diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 3c93fb8c19..600b86c8d8 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -727,13 +727,7 @@ void Ekf::fuseHeading() if (_control_status.flags.mag_hdg) { // Rotate the measurements into earth frame using the zero yaw angle const Dcmf R_to_earth = updateEuler321YawInRotMat(0.f, _R_to_earth); - if (_control_status.flags.mag_3D) { - // don't apply bias corrections if we are learning them - mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; - - } else { - mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B); - } + mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B); // the angle of the projection onto the horizontal gives the yaw angle measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); @@ -784,13 +778,7 @@ void Ekf::fuseHeading() // rotate the magnetometer measurements into earth frame using a zero yaw angle const Dcmf R_to_earth = updateEuler312YawInRotMat(0.f, _R_to_earth); - - if (_control_status.flags.mag_3D) { - // don't apply bias corrections if we are learning them - mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; - } else { - mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B); - } + mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B); // the angle of the projection onto the horizontal gives the yaw angle measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();