From f640ec30ff5d19d8462dc2bca0a83b0e449c554a Mon Sep 17 00:00:00 2001 From: priseborough Date: Sat, 1 Nov 2014 15:05:30 +1100 Subject: [PATCH] AP_NavEKF : Reset held velocity on arming This held velocity is used to constrain the drift in the optical flow solution before the vehicle is high enough for the optical flow sensor to work. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 1e6766fa7b..c46a106fad 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -511,6 +511,7 @@ void NavEKF::ResetVelocity(void) storedStates[i].velocity = velNED; } } + } // reset the vertical position state using the last height measurement @@ -725,6 +726,8 @@ void NavEKF::UpdateFilter() state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch); firstArmPosD = state.position.z; } + calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch); + heldVelNE.zero(); prevStaticMode = staticMode; } else if (!staticMode && !finalMagYawInit && firstArmPosD - state.position.z > 1.5f && !assume_zero_sideslip()) { // Do a final yaw and earth mag field initialisation when the vehicle has gained 1.5m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff)