AP_HAL: save a sliding window array of frequency bins

calculate frequency average from sliding window

libraries/AP_HAL/DSP.cpp

@@ -21,6 +21,9 @@
 #ifndef HAL_NO_UARTDRIVER
 #include <GCS_MAVLink/GCS.h>
 #endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include <assert.h>
+#endif
 
 #if HAL_WITH_DSP
 
@@ -31,28 +34,33 @@ extern const AP_HAL::HAL &hal;
 #define SQRT_2_3 0.816496580927726f
 #define SQRT_6   2.449489742783178f
 
-DSP::FFTWindowState::FFTWindowState(uint16_t window_size, uint16_t sample_rate)
+DSP::FFTWindowState::FFTWindowState(uint16_t window_size, uint16_t sample_rate, uint8_t sliding_window_size)
     : _window_size(window_size),
     _bin_count(window_size / 2),
-    _bin_resolution((float)sample_rate / (float)window_size)
+    _num_stored_freqs(window_size / 2 + 1),
+    _bin_resolution((float)sample_rate / (float)window_size),
+    _sliding_window_size(sliding_window_size),
+    _current_slice(0)
 {
     // includes DC ad Nyquist components and needs to be large enough for intermediate steps
-    _freq_bins = (float*)hal.util->malloc_type(sizeof(float) * (window_size), DSP_MEM_REGION);
-    _derivative_freq_bins = (float*)hal.util->malloc_type(sizeof(float) * (_bin_count + 1), DSP_MEM_REGION);
-    _hanning_window = (float*)hal.util->malloc_type(sizeof(float) * (window_size), DSP_MEM_REGION);
+    _freq_bins = (float*)hal.util->malloc_type(sizeof(float) * _window_size, DSP_MEM_REGION);
+    _derivative_freq_bins = (float*)hal.util->malloc_type(sizeof(float) * _num_stored_freqs, DSP_MEM_REGION);
+    _hanning_window = (float*)hal.util->malloc_type(sizeof(float) * _window_size, DSP_MEM_REGION);
     // allocate workspace, including Nyquist component
     _rfft_data = (float*)hal.util->malloc_type(sizeof(float) * (_window_size + 2), DSP_MEM_REGION);
+    // sliding window of frequency bin frames
+    if (_sliding_window_size > 0) {
+        _sliding_window = (float*)hal.util->malloc_type(sizeof(float) * _num_stored_freqs * _sliding_window_size, DSP_MEM_REGION);
+        _avg_freq_bins = (float*)hal.util->malloc_type(sizeof(float) * _num_stored_freqs, DSP_MEM_REGION);
+        // we can still fallback to non-averaging if there is not enough memory
+        if (_avg_freq_bins == nullptr) {
+            hal.util->free_type(_sliding_window, sizeof(float) * _num_stored_freqs * _sliding_window_size, DSP_MEM_REGION);
+            _sliding_window = nullptr;
+        }
+    }
 
     if (_freq_bins == nullptr || _hanning_window == nullptr || _rfft_data == nullptr || _derivative_freq_bins == nullptr) {
-        hal.util->free_type(_freq_bins, sizeof(float) * (_window_size), DSP_MEM_REGION);
-        hal.util->free_type(_derivative_freq_bins, sizeof(float) * (_bin_count + 1), DSP_MEM_REGION);
-        hal.util->free_type(_hanning_window, sizeof(float) * (_window_size), DSP_MEM_REGION);
-        hal.util->free_type(_rfft_data, sizeof(float) * (_window_size + 2), DSP_MEM_REGION);
-
-        _freq_bins = nullptr;
-        _derivative_freq_bins = nullptr;
-        _hanning_window = nullptr;
-        _rfft_data = nullptr;
+        free_data_structures();
         return;
     }
 
@@ -68,16 +76,23 @@ DSP::FFTWindowState::FFTWindowState(uint16_t window_size, uint16_t sample_rate)
 
 DSP::FFTWindowState::~FFTWindowState()
 {
-    hal.util->free_type(_freq_bins, sizeof(float) * (_window_size), DSP_MEM_REGION);
+    free_data_structures();
+}
+
+void DSP::FFTWindowState::free_data_structures()
+{
+    hal.util->free_type(_freq_bins, sizeof(float) * _window_size * _sliding_window_size, DSP_MEM_REGION);
     _freq_bins = nullptr;
-    hal.util->free_type(_derivative_freq_bins, sizeof(float) * (_bin_count + 1), DSP_MEM_REGION);
+    hal.util->free_type(_derivative_freq_bins, sizeof(float) * _num_stored_freqs, DSP_MEM_REGION);
     _derivative_freq_bins = nullptr;
     hal.util->free_type(_hanning_window, sizeof(float) * (_window_size), DSP_MEM_REGION);
     _hanning_window = nullptr;
     hal.util->free_type(_rfft_data, sizeof(float) * (_window_size + 2), DSP_MEM_REGION);
     _rfft_data = nullptr;
-    hal.util->free_type(_avg_freq_bins, sizeof(float) * (_window_size), DSP_MEM_REGION);
+    hal.util->free_type(_avg_freq_bins, sizeof(float) * _num_stored_freqs, DSP_MEM_REGION);
     _avg_freq_bins = nullptr;
+    hal.util->free_type(_sliding_window, sizeof(float) * _num_stored_freqs * _sliding_window_size, DSP_MEM_REGION);
+    _sliding_window = nullptr;
 }
 
 // step 3: find the magnitudes of the complex data
@@ -87,11 +102,21 @@ void DSP::step_cmplx_mag(FFTWindowState* fft, uint16_t start_bin, uint16_t end_b
     // find the maximum power in the range we are interested in
     // in order to see a peak in the last bin we need to allow all the way up to the nyquist
     const uint16_t smoothwidth = 1;
+    float* freq_data = fft->_freq_bins;
+
+    if (fft->_sliding_window != nullptr) {
+        update_average_from_sliding_window(fft);
+        freq_data = fft->_avg_freq_bins;
+    } else {
+        // scale the power to account for the input window
+        vector_scale_float(fft->_freq_bins, fft->_window_scale, fft->_freq_bins, fft->_bin_count);
+    }
+
     uint16_t bin_range = (MIN(end_bin + ((smoothwidth + 1) >> 1) + 2, fft->_bin_count) - start_bin) + 1;
     // find the three highest peaks using a zero crossing algorithm
     uint16_t peaks[MAX_TRACKED_PEAKS] {};
     memset(fft->_peak_data, 0, sizeof(fft->_peak_data));
-    uint16_t numpeaks = find_peaks(&fft->_freq_bins[start_bin], bin_range, fft->_derivative_freq_bins, peaks, MAX_TRACKED_PEAKS, 0.0f, -1.0f, smoothwidth, 2);
+    uint16_t numpeaks = find_peaks(&freq_data[start_bin], bin_range, fft->_derivative_freq_bins, peaks, MAX_TRACKED_PEAKS, 0.0f, -1.0f, smoothwidth, 2);
     //hal.console->printf("found %d peaks\n", numpeaks);
 
     for (uint16_t i = 0; i < MAX_TRACKED_PEAKS; i++) {
@@ -99,17 +124,14 @@ void DSP::step_cmplx_mag(FFTWindowState* fft, uint16_t start_bin, uint16_t end_b
     }
 
     uint16_t top = 0, bottom = 0;
-    fft->_peak_data[CENTER]._noise_width_hz = find_noise_width(fft, start_bin, end_bin, fft->_peak_data[CENTER]._bin, noise_att_cutoff, top, bottom);
+    fft->_peak_data[CENTER]._noise_width_hz = find_noise_width(freq_data, start_bin, end_bin, fft->_peak_data[CENTER]._bin, noise_att_cutoff, fft->_bin_resolution, top, bottom);
     if (numpeaks > 1) {
-        fft->_peak_data[LOWER_SHOULDER]._noise_width_hz = find_noise_width(fft, start_bin, end_bin, fft->_peak_data[LOWER_SHOULDER]._bin, noise_att_cutoff, top, bottom);
+        fft->_peak_data[LOWER_SHOULDER]._noise_width_hz = find_noise_width(freq_data, start_bin, end_bin, fft->_peak_data[LOWER_SHOULDER]._bin, noise_att_cutoff, fft->_bin_resolution, top, bottom);
     }
     if (numpeaks > 2) {
-        fft->_peak_data[UPPER_SHOULDER]._noise_width_hz = find_noise_width(fft, start_bin, end_bin, fft->_peak_data[UPPER_SHOULDER]._bin, noise_att_cutoff, top, bottom);
+        fft->_peak_data[UPPER_SHOULDER]._noise_width_hz = find_noise_width(freq_data, start_bin, end_bin, fft->_peak_data[UPPER_SHOULDER]._bin, noise_att_cutoff, fft->_bin_resolution, top, bottom);
     }
 
-    // scale the power to account for the input window
-    vector_scale_float(fft->_freq_bins, fft->_window_scale, fft->_freq_bins, fft->_bin_count);
-
     // average the FFT data
     if (fft->_averaging) {
         vector_add_float(fft->_avg_freq_bins, fft->_freq_bins, fft->_avg_freq_bins, fft->_bin_count);
@@ -118,7 +140,8 @@ void DSP::step_cmplx_mag(FFTWindowState* fft, uint16_t start_bin, uint16_t end_b
 }
 
 // calculate the noise width of a peak based on the input parameters
-float DSP::find_noise_width(FFTWindowState* fft, uint16_t start_bin, uint16_t end_bin, uint16_t max_energy_bin, float cutoff, uint16_t& peak_top, uint16_t& peak_bottom) const
+// freq_bins can be scaled or unscaled for power
+float DSP::find_noise_width(float* freq_bins, uint16_t start_bin, uint16_t end_bin, uint16_t max_energy_bin, float cutoff, float bin_resolution, uint16_t& peak_top, uint16_t& peak_bottom) const
 {
     // max_energy_bin is guaranteed to be between start_bin and end_bin
     peak_top = end_bin;
@@ -130,7 +153,7 @@ float DSP::find_noise_width(FFTWindowState* fft, uint16_t start_bin, uint16_t en
     // -attenuation/2 dB point above the center bin
     if (max_energy_bin < end_bin) {
         for (uint16_t b = max_energy_bin + 1; b <= end_bin; b++) {
-            if (fft->_freq_bins[b] < fft->_freq_bins[max_energy_bin] * cutoff) {
+            if (freq_bins[b] < freq_bins[max_energy_bin] * cutoff) {
                 // we assume that the 3dB point is in the middle of the final shoulder bin
                 noise_width_hz += (b - max_energy_bin - 0.5f);
                 peak_top = b;
@@ -141,7 +164,7 @@ float DSP::find_noise_width(FFTWindowState* fft, uint16_t start_bin, uint16_t en
     // -attenuation/2 dB point below the center bin
     if (max_energy_bin > start_bin) {
         for (uint16_t b = max_energy_bin - 1; b >= start_bin; b--) {
-            if (fft->_freq_bins[b] < fft->_freq_bins[max_energy_bin] * cutoff) {
+            if (freq_bins[b] < freq_bins[max_energy_bin] * cutoff) {
                 // we assume that the 3dB point is in the middle of the final shoulder bin
                 noise_width_hz += (max_energy_bin - b - 0.5f);
                 peak_bottom = b;
@@ -149,7 +172,7 @@ float DSP::find_noise_width(FFTWindowState* fft, uint16_t start_bin, uint16_t en
             }
         }
     }
-    noise_width_hz *= fft->_bin_resolution;
+    noise_width_hz *= bin_resolution;
 
     return noise_width_hz;
 }
@@ -164,10 +187,38 @@ uint16_t DSP::step_calc_frequencies(FFTWindowState* fft, uint16_t start_bin, uin
     return fft->_peak_data[CENTER]._bin;
 }
 
+void DSP::update_average_from_sliding_window(FFTWindowState* fft)
+{
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#define ASSERT_MAX(v) assert((v)<(sizeof(float) * fft->_num_stored_freqs * fft->_sliding_window_size))
+#else
+#define ASSERT_MAX(v)
+#endif
+
+    // copy and scale the new slice
+    const uint16_t slice_index = fft->_current_slice * fft->_num_stored_freqs;
+    ASSERT_MAX(slice_index);
+    float* slice = &fft->_sliding_window[slice_index];
+
+    const uint16_t old_slice_index = ((fft->_current_slice + 1) % fft->_sliding_window_size) * fft->_num_stored_freqs;
+    ASSERT_MAX(old_slice_index);
+    float* old_slice = &fft->_sliding_window[old_slice_index];
+
+    const float inv_ssize = 1.0f / fft->_sliding_window_size;
+
+    for (uint16_t i = 0; i < fft->_bin_count; i++) {
+        slice[i] = fft->_freq_bins[i] * fft->_window_scale * inv_ssize;
+        fft->_avg_freq_bins[i] = fft->_avg_freq_bins[i] + slice[i] - old_slice[i];
+    }
+
+    // advance the current slice
+    fft->_current_slice = (fft->_current_slice + 1) % fft->_sliding_window_size;
+}
+
 // calculate a single frequency
 uint16_t DSP::calc_frequency(FFTWindowState* fft, uint16_t start_bin, uint16_t peak_bin, uint16_t end_bin)
 {
-    if (peak_bin == 0 || is_zero(fft->_freq_bins[peak_bin])) {
+    if (peak_bin == 0 || is_zero(fft->get_freq_bin(peak_bin))) {
         return start_bin * fft->_bin_resolution;
     }
 
@@ -176,7 +227,11 @@ uint16_t DSP::calc_frequency(FFTWindowState* fft, uint16_t start_bin, uint16_t p
     // It turns out that Jain is pretty good and works with only magnitudes, but Candan is significantly better
     // if you have access to the complex values and Quinn is a little better still. Quinn is computationally
     // more expensive, but compared to the overall FFT cost seems worth it.
-    return (peak_bin + calculate_quinns_second_estimator(fft, fft->_rfft_data, peak_bin)) * fft->_bin_resolution;
+    if (fft->_sliding_window != nullptr) {
+        return (peak_bin + calculate_jains_estimator(fft, fft->_avg_freq_bins, peak_bin)) * fft->_bin_resolution;
+    } else {
+        return (peak_bin + calculate_quinns_second_estimator(fft, fft->_rfft_data, peak_bin)) * fft->_bin_resolution;
+    }
 }
 
 // Interpolate center frequency using https://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/
@@ -247,7 +302,7 @@ float DSP::calculate_jains_estimator(const FFTWindowState* fft, const float* rea
 bool DSP::fft_init_average(FFTWindowState* fft)
 {
     if (fft->_avg_freq_bins == nullptr) {
-        fft->_avg_freq_bins = (float*)hal.util->malloc_type(sizeof(float) * (fft->_window_size), DSP_MEM_REGION);
+        fft->_avg_freq_bins = (float*)hal.util->malloc_type(sizeof(float) * fft->_num_stored_freqs, DSP_MEM_REGION);
         if (fft->_avg_freq_bins == nullptr) {
             return false;
         }
@@ -294,14 +349,14 @@ uint16_t DSP::fft_stop_average(FFTWindowState* fft, uint16_t start_bin, uint16_t
 
     // find the three highest peaks using a zero crossing algorithm
     // allocate the scratch space locally as we are in a different thread to the regular FFT
-    float* scratch_space = (float*)hal.util->malloc_type(sizeof(float) * (fft->_bin_count + 1), DSP_MEM_REGION);
+    float* scratch_space = (float*)hal.util->malloc_type(sizeof(float) * fft->_num_stored_freqs, DSP_MEM_REGION);
     if (scratch_space == nullptr) {
         return false;
     }
     uint16_t peaks[MAX_TRACKED_PEAKS] {};
     uint16_t numpeaks = find_peaks(&fft->_avg_freq_bins[start_bin], bin_range,
         scratch_space, peaks, MAX_TRACKED_PEAKS, 0.0f, -1.0f, smoothwidth, 2);
-    hal.util->free_type(scratch_space, sizeof(float) * (fft->_bin_count + 1), DSP_MEM_REGION);
+    hal.util->free_type(scratch_space, sizeof(float) * fft->_num_stored_freqs, DSP_MEM_REGION);
 
     // now try and find the lowest harmonic
     for (uint16_t i = 0; i < numpeaks; i++) {

libraries/AP_HAL/DSP.h

@@ -47,14 +47,20 @@ public:
         float _noise_width_hz;
     };
 
+    static const uint8_t MAX_SLIDING_WINDOW_SIZE = 8;
+
     class FFTWindowState {
     public:
         // frequency width of a FFT bin
         const float _bin_resolution;
         // number of FFT bins
         const uint16_t _bin_count;
+        // number of stored frequencies (_bin_count + DC)
+        const uint16_t _num_stored_freqs;
         // size of the FFT window
         const uint16_t _window_size;
+        // size of the FFT sliding window
+        const uint8_t _sliding_window_size;
         // FFT data
         float* _freq_bins;
         // derivative real data scratch space
@@ -63,6 +69,8 @@ public:
         float* _rfft_data;
         // averaged frequency data via Welch's method
         float* _avg_freq_bins;
+        // sliding window of _bin_count frames
+        float* _sliding_window;
         // three highest peaks
         FrequencyPeakData _peak_data[MAX_TRACKED_PEAKS];
         // Hanning window for incoming samples, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
@@ -73,12 +81,17 @@ public:
         bool _averaging;
         // number of samples in the average
         uint32_t _averaging_samples;
+        // current sliding window slice
+        uint8_t _current_slice;
+        // get a frequency bin from an arbitrary slice
+        float get_freq_bin(uint16_t idx) { return _sliding_window == nullptr ? _freq_bins[idx] : _avg_freq_bins[idx]; }
 
+        void free_data_structures();
         virtual ~FFTWindowState();
-        FFTWindowState(uint16_t window_size, uint16_t sample_rate);
+        FFTWindowState(uint16_t window_size, uint16_t sample_rate, uint8_t sliding_window_size);
     };
     // initialise an FFT instance
-    virtual FFTWindowState* fft_init(uint16_t window_size, uint16_t sample_rate) = 0;
+    virtual FFTWindowState* fft_init(uint16_t window_size, uint16_t sample_rate, uint8_t sliding_window_size = 0) = 0;
     // start an FFT analysis with an ObjectBuffer
     virtual void fft_start(FFTWindowState* state, FloatBuffer& samples, uint16_t advance) = 0;
     // perform remaining steps of an FFT analysis
@@ -92,9 +105,12 @@ protected:
     // step 3: find the magnitudes of the complex data
     void step_cmplx_mag(FFTWindowState* fft, uint16_t start_bin, uint16_t end_bin, float noise_att_cutoff);
     // calculate the noise width of a peak based on the input parameters
-    float find_noise_width(FFTWindowState* fft, uint16_t start_bin, uint16_t end_bin, uint16_t max_energy_bin, float cutoff, uint16_t& peak_top, uint16_t& peak_bottom) const;
+    float find_noise_width(float* freq_bins, uint16_t start_bin, uint16_t end_bin, uint16_t max_energy_bin, float cutoff,
+        float bin_resolution, uint16_t& peak_top, uint16_t& peak_bottom) const;
     // step 4: find the bin with the highest energy and interpolate the required frequency
     uint16_t step_calc_frequencies(FFTWindowState* fft, uint16_t start_bin, uint16_t end_bin);
+    // calculate the final average output
+    void update_average_from_sliding_window(FFTWindowState* fft);
     // calculate a single frequency
     uint16_t calc_frequency(FFTWindowState* fft, uint16_t start_bin, uint16_t peak_bin, uint16_t end_bin);
     // find the maximum value in an vector of floats

libraries/AP_HAL/examples/DSP_test/DSP_test.cpp

@@ -40,7 +40,7 @@ public:
 
 class DSPTest : public AP_HAL::DSP {
 public:
-    virtual FFTWindowState* fft_init(uint16_t w, uint16_t sample_rate) override { return nullptr; }
+    virtual FFTWindowState* fft_init(uint16_t w, uint16_t sample_rate, uint8_t sliding_window_size) override { return nullptr; }
     virtual void fft_start(FFTWindowState* state, FloatBuffer& samples, uint16_t advance) override {}
     virtual uint16_t fft_analyse(FFTWindowState* state, uint16_t start_bin, uint16_t end_bin, float noise_att_cutoff) override { return 0; }
 protected: