mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF: Prevent large rotation rates corrupting the in-flight mag reset
This commit is contained in:
parent
1ce03c53a6
commit
a1e32d71ec
|
@ -5174,16 +5174,18 @@ void NavEKF::performArmingChecks()
|
|||
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)
|
||||
// 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;
|
||||
getEulerAngles(eulerAngles);
|
||||
state.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
|
||||
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)
|
||||
// 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;
|
||||
getEulerAngles(eulerAngles);
|
||||
state.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
|
||||
|
|
Loading…
Reference in New Issue