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

AP_NavEKF: Always declare a velocity measurement timeout if velocity not used

Browse Source 
If position and velocity aiding is turned off, then the velocity measurement should always be reported as timed out.
This commit is contained in:
priseborough 2015-01-09 23:39:03 +11:00 committed by Randy Mackay
parent 95c3197170
commit 3b166372cc
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

@ -1999,8 +1999,8 @@ void NavEKF::FuseVelPosNED()
velTestRatio = innovVelSumSq / (varVelSum * sq(_gpsVelInnovGate));
// fail if the ratio is greater than 1
velHealth = ((velTestRatio < 1.0f) || badIMUdata);
// declare a timeout if we have not fused velocity data for too long or in constant velocity mode
velTimeout = ((imuSampleTime_ms - velFailTime) > gpsRetryTime) || constVelMode;
// declare a timeout if we have not fused velocity data for too long or not aiding
velTimeout = (((imuSampleTime_ms - velFailTime) > gpsRetryTime) || PV_AidingMode == AID_NONE);
// if data is healthy or in constant velocity mode we fuse it
if (velHealth || constVelMode) {
velHealth = true;