From f00b1ff22d6ca2149282764e92d45f7364c7772e Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Sun, 8 Nov 2015 20:42:09 +1100
Subject: [PATCH] AP_NavEKF2: Publish the magnetometer selection index

---
 libraries/AP_NavEKF2/AP_NavEKF2.cpp         | 9 +++++++++
 libraries/AP_NavEKF2/AP_NavEKF2.h           | 4 ++++
 libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 5 +++++
 libraries/AP_NavEKF2/AP_NavEKF2_core.h      | 3 +++
 4 files changed, 21 insertions(+)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
index 76e7cb4aae..e6867522b8 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
@@ -683,6 +683,15 @@ void NavEKF2::getMagXYZ(int8_t instance, Vector3f &magXYZ)
     }
 }
 
+// return the magnetometer in use for the specified instance
+uint8_t NavEKF2::getActiveMag(int8_t instance)
+{
+    if (instance < 0 || instance >= num_cores) instance = primary;
+    if (core) {
+        return core[instance].getActiveMag();
+    }
+}
+
 // Return estimated magnetometer offsets
 // Return true if magnetometer offsets are valid
 bool NavEKF2::getMagOffsets(Vector3f &magOffsets) const
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h
index 19840a943d..cb20b63e9f 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.h
@@ -130,6 +130,10 @@ public:
     // An out of range instance (eg -1) returns data for the the primary instance
     void getMagXYZ(int8_t instance, Vector3f &magXYZ);
 
+    // return the magnetometer in use for the specified instance
+    // An out of range instance (eg -1) returns data for the the primary instance
+    uint8_t getActiveMag(int8_t instance);
+
     // Return estimated magnetometer offsets
     // Return true if magnetometer offsets are valid
     bool getMagOffsets(Vector3f &magOffsets) const;
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
index cc3e400d66..94b6b44e78 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
@@ -341,6 +341,11 @@ void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const
     magXYZ = stateStruct.body_magfield*1000.0f;
 }
 
+// return the index for the active magnetometer
+uint8_t NavEKF2_core::getActiveMag() const
+{
+    return (uint8_t)magSelectIndex;
+}
 
 // return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements
 void  NavEKF2_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
index 347d25f3a4..6405fe0a40 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
@@ -143,6 +143,9 @@ public:
     // return body magnetic field estimates in measurement units / 1000
     void getMagXYZ(Vector3f &magXYZ) const;
 
+    // return the index for the active magnetometer
+    uint8_t getActiveMag() const;
+
     // Return estimated magnetometer offsets
     // Return true if magnetometer offsets are valid
     bool getMagOffsets(Vector3f &magOffsets) const;