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

AP_NavEKF2: Better switching in response to faults 

Make selection sticky
If fault detected or unhealthy, then switch to healthy core with lowest fault score.
If no healthy core found, do not switch.
This commit is contained in:
Paul Riseborough 2015-11-06 13:11:49 +11:00 committed by Andrew Tridgell
parent 5f6d1d8d25
commit 1858da8307
1 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -477,6 +477,9 @@ bool NavEKF2::InitialiseFilter(void)
num_cores++;
}
}
// Set the primary initially to be the lowest index
primary = 0;
}
// initialse the cores. We return success only if all cores
@ -498,12 +501,17 @@ void NavEKF2::UpdateFilter(void)
core[i].UpdateFilter();
}
// set primary to first healthy filter
primary = 0;
for (uint8_t i=0; i<num_cores; i++) {
if (core[i].healthy()) {
primary = i;
break;
// If the current core selected has a bad fault score or is unhealthy, switch to a healthy core with the lowest fault score
if (core[primary].faultScore() > 0.0f || !core[primary].healthy()) {
float score = 1e9f;
for (uint8_t i=0; i<num_cores; i++) {
if (core[i].healthy()) {
float tempScore = core[i].faultScore();
if (tempScore < score) {
primary = i;
score = tempScore;
}
}
}
}
}