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

AP_InertialSensor: suppress expected errors from invensense IMUs

This commit is contained in:
Andrew Tridgell 2020-03-11 12:21:55 +11:00
parent 8088923162
commit 739d921813
2 changed files with 24 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

@ -428,7 +428,9 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl
int16_t t2 = int16_val(data, 3);
if (!_check_raw_temp(t2)) {
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
if (!hal.scheduler->in_expected_delay()) {
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
}
_fifo_reset();
return false;
}
@ -471,7 +473,9 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam
// use temperatue to detect FIFO corruption
int16_t t2 = int16_val(data, 3);
if (!_check_raw_temp(t2)) {
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
if (!hal.scheduler->in_expected_delay()) {
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
}
_fifo_reset();
ret = false;
break;
@ -588,7 +592,9 @@ void AP_InertialSensor_Invensense::_read_fifo()
}
memset(rx, 0, n * MPU_SAMPLE_SIZE);
if (!_dev->transfer(rx, n * MPU_SAMPLE_SIZE, rx, n * MPU_SAMPLE_SIZE)) {
hal.console->printf("MPU60x0: error in fifo read %u bytes\n", n * MPU_SAMPLE_SIZE);
if (!hal.scheduler->in_expected_delay()) {
debug("MPU60x0: error in fifo read %u bytes\n", n * MPU_SAMPLE_SIZE);
}
_dev->set_chip_select(false);
goto check_registers;
}
@ -597,7 +603,9 @@ void AP_InertialSensor_Invensense::_read_fifo()
if (_fast_sampling) {
if (!_accumulate_sensor_rate_sampling(rx, n)) {
debug("IMU[%u] stop at %u of %u", _accel_instance, n_samples, bytes_read/MPU_SAMPLE_SIZE);
if (!hal.scheduler->in_expected_delay()) {
debug("IMU[%u] stop at %u of %u", _accel_instance, n_samples, bytes_read/MPU_SAMPLE_SIZE);
}
break;
}
} else {

16
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp
View File

@ -321,7 +321,9 @@ bool AP_InertialSensor_Invensensev2::_accumulate(uint8_t *samples, uint8_t n_sam
int16_t t2 = int16_val(data, 6);
if (!_check_raw_temp(t2)) {
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
if (!hal.scheduler->in_expected_delay()) {
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
}
_fifo_reset();
return false;
}
@ -364,7 +366,9 @@ bool AP_InertialSensor_Invensensev2::_accumulate_sensor_rate_sampling(uint8_t *s
// use temperature to detect FIFO corruption
int16_t t2 = int16_val(data, 6);
if (!_check_raw_temp(t2)) {
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
if (!hal.scheduler->in_expected_delay()) {
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
}
_fifo_reset();
ret = false;
break;
@ -478,7 +482,9 @@ void AP_InertialSensor_Invensensev2::_read_fifo()
}
memset(rx, 0, n * INV2_SAMPLE_SIZE);
if (!_dev->transfer(rx, n * INV2_SAMPLE_SIZE, rx, n * INV2_SAMPLE_SIZE)) {
hal.console->printf("INV2: error in fifo read %u bytes\n", n * INV2_SAMPLE_SIZE);
if (!hal.scheduler->in_expected_delay()) {
debug("INV2: error in fifo read %u bytes\n", n * INV2_SAMPLE_SIZE);
}
_dev->set_chip_select(false);
goto check_registers;
}
@ -487,7 +493,9 @@ void AP_InertialSensor_Invensensev2::_read_fifo()
if (_fast_sampling) {
if (!_accumulate_sensor_rate_sampling(rx, n)) {
debug("IMU[%u] stop at %u of %u", _accel_instance, n_samples, bytes_read/INV2_SAMPLE_SIZE);
if (!hal.scheduler->in_expected_delay()) {
debug("IMU[%u] stop at %u of %u", _accel_instance, n_samples, bytes_read/INV2_SAMPLE_SIZE);
}
break;
}
} else {