From ccfd89240b72c24a0ebb965c29de9a5bee72ecf7 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Tue, 15 Dec 2020 07:54:47 +1100
Subject: [PATCH] AP_NavEKF3: Fix error in calculation of static 312 order yaw
 reference

---
 libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
index 1a2e4ece61..037d65fba5 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
@@ -246,7 +246,7 @@ void NavEKF3_core::SelectMagFusion()
                     if (yawAngDataDelayed.type == 2) {
                         yawAngDataDelayed.yawAng = atan2f(prevTnb[0][1], prevTnb[0][0]);
                     } else if (yawAngDataDelayed.type == 1) {
-                        yawAngDataDelayed.yawAng = atan2f(-prevTnb[0][1], prevTnb[1][1]);
+                        yawAngDataDelayed.yawAng = atan2f(-prevTnb[1][0], prevTnb[1][1]);
                     }
                 }
                 if (onGroundNotMoving) {
@@ -290,7 +290,7 @@ void NavEKF3_core::SelectMagFusion()
             } else {
                 // A 312 rotation order is best conditioned because the Y axis is closer to horizontal than the X axis
                 if (!onGroundNotMoving) {
-                    yawAngDataDelayed.yawAng = atan2f(-prevTnb[0][1], prevTnb[1][1]);
+                    yawAngDataDelayed.yawAng = atan2f(-prevTnb[1][0], prevTnb[1][1]);
                 }
                 yawAngDataDelayed.type = 1;
             }