Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : Reset held velocity on arming

Browse Source 
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.
This commit is contained in:
priseborough 2014-11-01 15:05:30 +11:00 committed by Andrew Tridgell
parent 4616721b0d
commit f640ec30ff
1 changed files with 3 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -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)