From 5896d476da9e38543ca59844e41ca7ac8682c7ab Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Wed, 4 Dec 2024 20:22:15 +0000
Subject: [PATCH] AP_InertialSensor: only use dynamic FIFO when using fast rate
 loop

---
 libraries/AP_InertialSensor/AP_InertialSensor.h       |  2 ++
 .../AP_InertialSensor_Invensense.cpp                  |  1 +
 .../AP_InertialSensor_Invensensev2.cpp                |  1 +
 .../AP_InertialSensor_Invensensev3.cpp                |  7 ++++---
 .../AP_InertialSensor_Invensensev3.h                  |  2 --
 .../AP_InertialSensor/AP_InertialSensor_config.h      |  8 --------
 .../AP_InertialSensor/AP_InertialSensor_rate_config.h |  4 ++++
 libraries/AP_InertialSensor/FastRateBuffer.cpp        | 11 +++++++++++
 8 files changed, 23 insertions(+), 13 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index a0c6c5ea4d4298..be4bc5ce4d6cd8 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -831,6 +831,8 @@ class AP_InertialSensor : AP_AccelCal_Client
     void update_backend_filters();
     // are rate loop samples enabled for this instance?
     bool is_rate_loop_gyro_enabled(uint8_t instance) const;
+    // is dynamic fifo enabled for this instance
+    bool is_dynamic_fifo_enabled(uint8_t instance) const;
     // endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
 };
 
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
index 18eb0a2080fa52..ee71ae48d14f24 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
@@ -26,6 +26,7 @@
 #include <AP_InternalError/AP_InternalError.h>
 #include <AP_Logger/AP_Logger.h>
 
+#include "AP_InertialSensor_rate_config.h"
 #include "AP_InertialSensor_Invensense.h"
 #include <GCS_MAVLink/GCS.h>
 
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp
index b8279d339fd624..f234275c395e1f 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp
@@ -23,6 +23,7 @@
 
 #include <AP_HAL/AP_HAL.h>
 
+#include "AP_InertialSensor_rate_config.h"
 #include "AP_InertialSensor_Invensensev2.h"
 
 extern const AP_HAL::HAL& hal;
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
index 20479faafb5ba7..2bf21b7f8a2a7a 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
@@ -31,6 +31,7 @@
  */
 
 #include <AP_HAL/AP_HAL.h>
+#include "AP_InertialSensor_rate_config.h"
 #include "AP_InertialSensor_Invensensev3.h"
 #include <utility>
 #include <stdio.h>
@@ -405,18 +406,18 @@ bool AP_InertialSensor_Invensensev3::update()
     return true;
 }
 
-#if AP_INERTIALSENSOR_DYNAMIC_FIFO
 void AP_InertialSensor_Invensensev3::set_primary_gyro(bool is_primary)
 {
-    if (((1U<<gyro_instance) & AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK) && enable_fast_sampling(gyro_instance)) {
+#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
+    if (_imu.is_dynamic_fifo_enabled(gyro_instance)) {
         if (is_primary) {
             dev->adjust_periodic_callback(periodic_handle, backend_period_us);
         } else {
             dev->adjust_periodic_callback(periodic_handle, backend_period_us * get_fast_sampling_rate());
         }
     }
-}
 #endif
+}
 
 /*
   accumulate new samples
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
index c6e7ecaf612a1a..91f3eda4a3f592 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
@@ -46,9 +46,7 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend
     const uint16_t multiplier_accel = INT16_MAX/(32*GRAVITY_MSS);
 
 protected:
-#if AP_INERTIALSENSOR_DYNAMIC_FIFO
     void set_primary_gyro(bool is_primary) override;
-#endif
 
 private:
     AP_InertialSensor_Invensensev3(AP_InertialSensor &imu,
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_config.h b/libraries/AP_InertialSensor/AP_InertialSensor_config.h
index aae26186ad9fb4..5ad27d03003346 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_config.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_config.h
@@ -73,11 +73,3 @@
 #ifndef AP_INERTIALSENSOR_KILL_IMU_ENABLED
 #define AP_INERTIALSENSOR_KILL_IMU_ENABLED 1
 #endif
-
-#ifndef AP_INERTIALSENSOR_DYNAMIC_FIFO
-#define AP_INERTIALSENSOR_DYNAMIC_FIFO 1
-#endif
-
-#ifndef AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK
-#define AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK 127
-#endif
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h b/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h
index 8bf45fecab7f05..b7305e76964a09 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h
@@ -7,3 +7,7 @@
 #ifndef AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
 #define AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED (AP_INERTIALSENSOR_ENABLED && HAL_INS_RATE_LOOP && AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED && APM_BUILD_TYPE(APM_BUILD_ArduCopter))
 #endif
+
+#ifndef AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK
+#define AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK 127
+#endif
diff --git a/libraries/AP_InertialSensor/FastRateBuffer.cpp b/libraries/AP_InertialSensor/FastRateBuffer.cpp
index 29601965d38ed5..b47450493d41ea 100644
--- a/libraries/AP_InertialSensor/FastRateBuffer.cpp
+++ b/libraries/AP_InertialSensor/FastRateBuffer.cpp
@@ -79,6 +79,17 @@ bool AP_InertialSensor::is_rate_loop_gyro_enabled(uint8_t instance) const
     return fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index();
 }
 
+// whether or not to use the dynamic fifo
+bool AP_InertialSensor::is_dynamic_fifo_enabled(uint8_t instance) const
+{
+    if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) {
+        return false;
+    }
+    return ((1U<<instance) & AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK)
+            && (_fast_sampling_mask & (1U<<instance)) != 0
+            && fast_rate_buffer->use_rate_loop_gyro_samples();
+}
+
 // get the next available gyro sample from the fast rate buffer
 bool AP_InertialSensor::get_next_gyro_sample(Vector3f& gyro)
 {