updateYawInRotMat with hidden rotation sequence handling

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) ?
+		R = updateYawInRotMat(yaw, R);
-		    updateEuler321YawInRotMat(yaw, R) :
-		    updateEuler312YawInRotMat(yaw, R);
 
 		_ahrs_ekf_gsf[model_index].aligned = true;
 	}

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) ?
+		const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
-						updateEuler321YawInRotMat(0.f, _R_to_earth) :
-						updateEuler312YawInRotMat(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) ?
+	_R_to_earth = updateYawInRotMat(yaw, _R_to_earth);
-			updateEuler321YawInRotMat(yaw, _R_to_earth) :
-			updateEuler312YawInRotMat(yaw, _R_to_earth);
 
 	// calculate the amount that the quaternion has changed by
 	const Quatf quat_after_reset(_R_to_earth);

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

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