AP_NavEKF3: Initialise EKF origin height to field elevation

Enables copters to set bit position 2 in the EK3_OGN_HGT_MASK parameter without having a large jump in reported local position height.

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -477,6 +477,10 @@ void NavEKF3_core::setOrigin()
 {
     // assume origin at current GPS location (no averaging)
     EKF_origin = _ahrs->get_gps().location();
+    // 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, _ahrs->get_home().lat);