Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 16:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : improved On Ground check

Browse Source
This commit is contained in:
Paul Riseborough 2014-01-01 17:14:10 +11:00 committed by Andrew Tridgell
parent 8bc14d09b2
commit 3ee5ef852b
1 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

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