AP_NavEKF2: Fix one frame delay in processing yaw estimator velocity data

5 years ago · ccaa4d6aa9
1 changed files with 1 additions and 1 deletions
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
@ -565,7 +565,7 @@ void NavEKF2_core::runYawEstimatorCorrection()
				@@ -565,7 +565,7 @@ void NavEKF2_core::runYawEstimatorCorrection()
        if (gpsDataToFuse) {
            Vector2f gpsVelNE = Vector2f(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y);
            float gpsVelAcc = fmaxf(gpsSpdAccuracy, frontend->_gpsHorizVelNoise);
-            yawEstimator->pushVelData(gpsVelNE, gpsVelAcc);
+            yawEstimator->fuseVelData(gpsVelNE, gpsVelAcc);
        }

        // action an external reset request