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

AP_NavEKF3: Fix EKF selection bug

Browse Source
This commit is contained in:
Paul Riseborough 2021-01-03 11:33:50 +11:00 committed by Andrew Tridgell
parent 85c0040fa3
commit 912db3512e
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -920,6 +920,11 @@ void NavEKF3::UpdateFilter(void)
// if this core has a significantly lower relative error to the active primary, we consider it as a // if this core has a significantly lower relative error to the active primary, we consider it as a
// better core and would like to switch to it even if the current primary is healthy // better core and would like to switch to it even if the current primary is healthy
betterCore = altCoreError <= -BETTER_THRESH; // a better core if its relative error is below a substantial level than the primary's betterCore = altCoreError <= -BETTER_THRESH; // a better core if its relative error is below a substantial level than the primary's
// handle the case where the secondary core is faster to complete yaw alignment which can happen
// in flight when oeprating without a magnetomer
const NavEKF3_core &newCore = core[coreIndex];
const NavEKF3_core &oldCore = core[primary];
betterCore |= newCore.have_aligned_yaw() && !oldCore.have_aligned_yaw();
newPrimaryIndex = coreIndex; newPrimaryIndex = coreIndex;
} }
} }