AP_NavEKF3: Fix #17789 core's ekf origin altitude different if flying

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -607,10 +607,6 @@ bool NavEKF3_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_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -659,8 +659,13 @@ void NavEKF3_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;