Clean euler321 and euler312 code and comment

EKF/EKFGSF_yaw.cpp

@@ -224,17 +224,12 @@ void EKFGSF_yaw::ahrsAlignYaw()
 {
 	// Align yaw angle for each model
 	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
-		if (shouldUse321RotationSequence(_ahrs_ekf_gsf[model_index].R)) {
+		Dcmf& R = _ahrs_ekf_gsf[model_index].R;
-			// update the rotation matrix with 321 rotation sequence
+		const float yaw = wrap_pi(_ekf_gsf[model_index].X(2));
-			_ahrs_ekf_gsf[model_index].R = updateEuler321YawInRotMat(wrap_pi(_ekf_gsf[model_index].X(2)),
+		R = shouldUse321RotationSequence(R) ?
-										_ahrs_ekf_gsf[model_index].R);
+		    updateEuler321YawInRotMat(yaw, R) :
+		    updateEuler312YawInRotMat(yaw, R);
 
-		} else {
-			// update the rotation matrix with 312 rotation sequence
-			_ahrs_ekf_gsf[model_index].R = updateEuler312YawInRotMat(wrap_pi(_ekf_gsf[model_index].X(2)),
-										_ahrs_ekf_gsf[model_index].R);
-
-		}
 		_ahrs_ekf_gsf[model_index].aligned = true;
 	}
 }
@@ -250,11 +245,10 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
 	}
 
 	// Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock
-	if (shouldUse321RotationSequence(_ahrs_ekf_gsf[model_index].R)) {
+	const Dcmf& R = _ahrs_ekf_gsf[model_index].R;
-		_ekf_gsf[model_index].X(2) = getEuler321Yaw(_ahrs_ekf_gsf[model_index].R);
+	_ekf_gsf[model_index].X(2) = shouldUse321RotationSequence(R) ?
-	} else {
+					getEuler321Yaw(R) :
-		_ekf_gsf[model_index].X(2) = getEuler312Yaw(_ahrs_ekf_gsf[model_index].R);
+					getEuler312Yaw(R);
-	}
 
 	// calculate delta velocity in a horizontal front-right frame
 	const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * _delta_vel;

EKF/ekf_helper.cpp

@@ -481,16 +481,9 @@ 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;
+		const Dcmf R_to_earth = shouldUse321RotationSequence(_R_to_earth) ?
-		if (shouldUse321RotationSequence(_R_to_earth)) {
+						updateEuler321YawInRotMat(0.f, _R_to_earth) :
-			// rolled more than pitched so use 321 rotation order
+						updateEuler312YawInRotMat(0.f, _R_to_earth);
-			R_to_earth = updateEuler321YawInRotMat(0.f, _R_to_earth);
-
-		} else {
-			// pitched more than rolled so use 312 rotation order
-			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;
@@ -1647,16 +1640,9 @@ 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
-	// determine if a 321 or 312 Euler sequence is best
+	_R_to_earth = shouldUse321RotationSequence(_R_to_earth) ?
-	if (shouldUse321RotationSequence(_R_to_earth)) {
+			updateEuler321YawInRotMat(yaw, _R_to_earth) :
-		_R_to_earth = updateEuler321YawInRotMat(yaw, _R_to_earth);
+			updateEuler312YawInRotMat(yaw, _R_to_earth);
-
-	} else {
-		// We use a 312 sequence as an alternate when there is more pitch tilt than roll tilt
-		// to avoid gimbal lock
-		_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/mag_fusion.cpp

@@ -721,9 +721,7 @@ void Ekf::fuseHeading()
 	// update transformation matrix from body to world frame using the current state estimate
 	_R_to_earth = Dcmf(_state.quat_nominal);
 
-	// determine if a 321 or 312 Euler sequence is best
 	if (shouldUse321RotationSequence(_R_to_earth)) {
-		// rolled more than pitched so use 321 rotation order to calculate the observed yaw angle
 		const float predicted_hdg = getEuler321Yaw(_R_to_earth);
 
 		if (_control_status.flags.mag_hdg) {
@@ -741,7 +739,6 @@ void Ekf::fuseHeading()
 			measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
 
 		} else if (_control_status.flags.ev_yaw) {
-			// calculate the yaw angle for a 321 sequence
 			measured_hdg = getEuler321Yaw(_ev_sample_delayed.quat);
 
 		} else {
@@ -781,16 +778,13 @@ void Ekf::fuseHeading()
 		fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov);
 
 	} else {
-		// pitched more than rolled so use 312 rotation order to calculate the observed yaw angle
 		const float predicted_hdg = getEuler312Yaw(_R_to_earth);
 
 		if (_control_status.flags.mag_hdg) {
 
-			// Calculate the body to earth frame rotation matrix from the euler angles
+			// rotate the magnetometer measurements into earth frame using a zero yaw angle
-			// using a 312 rotation sequence with yaw angle set to to zero
 			const Dcmf R_to_earth = updateEuler312YawInRotMat(0.f, _R_to_earth);
 
-			// rotate the magnetometer measurements into earth frame using a zero yaw angle
 			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;

EKF/utils.hpp

@@ -23,6 +23,9 @@ 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);
 
+// We should use a 3-2-1 Tait-Bryan (yaw-pitch-roll) rotation sequence
+// when there is more roll than pitch tilt and a 3-1-2 rotation sequence
+// when there is more pitch than roll tilt to avoid gimbal lock.
 bool shouldUse321RotationSequence(const matrix::Dcmf& R);
 
 float getEuler321Yaw(const matrix::Quatf& q);