aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: fixed check for airspeed sensor available

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

8
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1768,7 +1768,8 @@ bool NavEKF::getLLH(struct Location &loc) const
void NavEKF::OnGroundCheck() void NavEKF::OnGroundCheck()
{ {
uint8_t lowAirSpd = (!_ahrs->airspeed_estimate_true(&VtasMeas) || VtasMeas < 8.0f); const AP_Airspeed *airspeed = _ahrs->get_airspeed();
uint8_t lowAirSpd = (!airspeed || !airspeed->use() || airspeed->get_airspeed() * airspeed->get_EAS2TAS() < 8.0f);
uint8_t lowGndSpd = (uint8_t)((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.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)); uint8_t lowHgt = (uint8_t)(fabsf(hgtMea < 15.0f));
// Go with a majority vote from three criteria // Go with a majority vote from three criteria
@ -1893,8 +1894,9 @@ void NavEKF::readAirSpdData()
{ {
const AP_Airspeed *aspeed = _ahrs->get_airspeed(); const AP_Airspeed *aspeed = _ahrs->get_airspeed();
if (aspeed && if (aspeed &&
aspeed->last_update_ms() != lastAirspeedUpdate && aspeed->use() &&
_ahrs->airspeed_estimate_true(&VtasMeas)) { aspeed->last_update_ms() != lastAirspeedUpdate) {
VtasMeas = aspeed->get_airspeed() * aspeed->get_EAS2TAS();
lastAirspeedUpdate = aspeed->last_update_ms(); lastAirspeedUpdate = aspeed->last_update_ms();
newDataTas = true; newDataTas = true;
RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay)); RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay));