AP_NavEKF2: Fix #17789 core's EKF origin altitude different if flying

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -444,10 +444,6 @@ bool NavEKF2_core::setOrigin(const Location &loc)
     }
 
     EKF_origin = loc;
-    // if flying, correct for height change from takeoff so that the origin is at field elevation
-    if (inFlight) {
-        EKF_origin.alt += (int32_t)(100.0f * stateStruct.position.z);
-    }
     ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
     // define Earth rotation vector in the NED navigation frame at the origin
     calcEarthRateNED(earthRateNED, EKF_origin.lat);

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -598,8 +598,13 @@ void NavEKF2_core::readGpsData()
 
             // Set the EKF origin and magnetic field declination if not previously set  and GPS checks have passed
             if (gpsGoodToAlign && !validOrigin) {
+                Location gpsloc_fieldelevation = gpsloc;
+                // if flying, correct for height change from takeoff so that the origin is at field elevation
+                if (inFlight) {
+                    gpsloc_fieldelevation.alt += (int32_t)(100.0f * stateStruct.position.z);
+                }
 
-                if (!setOrigin(gpsloc)) {
+                if (!setOrigin(gpsloc_fieldelevation)) {
                     // set an error as an attempt was made to set the origin more than once
                     INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
                     return;