From 58d58b4eac1059ec01ba00398f91164656260d44 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Mon, 17 Jul 2017 09:07:40 +1000
Subject: [PATCH] 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 | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
index 92525cab75..9493137d47 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
+++ b/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);