From 15afa8ae17d91a0c2ab8849f2ea34e3b696f0b4c Mon Sep 17 00:00:00 2001
From: Kamil Ritz <kritz@ethz.ch>
Date: Sat, 15 Aug 2020 13:43:55 +0200
Subject: [PATCH] shouldUse321RotationSequence(const Dcmf& R)

---
 EKF/EKFGSF_yaw.cpp | 4 ++--
 EKF/ekf_helper.cpp | 4 ++--
 EKF/mag_fusion.cpp | 2 +-
 EKF/utils.cpp      | 4 ++++
 EKF/utils.hpp      | 1 +
 5 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/EKF/EKFGSF_yaw.cpp b/EKF/EKFGSF_yaw.cpp
index e30f13e4e1..32020d3000 100644
--- a/EKF/EKFGSF_yaw.cpp
+++ b/EKF/EKFGSF_yaw.cpp
@@ -224,7 +224,7 @@ void EKFGSF_yaw::ahrsAlignYaw()
 {
 	// Align yaw angle for each model
 	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
-		if (fabsf(_ahrs_ekf_gsf[model_index].R(2, 0)) < fabsf(_ahrs_ekf_gsf[model_index].R(2, 1))) {
+		if (shouldUse321RotationSequence(_ahrs_ekf_gsf[model_index].R)) {
 			// get the roll, pitch, yaw estimates from the rotation matrix using a  321 Tait-Bryan rotation sequence
 			Eulerf euler_init(_ahrs_ekf_gsf[model_index].R);
 
@@ -260,7 +260,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
 	}
 
 	// Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock
-	if (fabsf(_ahrs_ekf_gsf[model_index].R(2, 0)) < fabsf(_ahrs_ekf_gsf[model_index].R(2, 1))) {
+	if (shouldUse321RotationSequence(_ahrs_ekf_gsf[model_index].R)) {
 		// use 321 Tait-Bryan rotation to define yaw state
 		_ekf_gsf[model_index].X(2) = atan2f(_ahrs_ekf_gsf[model_index].R(1, 0), _ahrs_ekf_gsf[model_index].R(0, 0));
 	} else {
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index d31f0a8055..76ec1a63a4 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -486,7 +486,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
 		Dcmf R_to_earth;
-		if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
+		if (shouldUse321RotationSequence(_R_to_earth)) {
 			// rolled more than pitched so use 321 rotation order
 			Eulerf euler321(_state.quat_nominal);
 			euler321(2) = 0.0f;
@@ -1658,7 +1658,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
 
 	// update the rotation matrix using the new yaw value
 	// determine if a 321 or 312 Euler sequence is best
-	if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
+	if (shouldUse321RotationSequence(_R_to_earth)) {
 		// use a 321 sequence
 		Eulerf euler321(_R_to_earth);
 		euler321(2) = yaw;
diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp
index 9cb34580be..69393c3731 100644
--- a/EKF/mag_fusion.cpp
+++ b/EKF/mag_fusion.cpp
@@ -723,7 +723,7 @@ void Ekf::fuseHeading()
 	_R_to_earth = Dcmf(_state.quat_nominal);
 
 	// determine if a 321 or 312 Euler sequence is best
-	if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
+	if (shouldUse321RotationSequence(_R_to_earth)) {
 		// rolled more than pitched so use 321 rotation order to calculate the observed yaw angle
 		Eulerf euler321(_state.quat_nominal);
 		predicted_hdg = euler321(2);
diff --git a/EKF/utils.cpp b/EKF/utils.cpp
index 4f1791dc9c..903f8f6a11 100644
--- a/EKF/utils.cpp
+++ b/EKF/utils.cpp
@@ -58,3 +58,7 @@ matrix::Dcmf quatToInverseRotMat(const  matrix::Quatf &quat)
 
 	return dcm;
 }
+
+bool shouldUse321RotationSequence(const matrix::Dcmf& R) {
+	return fabsf(R(2, 0)) < fabsf(R(2, 1));
+}
\ No newline at end of file
diff --git a/EKF/utils.hpp b/EKF/utils.hpp
index 66884e1485..60debe6468 100644
--- a/EKF/utils.hpp
+++ b/EKF/utils.hpp
@@ -20,6 +20,7 @@ float kahanSummation(float sum_previous, float input, float &accumulator);
 // this produces the inverse rotation to that produced by the math library quaternion to Dcmf operator
 matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat);
 
+bool shouldUse321RotationSequence(const matrix::Dcmf& R);
 namespace ecl{
 	inline float powf(float x, int exp)
 	{