From ac568bae538b4d08931b09324add5aec8cd2c12d Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Mon, 17 Jul 2017 09:07:24 +1000
Subject: [PATCH] AP_NavEKF2: Initialise EKF origin height to field elevation

Enables copters to set bit position 2 in the EK2_OGN_HGT_MASK parameter without having a large jump in reported local position height.
---
 libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
index 69f2d18f20..848926950b 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
@@ -421,6 +421,10 @@ void NavEKF2_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);