aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: fixed bug in EKF lane selection

Browse Source 
this fixes an issue in selecting the best lane to change to when we
have 3 or more EKF cores. The bug is that if the current lane is
unhealthy it would always choose the last healthy lane instead of
choosing the lane with the lowest score
This commit is contained in:
Andrew Tridgell 2019-05-03 19:48:18 +10:00
parent cc62998d40
commit e4f6ffe719
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -777,7 +777,7 @@ void NavEKF3::UpdateFilter(void)
// If the primary core is still healthy,then switching is optional and will only be done if
// a core with a significantly lower error score can be found
float altErrorScore = core[coreIndex].errorScore();
if (altCoreAvailable && (!core[primary].healthy() || altErrorScore < lowestErrorScore)) {
if (altCoreAvailable && (!core[newPrimaryIndex].healthy() || altErrorScore < lowestErrorScore)) {
newPrimaryIndex = coreIndex;
lowestErrorScore = altErrorScore;
}