5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00

AP_NavEKF2: remove unnecessary PV_AidingMode check

Thanks to OXINARF for catching this
This commit is contained in:
Randy Mackay 2015-10-30 12:52:49 +09:00
parent b459d937ad
commit 4d458833dc

View File

@ -379,7 +379,7 @@ void NavEKF2_core::FuseVelPosNED()
// declare a timeout if we have not fused velocity data for too long or not aiding // declare a timeout if we have not fused velocity data for too long or not aiding
velTimeout = (((imuSampleTime_ms - lastVelPassTime_ms) > gpsRetryTime) || PV_AidingMode == AID_NONE); velTimeout = (((imuSampleTime_ms - lastVelPassTime_ms) > gpsRetryTime) || PV_AidingMode == AID_NONE);
// use velocity data if healthy, timed out, or in constant position mode // use velocity data if healthy, timed out, or in constant position mode
if (velHealth || velTimeout || (PV_AidingMode == AID_NONE)) { if (velHealth || velTimeout) {
velHealth = true; velHealth = true;
// restart the timeout count // restart the timeout count
lastVelPassTime_ms = imuSampleTime_ms; lastVelPassTime_ms = imuSampleTime_ms;