Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: fixed error detection on secondary IMUs

Browse Source 
the break; was preventing error detection on lsm303d/l3gd20
This commit is contained in:
Andrew Tridgell 2015-01-01 08:55:34 +11:00
parent 765fa0e223
commit 06a72839ed
1 changed files with 9 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -980,22 +980,26 @@ void AP_InertialSensor::update(void)
have_zero_gyro_error_count = true; have_zero_gyro_error_count = true;
} }
} }
// set primary to first healthy accel and gyro
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_gyro_healthy[i] && _gyro_error_count[i] != 0 && have_zero_gyro_error_count) { if (_gyro_healthy[i] && _gyro_error_count[i] != 0 && have_zero_gyro_error_count) {
// we prefer not to use a gyro that has had errors // we prefer not to use a gyro that has had errors
_gyro_healthy[i] = false; _gyro_healthy[i] = false;
} }
if (_accel_healthy[i] && _accel_error_count[i] != 0 && have_zero_accel_error_count) {
// we prefer not to use a accel that has had errors
_accel_healthy[i] = false;
}
}
// set primary to first healthy accel and gyro
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_gyro_healthy[i]) { if (_gyro_healthy[i]) {
_primary_gyro = i; _primary_gyro = i;
break; break;
} }
} }
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_healthy[i] && _accel_error_count[i] != 0 && have_zero_accel_error_count) {
// we prefer not to use a accel that has had errors
_accel_healthy[i] = false;
}
if (_accel_healthy[i]) { if (_accel_healthy[i]) {
_primary_accel = i; _primary_accel = i;
break; break;