From cbf23090238e3d6ca042bbefc94a8fad3c0e16fc Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Mon, 4 Jan 2016 15:57:17 -0800
Subject: [PATCH] AP_AHRS: add get_mag_field_NED and get_mag_field_correction

---
 libraries/AP_AHRS/AP_AHRS.h          | 10 ++++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 46 ++++++++++++++++++++++++++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   |  6 ++++
 3 files changed, 62 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 0d5ad7dcc6..0167403ec6 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -272,6 +272,16 @@ public:
         return false;
     }
 
+    // returns the expected NED magnetic field
+    virtual bool get_expected_mag_field_NED(Vector3f &ret) const {
+        return false;
+    }
+
+    // returns the estimated magnetic field offsets in body frame
+    virtual bool get_mag_field_correction(Vector3f &ret) const {
+        return false;
+    }
+
     // return a position relative to home in meters, North/East/Down
     // order. This will only be accurate if have_inertial_nav() is
     // true
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index bbbf6a9b64..c8366411ab 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -515,6 +515,52 @@ bool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const
     }
 }
 
+// returns the expected NED magnetic field
+bool AP_AHRS_NavEKF::get_mag_field_NED(Vector3f &vec) const
+{
+    switch (active_EKF_type()) {
+        case EKF_TYPE_NONE:
+            return false;
+
+        case EKF_TYPE1:
+        default:
+            EKF1.getMagNED(vec);
+            return true;
+
+        case EKF_TYPE2:
+            EKF2.getMagNED(-1,vec);
+            return true;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+        case EKF_TYPE_SITL:
+            return false;
+#endif
+    }
+}
+
+// returns the estimated magnetic field offsets in body frame
+bool AP_AHRS_NavEKF::get_mag_field_correction(Vector3f &vec) const
+{
+    switch (active_EKF_type()) {
+        case EKF_TYPE_NONE:
+            return false;
+
+        case EKF_TYPE1:
+        default:
+            EKF1.getMagXYZ(vec);
+            return true;
+
+        case EKF_TYPE2:
+            EKF2.getMagXYZ(-1,vec);
+            return true;
+
+            #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+        case EKF_TYPE_SITL:
+            return false;
+            #endif
+    }
+}
+
 // Get a derivative of the vertical position which is kinematically consistent with the vertical position is required by some control loops.
 // This is different to the vertical velocity from the EKF which is not always consistent with the verical position due to the various errors that are being corrected for.
 bool AP_AHRS_NavEKF::get_vert_pos_rate(float &velocity)
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index b4ec5ef72b..3740bf1c2f 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -197,6 +197,12 @@ public:
     // boolean false is returned if variances are not available
     bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
 
+    // returns the expected NED magnetic field
+    bool get_mag_field_NED(Vector3f& ret) const;
+
+    // returns the estimated magnetic field offsets in body frame
+    bool get_mag_field_correction(Vector3f &ret) const;
+
     void setTakeoffExpected(bool val);
     void setTouchdownExpected(bool val);