From cdc42c1de067c20c477ca3d4e13f3066cedc81d7 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 8 Feb 2016 14:15:41 +1100
Subject: [PATCH] EKF: Add simple control logic for magnetic declination
 constraint

---
 EKF/control.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/EKF/control.cpp b/EKF/control.cpp
index 64ca3820e1..23df635ec5 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -109,6 +109,13 @@ void Ekf::controlFusionModes()
 			_control_status.flags.mag_3D = false;
 		}
 	}
+
+        // if we are using 3-axis magnetometer fusion, but without external aiding, then the declination needs to be fused as an observation to prevent long term heading drift
+        if(_control_status.flags.mag_3D && _control_status.flags.gps) {
+            _control_status.flags.mag_dec = false;
+        } else {
+            _control_status.flags.mag_dec = true;
+        }
 }
 
 void Ekf::calculateVehicleStatus()