AP_InertialSensor: avoid multiple allocations of rate loop buffer

add nullptr checks and comments to FastRateBuffer

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.

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()
 {
+    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;
     }

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:
     /*