Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Fix bug preventing planes recovering from bad magnetometers

Browse Source 
This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset.
This commit is contained in:
priseborough 2016-07-18 22:19:53 +10:00 committed by Andrew Tridgell
parent e2b8807260
commit e34cdc6666
2 changed files with 19 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -27,6 +27,8 @@ void NavEKF2_core::controlMagYawReset()
if (assume_zero_sideslip() && !finalInflightYawInit && inFlight ) {
gpsYawResetRequest = true;
return;
} else {
gpsYawResetRequest = false;
}
// Quaternion and delta rotation vector that are re-used for different calculations
@ -96,7 +98,7 @@ void NavEKF2_core::controlMagYawReset()
// and get an updated quaternion
Quaternion newQuat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
// if a yaw reset has been requested, apply the updated quaterniont the current state
// if a yaw reset has been requested, apply the updated quaternion to the current state
if (magYawResetRequest) {
// previous value used to calculate a reset delta
Quaternion prevQuat = stateStruct.quat;
@ -140,11 +142,11 @@ void NavEKF2_core::controlMagYawReset()
// vector from GPS. It is used to align the yaw angle after launch or takeoff.
void NavEKF2_core::realignYawGPS()
{
// get quaternion from existing filter states and calculate roll, pitch and yaw angles
Vector3f eulerAngles;
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
if ((sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y)) > 25.0f) {
// get quaternion from existing filter states and calculate roll, pitch and yaw angles
Vector3f eulerAngles;
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
// calculate course yaw angle
float velYaw = atan2f(stateStruct.velocity.y,stateStruct.velocity.x);
@ -162,6 +164,9 @@ void NavEKF2_core::realignYawGPS()
// calculate new filter quaternion states from Euler angles
stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, gpsYaw);
// reset the velocity and posiiton states as they will be inaccurate due to bad yaw
ResetVelocity();
ResetPosition();
// send yaw alignment information to console
hal.console->printf("EKF2 IMU%u yaw aligned to GPS velocity\n",(unsigned)imu_index);
@ -169,28 +174,21 @@ void NavEKF2_core::realignYawGPS()
// zero the attitude covariances becasue the corelations will now be invalid
zeroAttCovOnly();
// reset the position and velocity fusion timers to cause the states to be reset to the GPS on the next GPS fusion cycle
lastPosPassTime_ms = 0;
lastVelPassTime_ms = 0;
// record the yaw reset event
recordYawReset();
// clear any GPS yaw requests
// clear all pending yaw reset requests
gpsYawResetRequest = false;
magYawResetRequest = false;
if (use_compass()) {
// request a mag field reset which may enable us to use the magnetoemter if the previous fault was due to bad initialisation
magStateResetRequest = true;
// clear the all sensors failed status so that the magnetometers sensors get a second chance now that we are flying
allMagSensorsFailed = false;
}
}
}
// fix magnetic field states and clear any compass fault conditions
if (use_compass()) {
// submit a request to reset the magnetic field states
magStateResetRequest = true;
// We shoud retry the primary magnetometer if previously switched or failed
magSelectIndex = 0;
allMagSensorsFailed = false;
}
}
/********************************************************

2
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
View File

@ -38,7 +38,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
if ((magTestRatio.x <= 1.0f && magTestRatio.y <= 1.0f && yawTestRatio <= 1.0f) || !consistentMagData) {
magYawResetTimer_ms = imuSampleTime_ms;
}
if (imuSampleTime_ms - magYawResetTimer_ms > 5000) {
if ((imuSampleTime_ms - magYawResetTimer_ms > 5000) && !motorsArmed) {
// request reset of heading and magnetic field states
magYawResetRequest = true;
// reset timer to ensure that bad magnetometer data cannot cause a heading reset more often than every 5 seconds