AP_NavEKF: Return more accurate validity status for height above ground

10 years ago · e98edaa6cb
1 changed files with 2 additions and 1 deletions
--- 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
				@@ -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