Browse Source

AP_NavEKF3: Fix bug preventing EKFGSF running when needed for reset

The gpsAccuracyGood flag should not be used because it will go false if GPS innovations become high due to bad yaw which is when the EKFGSF is required. to keep running.
c415-sdk
Paul Riseborough 4 years ago committed by Andrew Tridgell
parent
commit
0971ef55a4
  1. 4
      libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp

4
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp

@ -392,10 +392,10 @@ void NavEKF3_core::detectFlight() @@ -392,10 +392,10 @@ 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) || !gpsAccuracyGood) {
if ((!prevOnGround && onGround) || !gpsSpdAccPass) {
// disable filter bank
EKFGSF_run_filterbank = false;
} else if (yawEstimator != nullptr && !EKFGSF_run_filterbank && (inFlight || expectTakeoff) && gpsAccuracyGood) {
} else if (yawEstimator != nullptr && !EKFGSF_run_filterbank && (inFlight || expectTakeoff) && gpsSpdAccPass) {
// 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;

Loading…
Cancel
Save