aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: added optional logging of register changes

Browse Source
This commit is contained in:
Andrew Tridgell 2021-02-11 15:01:38 +11:00 committed by Peter Barker
parent 7d7a7a365d
commit 85ef6b7ac6
1 changed files with 48 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

@ -52,6 +52,13 @@ extern const AP_HAL::HAL& hal;
#define INVENSENSE_EXT_SYNC_ENABLE 0
#endif
// code to debug unexpected register changes
#define INVENSENSE_DEBUG_REG_CHANGE 0
#if INVENSENSE_DEBUG_REG_CHANGE
#include <GCS_MAVLink/GCS.h>
#endif
#include "AP_InertialSensor_Invensense_registers.h"
#define MPU_SAMPLE_SIZE 14
@ -424,6 +431,47 @@ bool AP_InertialSensor_Invensense::_data_ready()
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
*/
if (_mpu_type != Invensense_ICM20602) {
return;
}
static uint16_t counter;
if (++counter < 100) {
return;
}
counter = 0;
static bool reg_init;
static uint8_t reg_value[0x7f];
static uint8_t next_reg;
if (!reg_init) {
reg_init = true;
for (uint8_t i=0; i<ARRAY_SIZE(reg_value); i++) {
reg_value[i] = _register_read(i);
}
}
bool skip = false;
if ((next_reg >= MPUREG_ACCEL_XOUT_H && next_reg <= MPUREG_GYRO_ZOUT_L) ||
next_reg == MPUREG_MEM_R_W || next_reg == MPUREG_FIFO_R_W ||
next_reg == MPUREG_INT_STATUS ||
next_reg == MPUREG_FIFO_COUNTH || next_reg == MPUREG_FIFO_COUNTL) {
skip = true;
}
if (!skip) {
uint8_t v = _register_read(next_reg);
if (v != reg_value[next_reg]) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "change[%02x] 0x%02x -> 0x%02x",
next_reg, reg_value[next_reg], v);
reg_value[next_reg] = v;
}
}
next_reg = (next_reg+1) % ARRAY_SIZE(reg_value);
#endif // INVENSENSE_DEBUG_REG_CHANGE
}
bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_samples)