Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Prevent large rotation rates corrupting the in-flight mag reset

This commit is contained in:
Paul Riseborough 2015-10-28 19:49:15 +11:00 committed by Randy Mackay
parent 1ce03c53a6
commit a1e32d71ec
1 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -5174,16 +5174,18 @@ void NavEKF::performArmingChecks()
StoreStatesReset(); StoreStatesReset();
} }
} else if (vehicleArmed && !firstMagYawInit && (state.position.z - posDownAtArming) < -1.5f && !assume_zero_sideslip()) { } else if (vehicleArmed && !firstMagYawInit && (state.position.z - posDownAtArming) < -1.5f && !assume_zero_sideslip() && state.omega.length() < 1.0f) {
// Do the first in-air 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) // Do the first in-air 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)
// This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values // This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
// Do not do this alignment if the vehicle is rotating rapidly as timing erors in the mag data will cause significant errors
Vector3f eulerAngles; Vector3f eulerAngles;
getEulerAngles(eulerAngles); getEulerAngles(eulerAngles);
state.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y); state.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
firstMagYawInit = true; firstMagYawInit = true;
} else if (vehicleArmed && !secondMagYawInit && (state.position.z - posDownAtArming) < -5.0f && !assume_zero_sideslip()) { } else if (vehicleArmed && !secondMagYawInit && (state.position.z - posDownAtArming) < -5.0f && !assume_zero_sideslip() && state.omega.length() < 1.0f) {
// Do the second and final yaw and earth mag field initialisation when the vehicle has gained 5.0m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff) // Do the second and final yaw and earth mag field initialisation when the vehicle has gained 5.0m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff)
// This second and final correction is needed for flight from large metal structures where the magnetic field distortion can extend up to 5m // This second and final correction is needed for flight from large metal structures where the magnetic field distortion can extend up to 5m
// Do not do this alignment if the vehicle is rotating rapidly as timing erors in the mag data will cause significant errors
Vector3f eulerAngles; Vector3f eulerAngles;
getEulerAngles(eulerAngles); getEulerAngles(eulerAngles);
state.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y); state.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);