From 3ee5ef852b49c38bd9a5f4fe553cf92e43e2bb83 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Wed, 1 Jan 2014 17:14:10 +1100
Subject: [PATCH] AP_NavEKF : improved On Ground check

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index fa599789b7..9047672c4d 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -1956,11 +1956,14 @@ bool NavEKF::getLLH(struct Location &loc)
 
 void NavEKF::OnGroundCheck()
 {
-    bool noAirSpd;
-    bool noGndSpd;
-    noAirSpd = (_ahrs.airspeed_estimate_true(&VtasMeas) < 8.0f);
-    noGndSpd = ((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f);
-    onGround = (noAirSpd && noGndSpd);
+    uint8_t lowAirSpd;
+    uint8_t lowGndSpd;
+    uint8_t lowHgt;
+    lowAirSpd = (uint8_t)(_ahrs.airspeed_estimate_true(&VtasMeas) < 8.0f);
+    lowGndSpd = (uint8_t)((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f);
+    lowHgt = (uint8_t)(hgtMea < 15.0f);
+    // Go with a majority vote from three criteria
+    onGround = ((lowAirSpd + lowGndSpd + lowHgt) >= 2);
 }
 
 void NavEKF::CovarianceInit()