Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-25 09:13:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: fixed ICM-20602 y offset bug

Browse Source 
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)
This commit is contained in:
Andrew Tridgell 2021-02-24 10:46:23 +11:00 committed by Peter Barker
parent 4cddf37984
commit bf999f701f
3 changed files with 97 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

@ -23,6 +23,7 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_InternalError/AP_InternalError.h>
#include <AP_Logger/AP_Logger.h>
#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; i<ARRAY_SIZE(regs); i++) {
_register_write(regs[i], _register_read(regs[i]), true);
}
}
if (_mpu_type == Invensense_ICM20602) {
/*
save y offset high separately for ICM20602 to quickly
recover from a change in this register. The ICM20602 has a
bug where every few hours it can change the factory Y offset
register, which leads to a sudden change in Y accelerometer
output
*/
_saved_y_ofs_high = _register_read(MPUREG_ACC_OFF_Y_H);
}
// now that we have initialised, we set the bus speed to high
_dev->set_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);

9
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h
View File

@ -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<AP_HAL::Device> _dev;
AP_Invensense_AuxiliaryBus *_auxiliary_bus;

11
libraries/AP_InertialSensor/AP_InertialSensor_Invensense_registers.h
View File

@ -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