diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index 881bbaa142..a4b3610f9c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -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 { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp index f847218444..a0a05e80de 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp @@ -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 {