From d7f9bd21b17c5fee5f5b4e3b800380bb92f6b835 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Wed, 17 Jul 2024 12:54:26 +1000
Subject: [PATCH] AP_InertialSensor: do not read FIFO faster than requested
 rate for ICM45686

---
 .../AP_InertialSensor_Invensensev3.cpp        | 54 +++++++++----------
 .../AP_InertialSensor_Invensensev3.h          |  1 +
 2 files changed, 25 insertions(+), 30 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
index 9d1272cc5eebf1..3956b3bc74bd9d 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
@@ -348,10 +348,6 @@ void AP_InertialSensor_Invensensev3::start()
         return;
     }
 
-    // update backend sample rate
-    _set_accel_raw_sample_rate(accel_instance, backend_rate_hz);
-    _set_gyro_raw_sample_rate(gyro_instance, backend_rate_hz);
-
     // indicate what multiplier is appropriate for the sensors'
     // readings to fit them into an int16_t:
     _set_raw_sample_accel_multiplier(accel_instance, multiplier_accel);
@@ -381,16 +377,15 @@ void AP_InertialSensor_Invensensev3::start()
 
 // get a startup banner to output to the GCS
 bool AP_InertialSensor_Invensensev3::get_output_banner(char* banner, uint8_t banner_len) {
-    if (fast_sampling) {
-        snprintf(banner, banner_len, "IMU%u: fast%s sampling enabled %.1fkHz",
-            gyro_instance,
+    snprintf(banner, banner_len, "IMU%u:%s%s sampling BR:%.1fkHz SR:%.1fkHz",
+        gyro_instance,
+        fast_sampling ? " fast" : " normal",
 #if HAL_INS_HIGHRES_SAMPLE
-            highres_sampling ? ", high-resolution" :
+        highres_sampling ? " hi-res" :
 #endif
-            "" , backend_rate_hz * 0.001);
-        return true;
-    }
-    return false;
+        "", backend_rate_hz * 0.001,
+        sampling_rate_hz * 0.001);
+    return true;
 }
 
 /*
@@ -806,6 +801,7 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void)
             }
         }
     }
+    sampling_rate_hz = backend_rate_hz; // sampling rate and backend rate are the same
 
     // disable gyro and accel as per 12.9 in the ICM-42688 docs
     register_write(INV3REG_PWR_MGMT0, 0x00);
@@ -862,6 +858,7 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void)
 void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm42670(void)
 {
     backend_rate_hz = 1600;
+    sampling_rate_hz = 1600;
     // use low-noise mode
     register_write(INV3REG_70_PWR_MGMT0, 0x0f);
     hal.scheduler->delay_microseconds(300);
@@ -886,27 +883,24 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm42670(void)
  */
 void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm456xy(void)
 {
-    uint8_t odr_config = 4;
-    backend_rate_hz = 1600;
-    // always fast sampling
-    fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
+    uint8_t odr_config;
+    backend_rate_hz = 800;
 
-    if (enable_fast_sampling(accel_instance) && get_fast_sampling_rate() > 1) {
-        backend_rate_hz = calculate_fast_sampling_backend_rate(backend_rate_hz, backend_rate_hz * 4);
+    fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
+    if (enable_fast_sampling(accel_instance) && get_fast_sampling_rate() && fast_sampling) {
+        // For ICM45686 we only set 3200 or 6400Hz as sampling rates
+        // we don't need to read FIFO faster than requested rate
+        backend_rate_hz = calculate_fast_sampling_backend_rate(800, 6400);
+    } else {
+        fast_sampling = false;
     }
 
-    // this sensor actually only supports 2 speeds
-    backend_rate_hz = constrain_int16(backend_rate_hz, 3200, 6400);
-
-    switch (backend_rate_hz) {
-    case 6400: // 6.4Khz
-        odr_config = 3;
-        break;
-    case 3200: // 3.2Khz
-        odr_config = 4;
-        break;
-    default:
-        break;
+    if (backend_rate_hz <= 3200) {
+        odr_config = 0x04; // 3200Hz
+        sampling_rate_hz = 3200;
+    } else {
+        odr_config = 0x03; // 6400Hz
+        sampling_rate_hz = 6400;
     }
 
     // Disable FIFO first
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
index 36d83c433d5f44..889fda4f76c86c 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
@@ -141,4 +141,5 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend
 
     float temp_filtered;
     LowPassFilter2pFloat temp_filter;
+    uint32_t sampling_rate_hz;
 };