From c8f5e3b6b57e483a886cd906d43462bc05771fe2 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sat, 22 Oct 2022 10:18:46 +0530 Subject: [PATCH] AP_InertialSensor: add fast reset for ICM20602 instead of full reset on bad temp sample --- .../AP_InertialSensor_Invensense.cpp | 67 ++++++++++++++++--- .../AP_InertialSensor_Invensense.h | 8 +++ 2 files changed, 65 insertions(+), 10 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index a8acb4f386..fc448f97bd 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -27,6 +27,7 @@ #include #include "AP_InertialSensor_Invensense.h" +#include extern const AP_HAL::HAL& hal; @@ -195,6 +196,16 @@ void AP_InertialSensor_Invensense::_fifo_reset(bool log_error) notify_gyro_fifo_reset(_gyro_instance); } +void AP_InertialSensor_Invensense::_fast_fifo_reset() +{ + fast_reset_count++; + _register_write(MPUREG_USER_CTRL, _last_stat_user_ctrl | BIT_USER_CTRL_FIFO_RESET); + + notify_accel_fifo_reset(_accel_instance); + notify_gyro_fifo_reset(_gyro_instance); +} + + bool AP_InertialSensor_Invensense::_has_auxiliary_bus() { return _dev->bus_type() != AP_HAL::Device::BUS_TYPE_I2C; @@ -225,6 +236,10 @@ void AP_InertialSensor_Invensense::start() case Invensense_ICM20602: gdev = DEVTYPE_INS_ICM20602; adev = DEVTYPE_INS_ICM20602; + // ICM20602 has a bug where sometimes the data gets a huge offset + // this seems to be fixed by doing a quick FIFO reset via USR_CTRL + // reg + _enable_fast_fifo_reset = true; _enable_offset_checking = true; break; case Invensense_ICM20601: @@ -434,6 +449,28 @@ bool AP_InertialSensor_Invensense::update() /* front end */ _publish_temperature(_accel_instance, _temp_filtered); + if (fast_reset_count) { + // check if we have reported in the last 1 seconds or + // fast_reset_count changed +#if HAL_GCS_ENABLED && BOARD_FLASH_SIZE > 1024 + if (AP_HAL::millis() - last_fast_reset_count_report_ms > 1000) { + last_fast_reset_count_report_ms = AP_HAL::millis(); + char param_name[sizeof("IMUx_RST")]; + if (_gyro_instance <= 9) { + snprintf(param_name, sizeof(param_name), "IMU%u_RST", _gyro_instance); + } else { + snprintf(param_name, sizeof(param_name), "IMUx_RST"); + } + gcs().send_named_float(param_name, fast_reset_count); + } +#endif +#if HAL_LOGGING_ENABLED + if (last_fast_reset_count != fast_reset_count) { + AP::logger().Write_MessageF("IMU%u fast fifo reset %u", _gyro_instance, fast_reset_count); + } +#endif + } + return true; } @@ -547,11 +584,16 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl int16_t t2 = int16_val(data, 3); if (!_check_raw_temp(t2)) { - if (!hal.scheduler->in_expected_delay()) { - debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2); + if (_enable_fast_fifo_reset) { + _fast_fifo_reset(); + return false; + } else { + if (!hal.scheduler->in_expected_delay()) { + debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2); + } + _fifo_reset(true); + return false; } - _fifo_reset(true); - return false; } float temp = t2 * temp_sensitivity + temp_zero; @@ -589,14 +631,19 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam for (uint8_t i = 0; i < n_samples; i++) { const uint8_t *data = samples + MPU_SAMPLE_SIZE * i; - // use temperatue to detect FIFO corruption + // use temperature to detect FIFO corruption int16_t t2 = int16_val(data, 3); if (!_check_raw_temp(t2)) { - if (!hal.scheduler->in_expected_delay()) { - debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2); + if (_enable_fast_fifo_reset) { + _fast_fifo_reset(); + ret = false; + } else { + if (!hal.scheduler->in_expected_delay()) { + debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2); + } + _fifo_reset(true); + ret = false; } - _fifo_reset(true); - ret = false; break; } tsum += t2; @@ -727,7 +774,7 @@ void AP_InertialSensor_Invensense::_read_fifo() if (_fast_sampling) { if (!_accumulate_sensor_rate_sampling(rx, n)) { - if (!hal.scheduler->in_expected_delay()) { + if (!hal.scheduler->in_expected_delay() && !_enable_fast_fifo_reset) { debug("IMU[%u] stop at %u of %u", _accel_instance, n_samples, bytes_read/MPU_SAMPLE_SIZE); } break; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h index 3230a7f83d..7f77e35e48 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h @@ -86,6 +86,8 @@ private: void _set_filter_register(void); void _fifo_reset(bool log_error) __RAMFUNC__; + void _fast_fifo_reset() __RAMFUNC__; + bool _has_auxiliary_bus(); /* Read samples from FIFO (FIFO enabled) */ @@ -129,12 +131,18 @@ private: LowPassFilter2pFloat _temp_filter; uint32_t last_reset_ms; uint8_t reset_count; + uint8_t fast_reset_count; + uint8_t last_fast_reset_count; + uint32_t last_fast_reset_count_report_ms; enum Rotation _rotation; // enable checking of unexpected resets of offsets bool _enable_offset_checking; + // enable fast fifo reset instead of full fifo reset + bool _enable_fast_fifo_reset; + // ICM-20602 y offset register. See usage for explanation uint8_t _saved_y_ofs_high;