From bf999f701fc2466e8fb8ba1c3debb5f241996c6c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 24 Feb 2021 10:46:23 +1100 Subject: [PATCH] AP_InertialSensor: fixed ICM-20602 y offset bug this works around a bug in the ICM-20602 that can cause the Y facttory offset register to change unexpectedly. We don't know what triggers this. The fix is to save the factory offset at boot and restore it if it changes. We log a message describing the change, but don't mark the IMU unhealthy as this happens too often and we don't want to fallback to a 2nd less good quality IMU (eg. MPU6000 on MatekH743) --- .../AP_InertialSensor_Invensense.cpp | 88 ++++++++++++++++--- .../AP_InertialSensor_Invensense.h | 9 ++ .../AP_InertialSensor_Invensense_registers.h | 11 +++ 3 files changed, 97 insertions(+), 11 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index d6ce178f56..8f52818c76 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -23,6 +23,7 @@ #include #include +#include #include "AP_InertialSensor_Invensense.h" @@ -213,29 +214,40 @@ void AP_InertialSensor_Invensense::start() case Invensense_MPU9250: gdev = DEVTYPE_GYR_MPU9250; adev = DEVTYPE_ACC_MPU9250; + _enable_offset_checking = true; break; case Invensense_ICM20602: gdev = DEVTYPE_INS_ICM20602; adev = DEVTYPE_INS_ICM20602; + _enable_offset_checking = true; break; case Invensense_ICM20601: gdev = DEVTYPE_INS_ICM20601; adev = DEVTYPE_INS_ICM20601; + _enable_offset_checking = true; break; - case Invensense_MPU6000: - case Invensense_MPU6500: case Invensense_ICM20608: - default: + // unfortunately we don't have a separate ID for 20608, and we + // can't change this without breaking existing calibrations gdev = DEVTYPE_GYR_MPU6000; adev = DEVTYPE_ACC_MPU6000; + _enable_offset_checking = true; break; case Invensense_ICM20789: gdev = DEVTYPE_INS_ICM20789; adev = DEVTYPE_INS_ICM20789; + _enable_offset_checking = true; break; case Invensense_ICM20689: gdev = DEVTYPE_INS_ICM20689; adev = DEVTYPE_INS_ICM20689; + _enable_offset_checking = true; + break; + case Invensense_MPU6000: + case Invensense_MPU6500: + default: + gdev = DEVTYPE_GYR_MPU6000; + adev = DEVTYPE_ACC_MPU6000; break; } @@ -343,6 +355,35 @@ void AP_InertialSensor_Invensense::start() _register_write(MPUREG_INT_PIN_CFG, v); } + if (_enable_offset_checking) { + /* + there is a bug in at least the ICM-20602 where the + MPUREG_ACC_OFF_Y_H changes at runtime, which adds an offset + on the Y accelerometer. To prevent this we read the factory + cal values of the sensor at startup and write them back as + checked register values. Then we rely on the register + checking code to detect the change and fix it + */ + uint8_t regs[] = { MPUREG_ACC_OFF_X_H, MPUREG_ACC_OFF_X_L, + MPUREG_ACC_OFF_Y_H, MPUREG_ACC_OFF_Y_L, + MPUREG_ACC_OFF_Z_H, MPUREG_ACC_OFF_Z_L }; + for (uint8_t i=0; iset_speed(AP_HAL::Device::SPEED_HIGH); @@ -433,11 +474,18 @@ void AP_InertialSensor_Invensense::_poll_data() _read_fifo(); #if INVENSENSE_DEBUG_REG_CHANGE - /* - catch unexpected register changes on an IMU. This was used to - find the bug in the ICM-20602 that causes the Y accel offset - trim register to change value in flight - */ + _check_register_change(); +#endif // INVENSENSE_DEBUG_REG_CHANGE +} + +#if INVENSENSE_DEBUG_REG_CHANGE +/* + catch unexpected register changes on an IMU. This was used to + find the bug in the ICM-20602 that causes the Y accel offset + trim register to change value in flight +*/ +void AP_InertialSensor_Invensense::_check_register_change(void) +{ if (_mpu_type != Invensense_ICM20602) { return; } @@ -447,7 +495,7 @@ void AP_InertialSensor_Invensense::_poll_data() } counter = 0; static bool reg_init; - static uint8_t reg_value[0x7f]; + static uint8_t reg_value[0x80]; static uint8_t next_reg; if (!reg_init) { reg_init = true; @@ -471,8 +519,8 @@ void AP_InertialSensor_Invensense::_poll_data() } } next_reg = (next_reg+1) % ARRAY_SIZE(reg_value); -#endif // INVENSENSE_DEBUG_REG_CHANGE } +#endif // INVENSENSE_DEBUG_REG_CHANGE bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_samples) { @@ -692,6 +740,24 @@ void AP_InertialSensor_Invensense::_read_fifo() check_registers: // check next register value for correctness + + if (_mpu_type == Invensense_ICM20602) { + const uint8_t y_ofs = _register_read(MPUREG_ACC_OFF_Y_H); + if (y_ofs != _saved_y_ofs_high) { + /* + we check and restore the ICM20602 Y offset high register + on every update. We don't mark the IMU unhealthy when we + do this. This is a workaround for a bug in the ICM-20602 + where this register can change in flight. We log these + events to help with log analysis, but don't shout at the + GCS to prevent possible flood + */ + AP::logger().Write_MessageF("ICM20602 yofs fix: %x %x", y_ofs, _saved_y_ofs_high); + _register_write(MPUREG_ACC_OFF_Y_H, _saved_y_ofs_high); + } + } + + _dev->set_speed(AP_HAL::Device::SPEED_LOW); AP_HAL::Device::checkreg reg; if (!_dev->check_next_register(reg)) { @@ -868,7 +934,7 @@ bool AP_InertialSensor_Invensense::_hardware_init(void) // setup for register checking. We check much less often on I2C // where the cost of the checks is higher - _dev->setup_checked_registers(7, _dev->bus_type() == AP_HAL::Device::BUS_TYPE_I2C?200:20); + _dev->setup_checked_registers(13, _dev->bus_type() == AP_HAL::Device::BUS_TYPE_I2C?200:20); // initially run the bus at low speed _dev->set_speed(AP_HAL::Device::SPEED_LOW); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h index 7dfe08bdcf..c92a0e71f4 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h @@ -98,6 +98,9 @@ private: /* Poll for new data (non-blocking) */ void _poll_data(); + // debug function to watch for register changes + void _check_register_change(void); + /* Read and write functions taking the differences between buses into * account */ bool _block_read(uint8_t reg, uint8_t *buf, uint32_t size); @@ -130,6 +133,12 @@ private: enum Rotation _rotation; + // enable checking of unexpected resets of offsets + bool _enable_offset_checking; + + // ICM-20602 y offset register. See usage for explanation + uint8_t _saved_y_ofs_high; + AP_HAL::DigitalSource *_drdy_pin; AP_HAL::OwnPtr _dev; AP_Invensense_AuxiliaryBus *_auxiliary_bus; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense_registers.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense_registers.h index a9eb779f97..bf1973e354 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense_registers.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense_registers.h @@ -146,6 +146,17 @@ #define MPUREG_FIFO_R_W 0x74 #define MPUREG_WHOAMI 0x75 +// accelerometer offsets, valid on ICM-2xxxx and MPU-9250. These hold +// factory calibrated offsets. We need to ensure these do not change +// in flight. There is a bug in at least the ICM-26002 that can cause +// these to change value in flight. +#define MPUREG_ACC_OFF_X_H 0x77 +#define MPUREG_ACC_OFF_X_L 0x78 +#define MPUREG_ACC_OFF_Y_H 0x7a +#define MPUREG_ACC_OFF_Y_L 0x7b +#define MPUREG_ACC_OFF_Z_H 0x7d +#define MPUREG_ACC_OFF_Z_L 0x7e + // ICM20608 specific registers #define ICMREG_ACCEL_CONFIG2 0x1D #define ICM_ACC_DLPF_CFG_1046HZ_NOLPF 0x00