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

AP_NavEKF: fixed millisecond subtraction for rollover

Browse Source 
Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
This commit is contained in:
Andrew Tridgell 2014-04-21 13:16:20 +10:00
parent 2295632660
commit 4756dbee84
1 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -654,12 +654,14 @@ void NavEKF::SelectVelPosFusion()
// reset the counter used to schedule updates so that we always fuse data on the frame GPS data arrives
skipCounter = velPosFuseStepRatio;
// If a long time since last GPS update, then reset position and velocity and reset stored state history
if ((hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeUseTAS && useAirspeed()) || (hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeNoTAS && !useAirspeed())) {
ResetPosition();
ResetVelocity();
StoreStatesReset();
uint32_t gpsRetryTimeout = useAirspeed() ? _gpsRetryTimeUseTAS : _gpsRetryTimeNoTAS;
if (hal.scheduler->millis() - secondLastFixTime_ms > gpsRetryTimeout) {
ResetPosition();
ResetVelocity();
StoreStatesReset();
}
} else if (hal.scheduler->millis() > lastFixTime_ms + _msecGpsAvg + 40) {
} else if (hal.scheduler->millis() - lastFixTime_ms > _msecGpsAvg + 40) {
// Timeout fusion of GPS data if stale. Needed because we repeatedly fuse the same
// measurement until the next one arrives to provide a smoother output
fuseVelData = false;
@ -673,7 +675,7 @@ void NavEKF::SelectVelPosFusion()
newDataHgt = false;
// enable fusion
fuseHgtData = true;
} else if (hal.scheduler->millis() > lastHgtTime_ms + _msecHgtAvg + 40) {
} else if (hal.scheduler->millis() - lastHgtTime_ms > _msecHgtAvg + 40) {
// timeout fusion of height data if stale. Needed because we repeatedly fuse the same
// measurement until the next one arrives to provide a smoother output
fuseHgtData = false;