shouldUse321RotationSequence(const Dcmf& R)

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 {

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;

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);

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));
+}

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)
 	{