diff --git a/libraries/AP_HAL/DSP.cpp b/libraries/AP_HAL/DSP.cpp index 653afa89bd..ee3ca0e06c 100644 --- a/libraries/AP_HAL/DSP.cpp +++ b/libraries/AP_HAL/DSP.cpp @@ -21,6 +21,9 @@ #ifndef HAL_NO_UARTDRIVER #include #endif +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include +#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++) { diff --git a/libraries/AP_HAL/DSP.h b/libraries/AP_HAL/DSP.h index 3e8cb7b77c..88d306c411 100644 --- a/libraries/AP_HAL/DSP.h +++ b/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 diff --git a/libraries/AP_HAL/examples/DSP_test/DSP_test.cpp b/libraries/AP_HAL/examples/DSP_test/DSP_test.cpp index d00b31eb1a..f599a8c227 100644 --- a/libraries/AP_HAL/examples/DSP_test/DSP_test.cpp +++ b/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: