From ee87ef7013847bf7fb835f777d89d206d9f41719 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 5 Mar 2020 21:54:00 +0000 Subject: [PATCH] AP_HAL_SITL: make harmonics part of DSP add vector mean function to dsp allow fft_start() to use ObjectBuffer for lock-free access --- libraries/AP_HAL_SITL/DSP.cpp | 44 +++++++++++++++++++++-------------- libraries/AP_HAL_SITL/DSP.h | 17 +++++++------- 2 files changed, 36 insertions(+), 25 deletions(-) diff --git a/libraries/AP_HAL_SITL/DSP.cpp b/libraries/AP_HAL_SITL/DSP.cpp index 6974af108d..63e59028e6 100644 --- a/libraries/AP_HAL_SITL/DSP.cpp +++ b/libraries/AP_HAL_SITL/DSP.cpp @@ -22,6 +22,7 @@ #include #include "DSP.h" #include +#include using namespace HALSITL; @@ -34,10 +35,10 @@ extern const AP_HAL::HAL& hal; // important as frequency resolution. Referred to as [Heinz] throughout the code. // initialize the FFT state machine -AP_HAL::DSP::FFTWindowState* DSP::fft_init(uint16_t window_size, uint16_t sample_rate) +AP_HAL::DSP::FFTWindowState* DSP::fft_init(uint16_t window_size, uint16_t sample_rate, uint8_t harmonics) { - DSP::FFTWindowStateSITL* fft = new DSP::FFTWindowStateSITL(window_size, sample_rate); - if (fft->_hanning_window == nullptr || fft->_rfft_data == nullptr || fft->_freq_bins == nullptr) { + DSP::FFTWindowStateSITL* fft = new DSP::FFTWindowStateSITL(window_size, sample_rate, harmonics); + if (fft == nullptr || fft->_hanning_window == nullptr || fft->_rfft_data == nullptr || fft->_freq_bins == nullptr || fft->_derivative_freq_bins == nullptr) { delete fft; return nullptr; } @@ -45,26 +46,26 @@ AP_HAL::DSP::FFTWindowState* DSP::fft_init(uint16_t window_size, uint16_t sample } // start an FFT analysis -void DSP::fft_start(AP_HAL::DSP::FFTWindowState* state, const float* samples, uint16_t buffer_index, uint16_t buffer_size) +void DSP::fft_start(AP_HAL::DSP::FFTWindowState* state, FloatBuffer& samples, uint16_t advance) { - step_hanning((FFTWindowStateSITL*)state, samples, buffer_index, buffer_size); + step_hanning((FFTWindowStateSITL*)state, samples, advance); } // perform remaining steps of an FFT analysis -uint16_t DSP::fft_analyse(AP_HAL::DSP::FFTWindowState* state, uint16_t start_bin, uint16_t end_bin, uint8_t harmonics, float noise_att_cutoff) +uint16_t DSP::fft_analyse(AP_HAL::DSP::FFTWindowState* state, uint16_t start_bin, uint16_t end_bin, float noise_att_cutoff) { FFTWindowStateSITL* fft = (FFTWindowStateSITL*)state; step_fft(fft); - step_cmplx_mag(fft, start_bin, end_bin, harmonics, noise_att_cutoff); + step_cmplx_mag(fft, start_bin, end_bin, noise_att_cutoff); return step_calc_frequencies(fft, start_bin, end_bin); } // create an instance of the FFT state machine -DSP::FFTWindowStateSITL::FFTWindowStateSITL(uint16_t window_size, uint16_t sample_rate) - : AP_HAL::DSP::FFTWindowState::FFTWindowState(window_size, sample_rate) +DSP::FFTWindowStateSITL::FFTWindowStateSITL(uint16_t window_size, uint16_t sample_rate, uint8_t harmonics) + : AP_HAL::DSP::FFTWindowState::FFTWindowState(window_size, sample_rate, harmonics) { - if (_freq_bins == nullptr || _hanning_window == nullptr || _rfft_data == nullptr) { - gcs().send_text(MAV_SEVERITY_WARNING, "Failed to allocate window for DSP"); + if (_freq_bins == nullptr || _hanning_window == nullptr || _rfft_data == nullptr || _derivative_freq_bins == nullptr) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to allocate window for DSP"); return; } @@ -77,16 +78,15 @@ DSP::FFTWindowStateSITL::~FFTWindowStateSITL() } // step 1: filter the incoming samples through a Hanning window -void DSP::step_hanning(FFTWindowStateSITL* fft, const float* samples, uint16_t buffer_index, uint16_t buffer_size) +void DSP::step_hanning(FFTWindowStateSITL* fft, FloatBuffer& samples, uint16_t advance) { // 5us // apply hanning window to gyro samples and store result in _freq_bins // hanning starts and ends with 0, could be skipped for minor speed improvement - const uint16_t ring_buf_idx = MIN(buffer_size - buffer_index, fft->_window_size); - mult_f32(&samples[buffer_index], &fft->_hanning_window[0], &fft->_freq_bins[0], ring_buf_idx); - if (buffer_index > 0) { - mult_f32(&samples[0], &fft->_hanning_window[ring_buf_idx], &fft->_freq_bins[ring_buf_idx], fft->_window_size - ring_buf_idx); - } + uint32_t read_window = samples.peek(&fft->_freq_bins[0], fft->_window_size); + assert(read_window == fft->_window_size); + samples.advance(advance); + mult_f32(&fft->_freq_bins[0], &fft->_hanning_window[0], &fft->_freq_bins[0], fft->_window_size); } // step 2: performm an in-place FFT on the windowed data @@ -135,6 +135,16 @@ void DSP::vector_scale_float(const float* vin, float scale, float* vout, uint16_ } } +float DSP::vector_mean_float(const float* vin, uint16_t len) const +{ + float mean_value = 0.0f; + for (uint16_t i = 0; i < len; i++) { + mean_value += vin[i]; + } + mean_value /= len; + return mean_value; +} + // simple integer log2 static uint16_t fft_log2(uint16_t n) { diff --git a/libraries/AP_HAL_SITL/DSP.h b/libraries/AP_HAL_SITL/DSP.h index dab8552d49..76173a84b2 100644 --- a/libraries/AP_HAL_SITL/DSP.h +++ b/libraries/AP_HAL_SITL/DSP.h @@ -27,29 +27,30 @@ typedef std::complex complexf; class HALSITL::DSP : public AP_HAL::DSP { public: // initialise an FFT instance - virtual FFTWindowState* fft_init(uint16_t window_size, uint16_t sample_rate) override; - // start an FFT analysis - virtual void fft_start(FFTWindowState* state, const float* samples, uint16_t buffer_index, uint16_t buffer_size) override; + virtual FFTWindowState* fft_init(uint16_t window_size, uint16_t sample_rate, uint8_t harmonics) override; + // start an FFT analysis with an ObjectBuffer + virtual void fft_start(FFTWindowState* state, FloatBuffer& samples, uint16_t advance) override; // perform remaining steps of an FFT analysis - virtual uint16_t fft_analyse(FFTWindowState* state, uint16_t start_bin, uint16_t end_bin, uint8_t harmonics, float noise_att_cutoff) override; + virtual uint16_t fft_analyse(FFTWindowState* state, uint16_t start_bin, uint16_t end_bin, float noise_att_cutoff) override; // STM32-based FFT state class FFTWindowStateSITL : public AP_HAL::DSP::FFTWindowState { friend class HALSITL::DSP; - protected: - FFTWindowStateSITL(uint16_t window_size, uint16_t sample_rate); - ~FFTWindowStateSITL(); + public: + FFTWindowStateSITL(uint16_t window_size, uint16_t sample_rate, uint8_t harmonics); + virtual ~FFTWindowStateSITL(); private: complexf* buf; }; private: - void step_hanning(FFTWindowStateSITL* fft, const float* samples, uint16_t buffer_index, uint16_t buffer_size); + void step_hanning(FFTWindowStateSITL* fft, FloatBuffer& samples, uint16_t advance); void step_fft(FFTWindowStateSITL* fft); void mult_f32(const float* v1, const float* v2, float* vout, uint16_t len); void vector_max_float(const float* vin, uint16_t len, float* maxValue, uint16_t* maxIndex) const override; void vector_scale_float(const float* vin, float scale, float* vout, uint16_t len) const override; + float vector_mean_float(const float* vin, uint16_t len) const override; void calculate_fft(complexf* f, uint16_t length); };