From 14795719f6c9d902d45edb055ff9976ef06e1f3e Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 16 Mar 2015 12:05:45 -0700
Subject: [PATCH] AP_NavEKF: Add public function for estimated magnetometer
 offsets

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 14 ++++++++++++++
 libraries/AP_NavEKF/AP_NavEKF.h   |  4 ++++
 2 files changed, 18 insertions(+)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 53803dafed..2e2fb7cd99 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -3650,6 +3650,20 @@ void NavEKF::getMagXYZ(Vector3f &magXYZ) const
     magXYZ = state.body_magfield*1000.0f;
 }
 
+// return magnetometer offsets
+// return true if offsets are valid
+bool NavEKF::getMagOffsets(Vector3f &magOffsets) const
+{
+    // compass offsets are valid if we have finalised magnetic field initialisation and magnetic field learning is not prohibited
+    if (secondMagYawInit && (_magCal != 2)) {
+        magOffsets = _ahrs->get_compass()->get_offsets() - state.body_magfield*1000.0f;
+        return true;
+    } else {
+        magOffsets = _ahrs->get_compass()->get_offsets();
+        return false;
+    }
+}
+
 // return the last calculated latitude, longitude and height
 bool NavEKF::getLLH(struct Location &loc) const
 {
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index 687b71ec56..ea8108299f 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -145,6 +145,10 @@ public:
     // return body magnetic field estimates in measurement units / 1000
     void getMagXYZ(Vector3f &magXYZ) const;
 
+    // Return estimated magnetometer offsets
+    // Return true if magnetometer offsets are valid
+    bool getMagOffsets(Vector3f &magOffsets) const;
+
     // return the last calculated latitude, longitude and height
     bool getLLH(struct Location &loc) const;