Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Fix bug causing in flight yaw align to not complete

Browse Source
This commit is contained in:
Paul Riseborough 2023-09-27 10:33:01 +10:00 committed by Andrew Tridgell
parent ffde7f815c
commit 6baeb1cd9d
3 changed files with 21 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -696,9 +696,11 @@ bool NavEKF3_core::setOrigin(const Location &loc)
return true; return true;
} }
// record a yaw reset event // record all requested yaw resets completed
void NavEKF3_core::recordYawReset() void NavEKF3_core::recordYawResetsCompleted()
{ {
gpsYawResetRequest = false;
magYawResetRequest = false;
yawAlignComplete = true; yawAlignComplete = true;
if (inFlight) { if (inFlight) {
finalInflightYawInit = true; finalInflightYawInit = true;

32
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -154,6 +154,15 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset)
// If the angles disagree by more than 45 degrees and GPS innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad // If the angles disagree by more than 45 degrees and GPS innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
bool badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f) && (PV_AidingMode == AID_ABSOLUTE)) || !yawAlignComplete; bool badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f) && (PV_AidingMode == AID_ABSOLUTE)) || !yawAlignComplete;
// get yaw variance from GPS speed uncertainty
const ftype gpsVelAcc = fmaxF(gpsSpdAccuracy, ftype(frontend->_gpsHorizVelNoise));
const ftype gps_yaw_variance = sq(asinF(constrain_float(gpsVelAcc/gpsDataDelayed.vel.xy().length(), -1.0F, 1.0F)));
if (gps_yaw_variance < sq(radians(GPS_VEL_YAW_ALIGN_MAX_ANG_ERR))) {
yawAlignGpsValidCount++;
} else {
yawAlignGpsValidCount = 0;
}
// correct yaw angle using GPS ground course if compass yaw bad // correct yaw angle using GPS ground course if compass yaw bad
if (badMagYaw) { if (badMagYaw) {
// attempt to use EKF-GSF estimate if available as it is more robust to GPS glitches // attempt to use EKF-GSF estimate if available as it is more robust to GPS glitches
@ -163,16 +172,6 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset)
return; return;
} }
// get yaw variance from GPS speed uncertainty
const ftype gpsVelAcc = fmaxF(gpsSpdAccuracy, ftype(frontend->_gpsHorizVelNoise));
const ftype gps_yaw_variance = sq(asinF(constrain_float(gpsVelAcc/gpsDataDelayed.vel.xy().length(), -1.0F, 1.0F)));
if (gps_yaw_variance < sq(radians(GPS_VEL_YAW_ALIGN_MAX_ANG_ERR))) {
yawAlignGpsValidCount++;
} else {
yawAlignGpsValidCount = 0;
}
if (yawAlignGpsValidCount >= GPS_VEL_YAW_ALIGN_COUNT_THRESHOLD) { if (yawAlignGpsValidCount >= GPS_VEL_YAW_ALIGN_COUNT_THRESHOLD) {
yawAlignGpsValidCount = 0; yawAlignGpsValidCount = 0;
// keep roll and pitch and reset yaw // keep roll and pitch and reset yaw
@ -194,6 +193,10 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset)
allMagSensorsFailed = false; allMagSensorsFailed = false;
} }
} }
} else if (yawAlignGpsValidCount >= GPS_VEL_YAW_ALIGN_COUNT_THRESHOLD) {
// There is no need to do a yaw reset
yawAlignGpsValidCount = 0;
recordYawResetsCompleted();
} }
} else { } else {
yawAlignGpsValidCount = 0; yawAlignGpsValidCount = 0;
@ -1579,7 +1582,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw(bool emergency_reset)
} }
// record the yaw reset event // record the yaw reset event
recordYawReset(); recordYawResetsCompleted();
// reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly // reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly
ResetVelocity(resetDataSource::DEFAULT); ResetVelocity(resetDataSource::DEFAULT);
@ -1655,10 +1658,5 @@ void NavEKF3_core::resetQuatStateYawOnly(ftype yaw, ftype yawVariance, rotationO
lastYawReset_ms = imuSampleTime_ms; lastYawReset_ms = imuSampleTime_ms;
// record the yaw reset event // record the yaw reset event
recordYawReset(); recordYawResetsCompleted();
// clear all pending yaw reset requests
gpsYawResetRequest = false;
magYawResetRequest = false;
} }

4
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -957,8 +957,8 @@ private:
// zero attitude state covariances, but preserve variances // zero attitude state covariances, but preserve variances
void zeroAttCovOnly(); void zeroAttCovOnly();
// record a yaw reset event // record all requested yaw resets completed
void recordYawReset(); void recordYawResetsCompleted();
// record a magnetic field state reset event // record a magnetic field state reset event
void recordMagReset(); void recordMagReset();