From e8d9a1fae92e7b8c3a67413953802ab12a780ded Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 3 Jul 2019 12:36:07 +1000
Subject: [PATCH] AP_NavEKF2: don't allow height datum reset when not on ground

---
 libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
index 81b1472cc5..b256809bdb 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
@@ -203,8 +203,10 @@ void NavEKF2_core::ResetHeight(void)
 // Return true if the height datum reset has been performed
 bool NavEKF2_core::resetHeightDatum(void)
 {
-    if (activeHgtSource == HGT_SOURCE_RNG) {
-        // by definition the height datum is at ground level so cannot perform the reset
+    if (activeHgtSource == HGT_SOURCE_RNG || !onGround) {
+        // only allow resets when on the ground.
+        // If using using rangefinder for height then never perform a
+        // reset of the height datum
         return false;
     }
     // record the old height estimate
@@ -215,7 +217,7 @@ bool NavEKF2_core::resetHeightDatum(void)
     stateStruct.position.z = 0.0f;
     // adjust the height of the EKF origin so that the origin plus baro height before and after the reset is the same
 
-    if (validOrigin && !inFlight) {
+    if (validOrigin) {
         if (!gpsGoodToAlign) {
             // if we don't have GPS lock then we shouldn't be doing a
             // resetHeightDatum, but if we do then the best option is