AP_InertialSensor: added optional FIFO rate logging to invensensev3 d…

…river this is useful for tracking down and confirming the stuck gyro issue on the ICM42688
ArduPilot · Oct 21, 2023 · 3bf1984 · 3bf1984
1 parent 6f25ca9
commit 3bf1984
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 3 deletions.
diff --git 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
@@ -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
@@ -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,