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

View File

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