Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 17:48:35 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: work around gyro and accel errors on startup

Browse Source
This commit is contained in:
Jonathan Challinger 2015-04-30 02:15:36 -07:00 committed by Randy Mackay
parent ea5a60dd01
commit 07ca9cebf9
2 changed files with 38 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -309,7 +309,9 @@ AP_InertialSensor::AP_InertialSensor() :
_hil_mode(false),
_calibrating(false),
_log_raw_data(false),
_dataflash(NULL)
_dataflash(NULL),
_startup_error_counts_set(false),
_startup_ms(0)
{
AP_Param::setup_object_defaults(this, var_info);
for (uint8_t i=0; i<INS_MAX_BACKENDS; i++) {
@ -323,6 +325,9 @@ AP_InertialSensor::AP_InertialSensor() :
#endif
_accel_max_abs_offsets[i] = 3.5f;
_accel_startup_error_count[i] = 0;
_gyro_startup_error_count[i] = 0;
}
#if INS_VIBRATION_CHECK
for (uint8_t i=0; i<INS_VIBRATION_CHECK_INSTANCES; i++) {
@ -1199,25 +1204,47 @@ void AP_InertialSensor::update(void)
_backends[i]->update();
}
if (!_startup_error_counts_set) {
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
_accel_startup_error_count[i] = _accel_error_count[i];
_gyro_startup_error_count[i] = _gyro_error_count[i];
}
if (_startup_ms == 0) {
_startup_ms = hal.scheduler->millis();
} else if (hal.scheduler->millis()-_startup_ms > 2000) {
_startup_error_counts_set = true;
}
}
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_error_count[i] < _accel_startup_error_count[i]) {
_accel_startup_error_count[i] = _accel_error_count[i];
}
if (_gyro_error_count[i] < _gyro_startup_error_count[i]) {
_gyro_startup_error_count[i] = _gyro_error_count[i];
}
}
// adjust health status if a sensor has a non-zero error count
// but another sensor doesn't.
bool have_zero_accel_error_count = false;
bool have_zero_gyro_error_count = false;
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_healthy[i] && _accel_error_count[i] == 0) {
if (_accel_healthy[i] && _accel_error_count[i] <= _accel_startup_error_count[i]) {
have_zero_accel_error_count = true;
}
if (_gyro_healthy[i] && _gyro_error_count[i] == 0) {
if (_gyro_healthy[i] && _gyro_error_count[i] <= _gyro_startup_error_count[i]) {
have_zero_gyro_error_count = true;
}
}
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] > _gyro_startup_error_count[i] && have_zero_gyro_error_count) {
// we prefer not to use a gyro that has had errors
_gyro_healthy[i] = false;
}
if (_accel_healthy[i] && _accel_error_count[i] != 0 && have_zero_accel_error_count) {
if (_accel_healthy[i] && _accel_error_count[i] > _accel_startup_error_count[i] && have_zero_accel_error_count) {
// we prefer not to use a accel that has had errors
_accel_healthy[i] = false;
}

6
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -365,6 +365,12 @@ private:
} _hil {};
DataFlash_Class *_dataflash;
// sensor error count at startup (used to ignore errors within 2 seconds of startup)
uint32_t _accel_startup_error_count[INS_MAX_INSTANCES];
uint32_t _gyro_startup_error_count[INS_MAX_INSTANCES];
bool _startup_error_counts_set;
uint32_t _startup_ms;
};
#include "AP_InertialSensor_Backend.h"