From 9797e4d28f8e17f6666c8712687c0e64718c96c4 Mon Sep 17 00:00:00 2001
From: kamilritz <kritz@ethz.ch>
Date: Sat, 22 Aug 2020 09:36:31 +0200
Subject: [PATCH] updateYawInRotMat with hidden rotation sequence handling

---
 EKF/EKFGSF_yaw.cpp | 4 +---
 EKF/ekf_helper.cpp | 8 ++------
 EKF/utils.cpp      | 6 ++++++
 EKF/utils.hpp      | 3 +++
 4 files changed, 12 insertions(+), 9 deletions(-)

diff --git a/EKF/EKFGSF_yaw.cpp b/EKF/EKFGSF_yaw.cpp
index cf54820c92..ae50db5eac 100644
--- a/EKF/EKFGSF_yaw.cpp
+++ b/EKF/EKFGSF_yaw.cpp
@@ -226,9 +226,7 @@ void EKFGSF_yaw::ahrsAlignYaw()
 	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
 		Dcmf& R = _ahrs_ekf_gsf[model_index].R;
 		const float yaw = wrap_pi(_ekf_gsf[model_index].X(2));
-		R = shouldUse321RotationSequence(R) ?
-		    updateEuler321YawInRotMat(yaw, R) :
-		    updateEuler312YawInRotMat(yaw, R);
+		R = updateYawInRotMat(yaw, R);
 
 		_ahrs_ekf_gsf[model_index].aligned = true;
 	}
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index d35b6b658d..c0c8f4fa25 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -481,9 +481,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
 
 	} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
 		// rotate the magnetometer measurements into earth frame using a zero yaw angle
-		const Dcmf R_to_earth = shouldUse321RotationSequence(_R_to_earth) ?
-						updateEuler321YawInRotMat(0.f, _R_to_earth) :
-						updateEuler312YawInRotMat(0.f, _R_to_earth);
+		const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
 
 		// the angle of the projection onto the horizontal gives the yaw angle
 		const Vector3f mag_earth_pred = R_to_earth * mag_init;
@@ -1640,9 +1638,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
 	_R_to_earth = Dcmf(_state.quat_nominal);
 
 	// update the rotation matrix using the new yaw value
-	_R_to_earth = shouldUse321RotationSequence(_R_to_earth) ?
-			updateEuler321YawInRotMat(yaw, _R_to_earth) :
-			updateEuler312YawInRotMat(yaw, _R_to_earth);
+	_R_to_earth = updateYawInRotMat(yaw, _R_to_earth);
 
 	// calculate the amount that the quaternion has changed by
 	const Quatf quat_after_reset(_R_to_earth);
diff --git a/EKF/utils.cpp b/EKF/utils.cpp
index fd7c2425f8..e87566f391 100644
--- a/EKF/utils.cpp
+++ b/EKF/utils.cpp
@@ -99,3 +99,9 @@ matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf& rot_in) {
 				atan2f(-rot_in(2, 0), rot_in(2, 2)));  // pitch
 	return taitBryan312ToRotMat(rotVec312);
 }
+
+matrix::Dcmf updateYawInRotMat(float yaw, const matrix::Dcmf& rot_in) {
+	return shouldUse321RotationSequence(rot_in) ?
+		updateEuler321YawInRotMat(yaw, rot_in) :
+		updateEuler312YawInRotMat(yaw, rot_in);
+}
diff --git a/EKF/utils.hpp b/EKF/utils.hpp
index a718a6df6e..23b10516bb 100644
--- a/EKF/utils.hpp
+++ b/EKF/utils.hpp
@@ -37,6 +37,9 @@ float getEuler312Yaw(const matrix::Dcmf& R);
 matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf& rot_in);
 matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf& rot_in);
 
+// Checks which euler rotation sequence to use and update yaw in rotation matrix
+matrix::Dcmf updateYawInRotMat(float yaw, const matrix::Dcmf& rot_in);
+
 namespace ecl{
 	inline float powf(float x, int exp)
 	{