From 46b4c55fdc1d91b2879f476e46eaf0e49d162c2d Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Sat, 3 Jun 2017 20:03:51 +1000
Subject: [PATCH] AP_NavEKF2: Fix bug in declination fusion

---
 libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
index 309317047e..1d1e79ae9b 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
@@ -1006,7 +1006,7 @@ void NavEKF2_core::FuseDeclination(float declErr)
     float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
 
     // Calculate the innovation
-    float innovation = atanf(t4) - magDecAng;
+    float innovation = atan2f(magE , magN) - magDecAng;
 
     // limit the innovation to protect against data errors
     if (innovation > 0.5f) {