Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Correctly report position timeout when GPS is lost

Browse Source
This commit is contained in:
priseborough 2015-01-06 15:19:33 +11:00 committed by Andrew Tridgell
parent 3e583e3650
commit 1789dc08a3
1 changed files with 4 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1928,7 +1928,10 @@ void NavEKF::FuseVelPosNED()
// use position data if healthy, timed out, or in constant position mode // use position data if healthy, timed out, or in constant position mode
if (posHealth || posTimeout || constPosMode) { if (posHealth || posTimeout || constPosMode) {
posHealth = true; posHealth = true;
posFailTime = imuSampleTime_ms; // We don't reset the failed time if we are in constant position mode
if (!constPosMode) {
posFailTime = imuSampleTime_ms;
}
// if timed out or outside the specified glitch radius, increment the offset applied to GPS data to compensate for large GPS position jumps // if timed out or outside the specified glitch radius, increment the offset applied to GPS data to compensate for large GPS position jumps
if (posTimeout || (maxPosInnov2 > sq(float(_gpsGlitchRadiusMax)))) { if (posTimeout || (maxPosInnov2 > sq(float(_gpsGlitchRadiusMax)))) {
gpsPosGlitchOffsetNE.x += posInnov[0]; gpsPosGlitchOffsetNE.x += posInnov[0];