AP_NavEKF : improved On Ground check

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()