Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Improve protection against ground based mag interference

Browse Source 
Reset the mag field states and yaw earlier than the normal 5m height threshold if toilet bowling is detected.
This commit is contained in:
Paul Riseborough 2016-03-17 09:16:37 +11:00 committed by Andrew Tridgell
parent 36336e17b3
commit 6d9ba8c527
1 changed files with 23 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -30,21 +30,31 @@ void NavEKF2_core::controlMagYawReset()
// In-Flight reset for vehicle that cannot use a zero sideslip assumption
// Monitor the gain in height and reset the magnetic field states and heading when initial altitude has been gained
// Perform the reset earlier if high yaw and velocity innovations indicate that 'toilet bowling' is occurring
// This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
// Delay if rotated too far since the last check as rapid rotations will produce errors in the magnetic field states
if (!assume_zero_sideslip() && inFlight && !firstMagYawInit && (stateStruct.position.z - posDownAtTakeoff) < -5.0f && deltaRot < 0.1745f) {
firstMagYawInit = true;
// reset the timer used to prevent magnetometer fusion from affecting attitude until initial field learning is complete
magFuseTiltInhibit_ms = imuSampleTime_ms;
// Update the yaw angle and earth field states using the magnetic field measurements
Quaternion tempQuat;
Vector3f eulerAngles;
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
tempQuat = stateStruct.quat;
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
// calculate the change in the quaternion state and apply it to the ouput history buffer
tempQuat = stateStruct.quat/tempQuat;
StoreQuatRotate(tempQuat);
if (!firstMagYawInit && !assume_zero_sideslip() && inFlight && deltaRot < 0.1745f) {
// check that we have reached a height where ground magnetic interference effects are insignificant
bool hgtCheckPassed = (stateStruct.position.z - posDownAtTakeoff) < -5.0f;
// check for 'toilet bowling' which is characterised by large yaw and velocity innovations and caused by bad yaw alignment
// this can occur if there is severe magnetic interference on the ground
bool toiletBowling = (yawTestRatio > 1.0f) && (velTestRatio > 1.0f);
if (hgtCheckPassed || toiletBowling) {
firstMagYawInit = true;
// reset the timer used to prevent magnetometer fusion from affecting attitude until initial field learning is complete
magFuseTiltInhibit_ms = imuSampleTime_ms;
// Update the yaw angle and earth field states using the magnetic field measurements
Quaternion tempQuat;
Vector3f eulerAngles;
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
tempQuat = stateStruct.quat;
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
// calculate the change in the quaternion state and apply it to the ouput history buffer
tempQuat = stateStruct.quat/tempQuat;
StoreQuatRotate(tempQuat);
}
}
// In-Flight reset for vehicles that can use a zero sideslip assumption (Planes)