From 471079d5b61ca93eadc6d26e4961ae7a9b16c65d Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 30 Apr 2024 19:44:50 +0100 Subject: [PATCH] AP_InertialSensor: allow FIFO rate logging for hires sampling --- .../AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index 110d3586b6..e815994619 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -468,6 +468,9 @@ static inline float uint20_to_float(uint8_t msb, uint8_t bits, uint8_t lsb) bool AP_InertialSensor_Invensensev3::accumulate_highres_samples(const FIFODataHighRes *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 FIFODataHighRes &d = data[i]; @@ -488,6 +491,9 @@ bool AP_InertialSensor_Invensensev3::accumulate_highres_samples(const FIFODataHi 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