AP_InertialSensor: only use dynamic FIFO when using fast rate loop

ArduPilot · Dec 11, 2024 · 5896d47 · 5896d47
1 parent 860732b
commit 5896d47
Show file tree

Hide file tree

Showing 8 changed files with 23 additions and 13 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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)
 {