Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Don't update yaw estimator with bad GPS

This commit is contained in:
Paul Riseborough 2020-11-16 10:58:47 +11:00 committed by Andrew Tridgell
parent e5e8d0ba3c
commit db86a5acc8
2 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -638,7 +638,7 @@ void NavEKF3_core::runYawEstimatorPrediction()
void NavEKF3_core::runYawEstimatorCorrection()
{
if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1) {
if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1 && EKFGSF_run_filterbank) {
if (gpsDataToFuse) {
Vector2f gpsVelNE = Vector2f(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y);
float gpsVelAcc = fmaxf(gpsSpdAccuracy, frontend->_gpsHorizVelNoise);

8
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -402,11 +402,11 @@ void NavEKF3_core::detectFlight()
updateTouchdownExpected();
// handle reset of counters used to control how many times we will try to reset the yaw to the EKF-GSF value per flight
if (!prevOnGround && onGround) {
// landed so disable filter bank
if ((!prevOnGround && onGround) || !gpsAccuracyGood) {
// disable filter bank
EKFGSF_run_filterbank = false;
} else if (yawEstimator && !EKFGSF_run_filterbank && ((!prevInFlight && inFlight) || expectTakeoff)) {
// started flying so reset counters and enable filter bank
} else if (yawEstimator != nullptr && !EKFGSF_run_filterbank && (inFlight || expectTakeoff) && gpsAccuracyGood) {
// flying or about to fly so reset counters and enable filter bank when GPS is good
EKFGSF_yaw_reset_ms = 0;
EKFGSF_yaw_reset_request_ms = 0;
EKFGSF_yaw_reset_count = 0;