aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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 // 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 // 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 // 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 // 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) { if (!firstMagYawInit && !assume_zero_sideslip() && inFlight && deltaRot < 0.1745f) {
firstMagYawInit = true; // check that we have reached a height where ground magnetic interference effects are insignificant
// reset the timer used to prevent magnetometer fusion from affecting attitude until initial field learning is complete bool hgtCheckPassed = (stateStruct.position.z - posDownAtTakeoff) < -5.0f;
magFuseTiltInhibit_ms = imuSampleTime_ms;
// Update the yaw angle and earth field states using the magnetic field measurements // check for 'toilet bowling' which is characterised by large yaw and velocity innovations and caused by bad yaw alignment
Quaternion tempQuat; // this can occur if there is severe magnetic interference on the ground
Vector3f eulerAngles; bool toiletBowling = (yawTestRatio > 1.0f) && (velTestRatio > 1.0f);
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
tempQuat = stateStruct.quat; if (hgtCheckPassed || toiletBowling) {
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y); firstMagYawInit = true;
// calculate the change in the quaternion state and apply it to the ouput history buffer // reset the timer used to prevent magnetometer fusion from affecting attitude until initial field learning is complete
tempQuat = stateStruct.quat/tempQuat; magFuseTiltInhibit_ms = imuSampleTime_ms;
StoreQuatRotate(tempQuat); // 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) // In-Flight reset for vehicles that can use a zero sideslip assumption (Planes)