AP_NavEKF: Always return a NED relative position if possible

If a calculated position is not available, the function will return a value based on raw GPS or last calculation if available, but the status will be set to false to indicate that it cannot be used for control.

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -3608,23 +3608,44 @@ void NavEKF::getVelNED(Vector3f &vel) const
     vel = state.velocity;
 }
 
-// return the last calculated NED position relative to the reference point (m).
+// Return the last calculated NED position relative to the reference point (m).
-// return false if no position is available
+// if a calculated solution is not available, use the best available data and return false
 bool NavEKF::getPosNED(Vector3f &pos) const
 {
-    if (constPosMode) {
+    // The EKF always has a height estimate regardless of mode of operation
-        pos.x = state.position.x + lastKnownPositionNE.x;
+    pos.z = state.position.z;
-        pos.y = state.position.y + lastKnownPositionNE.y;
+    // There are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no position estimate available)
-    } else {
+    nav_filter_status status;
+    getFilterStatus(status);
+    if (status.flags.horiz_pos_abs || status.flags.horiz_pos_rel) {
+        // This is the normal mode of operation where we can use the EKF position states
         pos.x = state.position.x;
         pos.y = state.position.y;
-    }
+        return true;
-    if (_fusionModeGPS != 3) {
-        pos.z = state.position.z;
     } else {
-        pos.z = state.position.z - terrainState;
+        // 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);
+                pos.x = tempPosNE.x;
+                pos.y = tempPosNE.y;
+                return false;
+            } else {
+                // If no GPS fix is available, all we can do is provide the last known position
+                pos.x = state.position.x + lastKnownPositionNE.x;
+                pos.y = state.position.y + lastKnownPositionNE.y;
+                return false;
+            }
+        } else {
+            // If the origin has not been set, then we have no means of providing a relative position
+            pos.x = 0.0f;
+            pos.y = 0.0f;
+            return false;
+        }
     }
-    return true;
+    return false;
 }
 
 // return body axis gyro bias estimates in rad/sec

libraries/AP_NavEKF/AP_NavEKF.h

@@ -105,8 +105,9 @@ public:
     // Check basic filter health metrics and return a consolidated health status
     bool healthy(void) const;
 
-    // return the last calculated NED position relative to the reference point (m).
+    // Return the last calculated NED position relative to the reference point (m).
-    // return false if no position is available
+    // 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 getPosNED(Vector3f &pos) const;
 
     // return NED velocity in m/s