AP_NavEKF: Don't reset the position measurement timeout if not aiding

When PV aiding is disabled, then the timeout time reference should not be reset becasue we want the position measurement timeout status to remain true the whole time the measurement is not being used.
This commit is contained in:
priseborough 2015-01-09 23:47:22 +11:00 committed by Randy Mackay
parent c505a458de
commit 5c8e71a8d1

View File

@ -1936,8 +1936,8 @@ void NavEKF::FuseVelPosNED()
// use position data if healthy, timed out, or in constant position mode
if (posHealth || posTimeout || constPosMode) {
posHealth = true;
// We don't reset the failed time if we are in constant position mode
if (!constPosMode) {
// 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