Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

IMU: Fixed ICM42688 "stuck" gyro issue #25332

Merged
merged 2 commits into from
Oct 22, 2023
Merged

IMU: Fixed ICM42688 "stuck" gyro issue #25332

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6f25ca9
AP_InertialSensor: fix for ICM42688 stuck gyro issue
tridge Oct 21, 2023
3bf1984
AP_InertialSensor: added optional FIFO rate logging to invensensev3 d…
tridge Oct 21, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -319,6 +319,8 @@ class AP_InertialSensor_Backend

// logging
void Write_ACC(const uint8_t instance, const uint64_t sample_us, const Vector3f &accel) const __RAMFUNC__; // Write ACC data packet: raw accel data
void Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro) const __RAMFUNC__; // Write GYR data packet: raw gyro data

protected:
void Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro, bool use_sample_timestamp=false) const __RAMFUNC__; // Write GYR data packet: raw gyro data

};
17 changes: 17 additions & 0 deletions libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,6 +125,9 @@ extern const AP_HAL::HAL& hal;
#define INV3BANK_456_IPREG_SYS1_ADDR 0xA400
#define INV3BANK_456_IPREG_SYS2_ADDR 0xA500

// ICM42688 specific registers
#define INV3REG_42688_INTF_CONFIG1 0x4d

// WHOAMI values
#define INV3_ID_ICM40605 0x33
#define INV3_ID_ICM40609 0x3b
Expand All @@ -134,6 +137,9 @@ extern const AP_HAL::HAL& hal;
#define INV3_ID_ICM42670 0x67
#define INV3_ID_ICM45686 0xE9

// enable logging at FIFO rate for debugging
#define INV3_ENABLE_FIFO_LOGGING 0

/*
really nice that this sensor has an option to request little-endian
data
Expand Down Expand Up @@ -333,6 +339,9 @@ void AP_InertialSensor_Invensensev3::accumulate()

bool AP_InertialSensor_Invensensev3::accumulate_samples(const FIFOData *data, uint8_t n_samples)
{
#if INV3_ENABLE_FIFO_LOGGING
const uint64_t tstart = AP_HAL::micros64();
#endif
for (uint8_t i = 0; i < n_samples; i++) {
const FIFOData &d = data[i];

Expand All @@ -349,6 +358,10 @@ bool AP_InertialSensor_Invensensev3::accumulate_samples(const FIFOData *data, ui
accel *= accel_scale;
gyro *= gyro_scale;

#if INV3_ENABLE_FIFO_LOGGING
Write_GYR(gyro_instance, tstart+(i*backend_period_us), gyro, true);
#endif

const float temp = d.temperature * temp_sensitivity + temp_zero;

// these four calls are about 40us
Expand Down Expand Up @@ -909,6 +922,10 @@ bool AP_InertialSensor_Invensensev3::hardware_init(void)
// disable STC
uint8_t reg = register_read_bank_icm456xy(INV3BANK_456_IPREG_TOP1_ADDR, 0x68); // I3C_STC_MODE b2
register_write_bank_icm456xy(INV3BANK_456_IPREG_TOP1_ADDR, 0x68, reg & ~0x04);
} else if (inv3_type == Invensensev3_Type::ICM42688) {
// fix for the "stuck gyro" issue
const uint8_t v = register_read(INV3REG_42688_INTF_CONFIG1);
register_write(INV3REG_42688_INTF_CONFIG1, (v & 0x3F) | 0x40);
}

return true;
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,9 @@ void AP_InertialSensor_Backend::Write_ACC(const uint8_t instance, const uint64_t
}

// Write GYR data packet: raw gyro data
void AP_InertialSensor_Backend::Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro) const
void AP_InertialSensor_Backend::Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro, bool use_sample_timestamp) const
{
const uint64_t now = AP_HAL::micros64();
const uint64_t now = use_sample_timestamp?sample_us:AP_HAL::micros64();
const struct log_GYR pkt{
LOG_PACKET_HEADER_INIT(LOG_GYR_MSG),
time_us : now,
Expand Down