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

AP_InertialSensor: suppress expected errors from invensense IMUs

Browse Source
This commit is contained in:
Andrew Tridgell 2020-03-11 12:21:55 +11:00
parent 1540cf958b
commit d8b33bcac9
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

@ -426,7 +426,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;
}
@ -469,7 +471,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;
@ -586,7 +590,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;
}
@ -595,7 +601,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

@ -319,7 +319,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;
}
@ -362,7 +364,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;
@ -476,7 +480,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;
}
@ -485,7 +491,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 {