AP_NavEKF3: Fix bug in calculation of rotation order

5 years ago · a3725e2581
1 changed files with 1 additions and 1 deletions
--- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
@ -287,7 +287,7 @@ void NavEKF3_core::SelectMagFusion()
				@@ -287,7 +287,7 @@ void NavEKF3_core::SelectMagFusion()
                    // A 321 rotation order is best conditioned because the X axis is closer to horizontal than the Y axis
                    yawAngDataDelayed.yawAng = atan2f(prevTnb[0][1], prevTnb[0][0]);
                    yawAngDataDelayed.type = 2;
-                } else if (yawAngDataDelayed.type == 1) {
+                } else {
                    // A 312 rotation order is best conditioned because the Y axis is closer to horizontal than the X axis
                    yawAngDataDelayed.yawAng = atan2f(-prevTnb[0][1], prevTnb[1][1]);
                    yawAngDataDelayed.type = 1;