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

AP_NavEKF: Bypass GPS glitch logic when not aiding

Browse Source 
When we are not using GPS measurements, we should not be allowing the GPS glitch logic to reset position states as this can interfere with operation of non GPS modes.
This commit is contained in:
priseborough 2015-01-09 23:51:12 +11:00 committed by Randy Mackay
parent 5c8e71a8d1
commit 12c3368c4d
1 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1939,17 +1939,17 @@ void NavEKF::FuseVelPosNED()
// only reset the failed time and do glitch timeout checks if we are doing full aiding
if (PV_AidingMode == AID_ABSOLUTE) {
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 (posTimeout || (maxPosInnov2 > sq(float(_gpsGlitchRadiusMax)))) {
gpsPosGlitchOffsetNE.x += posInnov[0];
gpsPosGlitchOffsetNE.y += posInnov[1];
// limit the radius of the offset to 100m and decay the offset to zero radially
decayGpsOffset();
// reset the position to the current GPS position which will include the glitch correction offset
ResetPosition();
// don't fuse data on this time step
fusePosData = false;
// 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)))) {
gpsPosGlitchOffsetNE.x += posInnov[0];
gpsPosGlitchOffsetNE.y += posInnov[1];
// limit the radius of the offset to 100m and decay the offset to zero radially
decayGpsOffset();
// reset the position to the current GPS position which will include the glitch correction offset
ResetPosition();
// don't fuse data on this time step
fusePosData = false;
}
}
} else {
posHealth = false;