Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Don't update yaw estimator with bad GPS

Browse Source
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() void NavEKF2_core::runYawEstimatorCorrection()
{ {
if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1) { if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1 && EKFGSF_run_filterbank) {
if (gpsDataToFuse) { if (gpsDataToFuse) {
Vector2f gpsVelNE = Vector2f(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y); Vector2f gpsVelNE = Vector2f(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y);
float gpsVelAcc = fmaxf(gpsSpdAccuracy, frontend->_gpsHorizVelNoise); 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 // 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) { if ((!prevOnGround && onGround) || !gpsAccuracyGood) {
// landed so disable filter bank // disable filter bank
EKFGSF_run_filterbank = false; EKFGSF_run_filterbank = false;
} else if (yawEstimator != nullptr && !prevInFlight && inFlight) { } else if (yawEstimator != nullptr && !EKFGSF_run_filterbank && inFlight && gpsAccuracyGood) {
// started flying so reset counters and enable filter bank // flying so reset counters and enable filter bank when GPS is good
EKFGSF_yaw_reset_ms = 0; EKFGSF_yaw_reset_ms = 0;
EKFGSF_yaw_reset_request_ms = 0; EKFGSF_yaw_reset_request_ms = 0;
EKFGSF_yaw_reset_count = 0; EKFGSF_yaw_reset_count = 0;