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

AP_NavEKF2: set primary to first healthy core

This commit is contained in:
Andrew Tridgell 2015-11-05 12:13:11 +11:00
parent 2ab2afc86a
commit ee78e2d618
1 changed files with 13 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -481,9 +481,19 @@ bool NavEKF2::InitialiseFilter(void)
// Update Filter States - this should be called whenever new IMU data is available // Update Filter States - this should be called whenever new IMU data is available
void NavEKF2::UpdateFilter(void) void NavEKF2::UpdateFilter(void)
{ {
if (core) { if (!core) {
for (uint8_t i=0; i<num_cores; i++) { return;
core[i].UpdateFilter(); }
for (uint8_t i=0; i<num_cores; i++) {
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;
} }
} }
} }