AP_InertialSensor: do not read FIFO faster than requested rate for IC…

…M45686
ArduPilot · Nov 19, 2024 · 93fc998 · 93fc998
1 parent 37d1e8d
commit 93fc998
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 28 deletions.
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
@@ -349,8 +349,8 @@ void AP_InertialSensor_Invensensev3::start()
     }
 
     // update backend sample rate
-    _set_accel_raw_sample_rate(accel_instance, backend_rate_hz);
-    _set_gyro_raw_sample_rate(gyro_instance, backend_rate_hz);
+    _set_accel_raw_sample_rate(accel_instance, sampling_rate_hz);
+    _set_gyro_raw_sample_rate(gyro_instance, sampling_rate_hz);
 
     // indicate what multiplier is appropriate for the sensors'
     // readings to fit them into an int16_t:
@@ -381,16 +381,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 LR:%.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 +805,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 +862,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 +887,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
@@ -141,4 +141,5 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend
 
     float temp_filtered;
     LowPassFilter2pFloat temp_filter;
+    uint32_t sampling_rate_hz;
 };