zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_InertialSensor: suppress expected errors from invensense IMUs

c415-sdk
Andrew Tridgell 5 years ago
parent
1540cf958b
commit
d8b33bcac9
2 changed files with 24 additions and 8 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 16
      libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
  2. 16
      libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp

16
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
Unescape 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); int16_t t2 = int16_val(data, 3);
if (!_check_raw_temp(t2)) { 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(); _fifo_reset();
return false; return false;
} }
@ -469,7 +471,9 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam
// use temperatue to detect FIFO corruption // use temperatue to detect FIFO corruption
int16_t t2 = int16_val(data, 3); int16_t t2 = int16_val(data, 3);
if (!_check_raw_temp(t2)) { 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(); _fifo_reset();
ret = false; ret = false;
break; break;
@ -586,7 +590,9 @@ void AP_InertialSensor_Invensense::_read_fifo()
} }
memset(rx, 0, n * MPU_SAMPLE_SIZE); memset(rx, 0, n * MPU_SAMPLE_SIZE);
if (!_dev->transfer(rx, n * MPU_SAMPLE_SIZE, rx, 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); _dev->set_chip_select(false);
goto check_registers; goto check_registers;
} }
@ -595,7 +601,9 @@ void AP_InertialSensor_Invensense::_read_fifo()
if (_fast_sampling) { if (_fast_sampling) {
if (!_accumulate_sensor_rate_sampling(rx, n)) { 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; break;
} }
} else { } else {

16
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp
Unescape 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); int16_t t2 = int16_val(data, 6);
if (!_check_raw_temp(t2)) { 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(); _fifo_reset();
return false; return false;
} }
@ -362,7 +364,9 @@ bool AP_InertialSensor_Invensensev2::_accumulate_sensor_rate_sampling(uint8_t *s
// use temperature to detect FIFO corruption // use temperature to detect FIFO corruption
int16_t t2 = int16_val(data, 6); int16_t t2 = int16_val(data, 6);
if (!_check_raw_temp(t2)) { 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(); _fifo_reset();
ret = false; ret = false;
break; break;
@ -476,7 +480,9 @@ void AP_InertialSensor_Invensensev2::_read_fifo()
} }
memset(rx, 0, n * INV2_SAMPLE_SIZE); memset(rx, 0, n * INV2_SAMPLE_SIZE);
if (!_dev->transfer(rx, n * INV2_SAMPLE_SIZE, rx, 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); _dev->set_chip_select(false);
goto check_registers; goto check_registers;
} }
@ -485,7 +491,9 @@ void AP_InertialSensor_Invensensev2::_read_fifo()
if (_fast_sampling) { if (_fast_sampling) {
if (!_accumulate_sensor_rate_sampling(rx, n)) { 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; break;
} }
} else { } else {

Write Preview
Loading…
Cancel
Save