AP_InertialSensor: FFT gyro window would overflow on windows >= 256

convert FFT buffers to ObjectBuffer<float> for lock-free access
push gyro samples directly into the FFT ring buffer from the gyro thread

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -663,14 +663,12 @@ bool AP_InertialSensor::set_gyro_window_size(uint16_t size) {
     // allocate FFT gyro window
     for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
         for (uint8_t j = 0; j < XYZ_AXIS_COUNT; j++) {
-            _gyro_window[i][j] = (float*)hal.util->malloc_type(sizeof(float) * (size), DSP_MEM_REGION);
-            if (_gyro_window[i][j] == nullptr) {
+            if (!_gyro_window[i][j].set_size(size)) {
                 gcs().send_text(MAV_SEVERITY_WARNING, "Failed to allocate window for INS");
                 // clean up whatever we have currently allocated
                 for (uint8_t ii = 0; ii <= i; ii++) {
                     for (uint8_t jj = 0; jj < j; jj++) {
-                        hal.util->free_type(_gyro_window[ii][jj], sizeof(float) * (size), DSP_MEM_REGION);
-                        _gyro_window[ii][jj] = nullptr;
+                        _gyro_window[ii][jj].set_size(0);
                         _gyro_window_size = 0;
                     }
                 }

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -17,7 +17,6 @@
 #define XYZ_AXIS_COUNT    3
 // The maximum we need to store is gyro-rate / loop-rate, worst case ArduCopter with BMI088 is 2000/400
 #define INS_MAX_GYRO_WINDOW_SAMPLES 8
-typedef float* GyroWindow;
 
 #define DEFAULT_IMU_LOG_BAT_MASK 0
 
@@ -25,6 +24,7 @@ typedef float* GyroWindow;
 
 #include <AP_AccelCal/AP_AccelCal.h>
 #include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/utility/RingBuffer.h>
 #include <AP_Math/AP_Math.h>
 #include <Filter/LowPassFilter2p.h>
 #include <Filter/LowPassFilter.h>
@@ -155,11 +155,8 @@ public:
     // FFT support access
 #if HAL_WITH_DSP
     const Vector3f     &get_raw_gyro(void) const { return _gyro_raw[_primary_gyro]; }
-    GyroWindow  get_raw_gyro_window(uint8_t instance, uint8_t axis) const { return _gyro_window[instance][axis]; }
-    GyroWindow  get_raw_gyro_window(uint8_t axis) const { return get_raw_gyro_window(_primary_gyro, axis); }
-    // returns the index one above the last valid gyro sample
-    uint16_t  get_raw_gyro_window_index(void) const { return get_raw_gyro_window_index(_primary_gyro); }
-    uint16_t  get_raw_gyro_window_index(uint8_t instance) const { return _circular_buffer_idx[instance]; }
+    FloatBuffer&  get_raw_gyro_window(uint8_t instance, uint8_t axis) { return _gyro_window[instance][axis]; }
+    FloatBuffer&  get_raw_gyro_window(uint8_t axis) { return get_raw_gyro_window(_primary_gyro, axis); }
     uint16_t get_raw_gyro_rate_hz() const { return get_raw_gyro_rate_hz(_primary_gyro); }
     uint16_t get_raw_gyro_rate_hz(uint8_t instance) const { return _gyro_raw_sample_rates[_primary_gyro]; }
 #endif
@@ -445,9 +442,7 @@ private:
 #if HAL_WITH_DSP
     // Thread-safe public version of _last_raw_gyro
     Vector3f _gyro_raw[INS_MAX_INSTANCES];
-    // circular buffer of gyro data for frequency analysis
-    uint16_t _circular_buffer_idx[INS_MAX_INSTANCES];
-    GyroWindow _gyro_window[INS_MAX_INSTANCES][XYZ_AXIS_COUNT];
+    FloatBuffer _gyro_window[INS_MAX_INSTANCES][XYZ_AXIS_COUNT];
     uint16_t _gyro_window_size;
 #endif
     bool _new_accel_data[INS_MAX_INSTANCES];

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -227,8 +227,12 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
         _imu._last_raw_gyro[instance] = gyro;
 #if HAL_WITH_DSP
         // capture gyro window for FFT analysis
-        _last_gyro_window[_num_gyro_samples++] = gyro * _imu._gyro_raw_sampling_multiplier[instance];
-        _num_gyro_samples = _num_gyro_samples % INS_MAX_GYRO_WINDOW_SAMPLES; // protect against overrun
+        if (_imu._gyro_window_size > 0) {
+            const Vector3f& scaled_gyro = gyro * _imu._gyro_raw_sampling_multiplier[instance];
+            _imu._gyro_window[instance][0].push(scaled_gyro.x);
+            _imu._gyro_window[instance][1].push(scaled_gyro.y);
+            _imu._gyro_window[instance][2].push(scaled_gyro.z);
+        }
 #endif
         // apply the low pass filter
         Vector3f gyro_filtered = _imu._gyro_filter[instance].apply(gyro);
@@ -515,17 +519,6 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
         _publish_gyro(instance, _imu._gyro_filtered[instance]);
         // copy the gyro samples from the backend to the frontend window
 #if HAL_WITH_DSP
-        if (_imu._gyro_window_size > 0) {
-            uint8_t idx = _imu._circular_buffer_idx[instance];
-            for (uint8_t i = 0; i < _num_gyro_samples; i++) {
-                _imu._gyro_window[instance][0][idx] = _last_gyro_window[i].x;
-                _imu._gyro_window[instance][1][idx] = _last_gyro_window[i].y;
-                _imu._gyro_window[instance][2][idx] = _last_gyro_window[i].z;
-                idx = (idx + 1) % _imu._gyro_window_size;
-            }
-            _num_gyro_samples = 0;
-            _imu._circular_buffer_idx[instance] = idx;
-        }
         _imu._gyro_raw[instance] = _imu._last_raw_gyro[instance] * _imu._gyro_raw_sampling_multiplier[instance];
 #endif
         _imu._new_gyro_data[instance] = false;

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -276,11 +276,6 @@ protected:
     float _last_harmonic_notch_bandwidth_hz;
     float _last_harmonic_notch_attenuation_dB;
 
-    // local window of gyro values to be copied to the frontend for FFT analysis
-    uint16_t _last_circular_buffer_idx;
-    uint16_t _num_gyro_samples;
-    Vector3f _last_gyro_window[INS_MAX_GYRO_WINDOW_SAMPLES]; // The maximum we need to store is gyro-rate / loop-rate
-
     void set_gyro_orientation(uint8_t instance, enum Rotation rotation) {
         _imu._gyro_orientation[instance] = rotation;
     }