Browse Source

AP_NavEKF: fixed airspeed estimate check

mission-4.1.18
Andrew Tridgell 11 years ago
parent
commit
2dbfed19b8
  1. 9
      libraries/AP_NavEKF/AP_NavEKF.cpp

9
libraries/AP_NavEKF/AP_NavEKF.cpp

@ -1768,12 +1768,9 @@ bool NavEKF::getLLH(struct Location &loc) const @@ -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);
}

Loading…
Cancel
Save