From 9fa3a433f562dea6f00a46963fd26872196353c7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 22 Oct 2022 12:57:28 +1100 Subject: [PATCH] Revert "AP_InertialSensor: disable temperature based fifo check on ICM20602" This reverts commit 05f8e3c18d164ffe3766835786e67aae3a235b61. this leads to bad IMU data on ICM20602 --- libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp | 5 ++--- libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h | 3 --- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index 7d61461dff..a8acb4f386 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -226,7 +226,6 @@ void AP_InertialSensor_Invensense::start() gdev = DEVTYPE_INS_ICM20602; adev = DEVTYPE_INS_ICM20602; _enable_offset_checking = true; - _disable_temp_fifo_check = true; break; case Invensense_ICM20601: gdev = DEVTYPE_INS_ICM20601; @@ -547,7 +546,7 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl accel *= _accel_scale; int16_t t2 = int16_val(data, 3); - if (!_disable_temp_fifo_check && !_check_raw_temp(t2)) { + if (!_check_raw_temp(t2)) { if (!hal.scheduler->in_expected_delay()) { debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2); } @@ -592,7 +591,7 @@ 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 (!_disable_temp_fifo_check && !_check_raw_temp(t2)) { + if (!_check_raw_temp(t2)) { if (!hal.scheduler->in_expected_delay()) { debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2); } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h index 6cbcc62f48..3230a7f83d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h @@ -169,9 +169,6 @@ private: // buffer for fifo read uint8_t *_fifo_buffer; - // disable temperature based fifo check - bool _disable_temp_fifo_check; - /* accumulators for sensor_rate sampling See description in _accumulate_sensor_rate_sampling()