Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 17:53:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: fixed airspeed estimate check

Browse Source
This commit is contained in:
Andrew Tridgell 2014-01-03 21:31:36 +11:00
parent e69eea3086
commit 2dbfed19b8
1 changed files with 3 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1768,12 +1768,9 @@ bool NavEKF::getLLH(struct Location &loc) const
void NavEKF::OnGroundCheck()
{
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)(fabsf(hgtMea < 15.0f));
uint8_t lowAirSpd = (!_ahrs->airspeed_estimate_true(&VtasMeas) || VtasMeas < 8.0f);
uint8_t lowGndSpd = (uint8_t)((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f);
uint8_t lowHgt = (uint8_t)(fabsf(hgtMea < 15.0f));
// Go with a majority vote from three criteria
onGround = ((lowAirSpd + lowGndSpd + lowHgt) >= 2);
}