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

AP_NavEKF: Always declare a position measurement timeout if aiding not used

Browse Source 
If position and velocity aiding is turned off, then the position measurement should always be reported as timed out.
This commit is contained in:
priseborough 2015-01-09 23:41:15 +11:00 committed by Randy Mackay
parent 3b166372cc
commit c505a458de
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1931,8 +1931,8 @@ void NavEKF::FuseVelPosNED()
float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise + 0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(imuSampleTime_ms - posFailTime)));
posTestRatio = (sq(posInnov[0]) + sq(posInnov[1])) / maxPosInnov2;
posHealth = ((posTestRatio < 1.0f) || badIMUdata);
// declare a timeout condition if we have been too long without data
posTimeout = ((imuSampleTime_ms - posFailTime) > gpsRetryTime);
// declare a timeout condition if we have been too long without data or not aiding
posTimeout = (((imuSampleTime_ms - posFailTime) > gpsRetryTime) || PV_AidingMode == AID_NONE);
// use position data if healthy, timed out, or in constant position mode
if (posHealth || posTimeout || constPosMode) {
posHealth = true;