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

AP_NavEKF : fixed bug in pos and vel reset when in static mode

Browse Source
This commit is contained in:
Paul Riseborough 2014-02-16 22:32:17 +11:00 committed by Andrew Tridgell
parent 98f72a4864
commit 7a82746fcc
1 changed files with 10 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -557,8 +557,10 @@ void NavEKF::SelectVelPosFusion()
skipCounter = velPosFuseStepRatio;
// If a long time sinc last GPS update, then reset position and velocity
if ((hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeUseTAS && useAirspeed) || (hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeNoTAS && !useAirspeed)) {
ResetPosition();
ResetVelocity();
if (!staticMode) {
ResetPosition();
ResetVelocity();
}
}
}
@ -1515,7 +1517,9 @@ void NavEKF::FuseVelPosNED()
velFailTime = hal.scheduler->millis();
if (velTimeout)
{
ResetVelocity();
if (!staticMode) {
ResetVelocity();
}
fuseVelData = false;
}
@ -1541,7 +1545,9 @@ void NavEKF::FuseVelPosNED()
posFailTime = hal.scheduler->millis();
if (posTimeout)
{
ResetPosition();
if (!staticMode) {
ResetPosition();
}
fusePosData = false;
}
}