From 77d21f72d40b57cf9102e21645a3e870eadc9653 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Tue, 30 May 2017 11:37:37 +1000
Subject: [PATCH] AP_NavEKF2: Allow reporting of origin height for specified
 instance

---
 libraries/AP_NavEKF2/AP_NavEKF2.cpp | 8 +++++---
 libraries/AP_NavEKF2/AP_NavEKF2.h   | 5 +++--
 2 files changed, 8 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
index 92a2944b6a..f97826e0a4 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
@@ -998,15 +998,17 @@ bool NavEKF2::getLLH(struct Location &loc) const
     return core[primary].getLLH(loc);
 }
 
-// return the latitude and longitude and height used to set the NED origin
+// Return the latitude and longitude and height used to set the NED origin for the specified instance
+// An out of range instance (eg -1) returns data for the the primary instance
 // All NED positions calculated by the filter are relative to this location
 // Returns false if the origin has not been set
-bool NavEKF2::getOriginLLH(struct Location &loc) const
+bool NavEKF2::getOriginLLH(int8_t instance, struct Location &loc) const
 {
+    if (instance < 0 || instance >= num_cores) instance = primary;
     if (!core) {
         return false;
     }
-    return core[primary].getOriginLLH(loc);
+    return core[instance].getOriginLLH(loc);
 }
 
 // set the latitude and longitude and height used to set the NED origin
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h
index 1d0630ba29..cd682b5427 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.h
@@ -156,10 +156,11 @@ public:
     // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
     bool getLLH(struct Location &loc) const;
 
-    // return the latitude and longitude and height used to set the NED origin
+    // Return the latitude and longitude and height used to set the NED origin for the specified instance
+    // An out of range instance (eg -1) returns data for the the primary instance
     // All NED positions calculated by the filter are relative to this location
     // Returns false if the origin has not been set
-    bool getOriginLLH(struct Location &loc) const;
+    bool getOriginLLH(int8_t instance, struct Location &loc) const;
 
     // set the latitude and longitude and height used to set the NED origin
     // All NED positions calculated by the filter will be relative to this location