AP_NavEKF3: ensure that both AMSL and relative alt are correct after a reset

this forces the EKF origin to the GPS alt on a height datum reset if
we have GPS lock. If we don't do this then the reported AMSL alt will
drift over time away from the GPS alt when we reset while on the
ground

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -213,7 +213,19 @@ bool NavEKF3_core::resetHeightDatum(void)
     stateStruct.position.z = 0.0f;
     // adjust the height of the EKF origin so that the origin plus baro height before and after the reset is the same
     if (validOrigin) {
-        ekfGpsRefHgt += (double)oldHgt;
+        const AP_GPS &gps = AP::gps();
+        if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+            // if we have a good GPS lock then reset to the GPS
+            // altitude. This ensures the reported AMSL alt from
+            // getLLH() is equal to GPS altitude, while also ensuring
+            // that the relative alt is zero
+            ekfGpsRefHgt = gps.location().alt*0.01;
+        } else {
+            // if we don't have GPS lock then we shouldn't be doing a
+            // resetHeightDatum, but if we do then the best option is
+            // to maintain the old error
+            ekfGpsRefHgt += oldHgt;
+        }
     }
     // adjust the terrain state
     terrainState += oldHgt;