Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL: save a sliding window array of frequency bins 

calculate frequency average from sliding window
This commit is contained in:
Andy Piper 2022-03-12 14:45:41 +00:00 committed by Peter Hall
parent f244866329
commit d3e57be6e5
3 changed files with 108 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

119
libraries/AP_HAL/DSP.cpp
View File

@ -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,8 +227,12 @@ 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.
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/
float DSP::calculate_quinns_second_estimator(const FFTWindowState* fft, const float* complex_fft, uint16_t k_max) const
@ -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++) {

22
libraries/AP_HAL/DSP.h
View File

@ -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

2
libraries/AP_HAL/examples/DSP_test/DSP_test.cpp
View File

@ -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: