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

AP_NavEKF2: fixed bug in EKF lane selection 

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 505e1d8c1d
commit cc62998d40
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_NavEKF2/AP_NavEKF2.cpp
View File

@ -741,7 +741,7 @@ void NavEKF2::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;
}