From e98edaa6cb18c56438682da6f8dd4ff54ed45999 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Wed, 8 Apr 2015 19:56:43 +1000
Subject: [PATCH] AP_NavEKF: Return more accurate validity status for height
 above ground

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index cbfc2d6aa3..56f24cf243 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -3702,7 +3702,8 @@ bool NavEKF::getLLH(struct Location &loc) const
 bool NavEKF::getHAGL(float &HAGL) const
 {
     HAGL = terrainState - state.position.z;
-    return !inhibitGndState;
+    // If we know the terrain offset and altitude, then we have a valid height above ground estimate
+    return !hgtTimeout && gndOffsetValid && healthy();
 }
 
 // return data for debugging optical flow fusion