diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index f3e654d2de..07271b2913 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -36,24 +36,19 @@ void NavEKF2_core::ResetVelocity(void) stateStruct.velocity.y = gps_corrected.vel.y; // set the variances using the reported GPS speed accuracy P[4][4] = P[3][3] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy)); - // clear the timeout flags and counters - velTimeout = false; - lastVelPassTime_ms = imuSampleTime_ms; } else if (imuSampleTime_ms - extNavVelMeasTime_ms < 250) { // use external nav data as the 2nd preference stateStruct.velocity = extNavVelDelayed.vel; P[5][5] = P[4][4] = P[3][3] = sq(extNavVelDelayed.err); - velTimeout = false; - lastVelPassTime_ms = imuSampleTime_ms; } else { stateStruct.velocity.x = 0.0f; stateStruct.velocity.y = 0.0f; // set the variances using the likely speed range P[4][4] = P[3][3] = sq(25.0f); - // clear the timeout flags and counters - velTimeout = false; - lastVelPassTime_ms = imuSampleTime_ms; } + // clear the timeout flags and counters + velTimeout = false; + lastVelPassTime_ms = imuSampleTime_ms; } for (uint8_t i=0; i