AP_NavEKF2: Add separate horizontal/vertical local position interfaces

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -686,6 +686,30 @@ bool NavEKF2::getPosNED(int8_t instance, Vector3f &pos)
     return core[instance].getPosNED(pos);
 }
 
+// Return the last calculated NE position relative to the reference point (m).
+// If a calculated solution is not available, use the best available data and return false
+// If false returned, do not use for flight control
+bool NavEKF2::getPosNE(int8_t instance, Vector2f &posNE)
+{
+    if (instance < 0 || instance >= num_cores) instance = primary;
+    if (!core) {
+        return false;
+    }
+    return core[instance].getPosNE(posNE);
+}
+
+// Return the last calculated D position relative to the reference point (m).
+// If a calculated solution is not available, use the best available data and return false
+// If false returned, do not use for flight control
+bool NavEKF2::getPosD(int8_t instance, float &posD)
+{
+    if (instance < 0 || instance >= num_cores) instance = primary;
+    if (!core) {
+        return false;
+    }
+    return core[instance].getPosD(posD);
+}
+
 // return NED velocity in m/s
 void NavEKF2::getVelNED(int8_t instance, Vector3f &vel)
 {

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -68,6 +68,18 @@ public:
     // If false returned, do not use for flight control
     bool getPosNED(int8_t instance, Vector3f &pos);
 
+    // Return the last calculated NE position relative to the reference point (m) for the specified instance.
+    // An out of range instance (eg -1) returns data for the the primary instance
+    // If a calculated solution is not available, use the best available data and return false
+    // If false returned, do not use for flight control
+    bool getPosNE(int8_t instance, Vector2f &posNE);
+
+    // Return the last calculated D position relative to the reference point (m) for the specified instance.
+    // An out of range instance (eg -1) returns data for the the primary instance
+    // If a calculated solution is not available, use the best available data and return false
+    // If false returned, do not use for flight control
+    bool getPosD(int8_t instance, float &posD);
+
     // return NED velocity in m/s for the specified instance
     // An out of range instance (eg -1) returns data for the the primary instance
     void getVelNED(int8_t instance, Vector3f &vel);

libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp

@@ -249,7 +249,57 @@ bool NavEKF2_core::getPosNED(Vector3f &pos) const
     return false;
 }
 
+// Return the last estimated NE position relative to the reference point (m).
+// Return true if the estimate is valid
+bool NavEKF2_core::getPosNE(Vector2f &posNE) const
+{
+    // There are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no position estimate available)
+    nav_filter_status status;
+    getFilterStatus(status);
+    if (PV_AidingMode != AID_NONE) {
+        // This is the normal mode of operation where we can use the EKF position states
+        posNE.x = outputDataNew.position.x;
+        posNE.y = outputDataNew.position.y;
+        return true;
+    } else {
+        // In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate
+        if(validOrigin) {
+            if ((_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_2D)) {
+                // If the origin has been set and we have GPS, then return the GPS position relative to the origin
+                const struct Location &gpsloc = _ahrs->get_gps().location();
+                Vector2f tempPosNE = location_diff(EKF_origin, gpsloc);
+                posNE.x = tempPosNE.x;
+                posNE.y = tempPosNE.y;
+                return false;
+            } else {
+                // If no GPS fix is available, all we can do is provide the last known position
+                posNE.x = outputDataNew.position.x;
+                posNE.y = outputDataNew.position.y;
+                return false;
+            }
+        } else {
+            // If the origin has not been set, then we have no means of providing a relative position
+            posNE.x = 0.0f;
+            posNE.y = 0.0f;
+            return false;
+        }
+    }
+    return false;
+}
+
+// Return the last calculated D position relative to the reference point (m).
+// Return true if the estimte is valid
+bool NavEKF2_core::getPosD(float &posD) const
+{
+    // The EKF always has a height estimate regardless of mode of operation
+    posD = outputDataNew.position.z;
 
+    // Return the current height solution status
+    nav_filter_status status;
+    getFilterStatus(status);
+    return status.flags.vert_pos;
+
+}
 // return the estimated height above ground level
 bool NavEKF2_core::getHAGL(float &HAGL) const
 {

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -71,6 +71,16 @@ public:
     // If false returned, do not use for flight control
     bool getPosNED(Vector3f &pos) const;
 
+    // Return the last calculated NE position relative to the reference point (m).
+    // If a calculated solution is not available, use the best available data and return false
+    // If false returned, do not use for flight control
+    bool getPosNE(Vector2f &posNE) const;
+
+    // Return the last calculated D position relative to the reference point (m).
+    // If a calculated solution is not available, use the best available data and return false
+    // If false returned, do not use for flight control
+    bool getPosD(float &posD) const;
+
     // return NED velocity in m/s
     void getVelNED(Vector3f &vel) const;