From 398a70ec4b2a72aed6d3a2addf53a6c1c3a6bc6a Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Fri, 25 Oct 2024 13:04:27 +0900
Subject: [PATCH] AP_InertialSensor: avoid multiple allocations of rate loop
 buffer

add nullptr checks and comments to FastRateBuffer
---
 .../AP_InertialSensor/AP_InertialSensor.h     |  3 +-
 .../AP_InertialSensor/FastRateBuffer.cpp      | 45 ++++++++++++++-----
 libraries/AP_InertialSensor/FastRateBuffer.h  |  1 +
 3 files changed, 36 insertions(+), 13 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 446a46f285..8cd1fd2307 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -806,6 +806,7 @@ private:
     // if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
     // Support for the fast rate thread in copter
     FastRateBuffer* fast_rate_buffer;
+    bool fast_rate_buffer_enabled;
 
 public:
     // enable the fast rate buffer and start pushing samples to it
@@ -818,8 +819,6 @@ public:
     uint32_t get_num_gyro_samples();
     // set the rate at which samples are collected, unused samples are dropped
     void set_rate_decimation(uint8_t rdec);
-    // are gyro samples being sourced from the rate loop buffer
-    bool use_rate_loop_gyro_samples() const;
     // push a new gyro sample into the fast rate buffer
     bool push_next_gyro_sample(const Vector3f& gyro);
     // run the filter parmeter update code.
diff --git a/libraries/AP_InertialSensor/FastRateBuffer.cpp b/libraries/AP_InertialSensor/FastRateBuffer.cpp
index 6617915fec..c634c84d87 100644
--- a/libraries/AP_InertialSensor/FastRateBuffer.cpp
+++ b/libraries/AP_InertialSensor/FastRateBuffer.cpp
@@ -31,48 +31,62 @@ extern const AP_HAL::HAL& hal;
 #define debug(fmt, args ...)  do {printf("IMU: " fmt "\n", ## args); } while(0)
 #endif
 
+// enable the fast rate buffer and start pushing samples to it
 void AP_InertialSensor::enable_fast_rate_buffer()
 {
-    fast_rate_buffer = NEW_NOTHROW FastRateBuffer();
+    if (fast_rate_buffer_enabled) {
+        return;
+    }
+    if (fast_rate_buffer == nullptr) {
+        fast_rate_buffer = NEW_NOTHROW FastRateBuffer();
+    }
+    fast_rate_buffer_enabled = true;
 }
 
+// disable the fast rate buffer and stop pushing samples to it
 void AP_InertialSensor::disable_fast_rate_buffer()
 {
-    delete fast_rate_buffer;
-    fast_rate_buffer = nullptr;
+    fast_rate_buffer_enabled = false;
+    if (fast_rate_buffer != nullptr) {
+        fast_rate_buffer->reset();
+    }
 }
 
+// get the number of available gyro samples in the fast rate buffer
 uint32_t AP_InertialSensor::get_num_gyro_samples()
 {
+    if (fast_rate_buffer == nullptr) {
+        return 0;
+    }
     return fast_rate_buffer->get_num_gyro_samples();
 }
 
+// set the rate at which samples are collected, unused samples are dropped
 void AP_InertialSensor::set_rate_decimation(uint8_t rdec)
 {
+    if (fast_rate_buffer == nullptr) {
+        return;
+    }
     fast_rate_buffer->set_rate_decimation(rdec);
 }
 
-// are gyro samples being sourced from the rate loop buffer
-bool AP_InertialSensor::use_rate_loop_gyro_samples() const
-{
-    return fast_rate_buffer != nullptr;
-}
-
 // whether or not to push the current gyro sample
 bool AP_InertialSensor::is_rate_loop_gyro_enabled(uint8_t instance) const
 {
-    return use_rate_loop_gyro_samples() && fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index();
+    return fast_rate_buffer_enabled && fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index();
 }
 
+// get the next available gyro sample from the fast rate buffer
 bool AP_InertialSensor::get_next_gyro_sample(Vector3f& gyro)
 {
-    if (!use_rate_loop_gyro_samples()) {
+    if (!fast_rate_buffer_enabled) {
         return false;
     }
 
     return fast_rate_buffer->get_next_gyro_sample(gyro);
 }
 
+
 bool FastRateBuffer::get_next_gyro_sample(Vector3f& gyro)
 {
     if (!use_rate_loop_gyro_samples()) {
@@ -88,8 +102,17 @@ bool FastRateBuffer::get_next_gyro_sample(Vector3f& gyro)
     return _rate_loop_gyro_window.pop(gyro);
 }
 
+void FastRateBuffer::reset()
+{
+    _rate_loop_gyro_window.clear();
+}
+
 bool AP_InertialSensor::push_next_gyro_sample(const Vector3f& gyro)
 {
+    if (fast_rate_buffer == nullptr) {
+        return false;
+    }
+
     if (++fast_rate_buffer->rate_decimation_count < fast_rate_buffer->rate_decimation) {
         return false;
     }
diff --git a/libraries/AP_InertialSensor/FastRateBuffer.h b/libraries/AP_InertialSensor/FastRateBuffer.h
index 712500fb8d..c4f53b7bf6 100644
--- a/libraries/AP_InertialSensor/FastRateBuffer.h
+++ b/libraries/AP_InertialSensor/FastRateBuffer.h
@@ -35,6 +35,7 @@ public:
     // whether or not to push the current gyro sample
     bool use_rate_loop_gyro_samples() const { return rate_decimation > 0; }
     bool gyro_samples_available() { return  _rate_loop_gyro_window.available() > 0; }
+    void reset();
 
 private:
     /*