AP_NavEKF3: store and use lastKnownPositionD

When returning our last-known-position we were supplying lat/lng but not
altitude.

This usually really doesn't matter as this result generally isn't used,
especially for altitude.  OTOH, it may prevent a bug into the future.

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -381,6 +381,9 @@ void NavEKF3_core::setAidingMode()
             meaHgtAtTakeOff = baroDataDelayed.hgt;
             // reset the vertical position state to faster recover from baro errors experienced during touchdown
             stateStruct.position.z = -meaHgtAtTakeOff;
+            // store the current height to be used to keep reporting
+            // the last known position
+            lastKnownPositionD = stateStruct.position.z;
             // reset relative aiding sensor fusion activity status
             flowFusionActive = false;
             bodyVelFusionActive = false;

libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

@@ -279,7 +279,7 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
         float posD;
         if(getPosD(posD) && PV_AidingMode != AID_NONE) {
             // Altitude returned is an absolute altitude relative to the WGS-84 spherioid
-            loc.set_alt_cm(origin.alt - posD*100, Location::AltFrame::ABSOLUTE);
+            loc.set_alt_cm(origin.alt - posD*100.0, Location::AltFrame::ABSOLUTE);
             if (filterStatus.flags.horiz_pos_abs || filterStatus.flags.horiz_pos_rel) {
                 // The EKF is able to provide a position estimate
                 loc.lat = EKF_origin.lat;
@@ -309,6 +309,7 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
                 loc.lng = EKF_origin.lng;
                 loc.offset(lastKnownPositionNE.x + posOffsetNED.x,
                            lastKnownPositionNE.y + posOffsetNED.y);
+                loc.alt = EKF_origin.alt - lastKnownPositionD*100.0;
                 return false;
             }
         }

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -237,6 +237,7 @@ void NavEKF3_core::InitialiseVariables()
     dt = 0;
     velDotNEDfilt.zero();
     lastKnownPositionNE.zero();
+    lastKnownPositionD = 0;
     prevTnb.zero();
     memset(&P[0][0], 0, sizeof(P));
     memset(&KH[0][0], 0, sizeof(KH));

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -1047,6 +1047,7 @@ private:
     uint32_t lastSynthYawTime_ms;   // time stamp when yaw observation was last fused (msec)
     uint32_t ekfStartTime_ms;       // time the EKF was started (msec)
     Vector2F lastKnownPositionNE;   // last known position
+    float lastKnownPositionD;       // last known height
     uint32_t lastLaunchAccelTime_ms;
     ftype velTestRatio;             // sum of squares of GPS velocity innovation divided by fail threshold
     ftype posTestRatio;             // sum of squares of GPS position innovation divided by fail threshold