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

AP_NavEKF2: Don't update yaw estimator with bad GPS

This commit is contained in:
Paul Riseborough 2020-11-16 10:58:39 +11:00 committed by Andrew Tridgell
parent 1250f62c88
commit e5e8d0ba3c
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_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -557,7 +557,7 @@ void NavEKF2_core::runYawEstimatorPrediction()
void NavEKF2_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_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
View File

@ -391,11 +391,11 @@ void NavEKF2_core::detectFlight()
}
// 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 != nullptr && !prevInFlight && inFlight) {
// started flying so reset counters and enable filter bank
} else if (yawEstimator != nullptr && !EKFGSF_run_filterbank && inFlight && gpsAccuracyGood) {
// flying 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;