From 3bf198471a1da6f0cdb6f276c2b96cda6ef7444f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 22 Oct 2023 08:43:46 +1100 Subject: [PATCH] AP_InertialSensor: added optional FIFO rate logging to invensensev3 driver this is useful for tracking down and confirming the stuck gyro issue on the ICM42688 --- .../AP_InertialSensor/AP_InertialSensor_Backend.h | 4 +++- .../AP_InertialSensor_Invensensev3.cpp | 10 ++++++++++ .../AP_InertialSensor/AP_InertialSensor_Logging.cpp | 4 ++-- 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index a9bd7bb411808..4e43b1d55c621 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -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 }; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index e20ce39041095..c6c8607be7433 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -137,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 @@ -336,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]; @@ -352,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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp index cbf5f8db6836c..e59019d7ded19 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp @@ -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,