From f4a99a1589681b9ff1df10340d148ef6845ac702 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 9 Aug 2019 17:04:00 +0100 Subject: [PATCH] AP_HAL: hardware abstraction for FFT. control inclusion of FFT based on HAL_WITH_DSP and HAL_GYROFFT_ENABLED. target appropriate ARM cpus define hanning window and quinn's estimator start/analyse version of FFT to support threading allocate memory in a specific region calculate frequency and noise bandwidth of two noisiest peaks control inclusion of DSP based on board size --- libraries/AP_HAL/AP_HAL.h | 1 + libraries/AP_HAL/AP_HAL_Boards.h | 12 ++ libraries/AP_HAL/AP_HAL_Namespace.h | 1 + libraries/AP_HAL/DSP.cpp | 230 ++++++++++++++++++++++++++++ libraries/AP_HAL/DSP.h | 86 +++++++++++ libraries/AP_HAL/HAL.h | 10 +- 6 files changed, 337 insertions(+), 3 deletions(-) create mode 100644 libraries/AP_HAL/DSP.cpp create mode 100644 libraries/AP_HAL/DSP.h diff --git a/libraries/AP_HAL/AP_HAL.h b/libraries/AP_HAL/AP_HAL.h index 4c22f4fbdd..90bf55a020 100644 --- a/libraries/AP_HAL/AP_HAL.h +++ b/libraries/AP_HAL/AP_HAL.h @@ -19,6 +19,7 @@ #include "Util.h" #include "OpticalFlow.h" #include "Flash.h" +#include "DSP.h" #if HAL_WITH_UAVCAN #include "CAN.h" diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 6e844bc44b..934853dbfb 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -185,6 +185,18 @@ #define HAL_MINIMIZE_FEATURES 0 #endif +#ifndef BOARD_FLASH_SIZE +#define BOARD_FLASH_SIZE 2048 +#endif + +#ifndef HAL_WITH_DSP +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || defined(HAL_BOOTLOADER_BUILD) || defined(HAL_BUILD_AP_PERIPH) || BOARD_FLASH_SIZE <= 1024 +#define HAL_WITH_DSP 0 +#else +#define HAL_WITH_DSP !HAL_MINIMIZE_FEATURES +#endif +#endif + #ifndef HAL_OS_FATFS_IO #define HAL_OS_FATFS_IO 0 #endif diff --git a/libraries/AP_HAL/AP_HAL_Namespace.h b/libraries/AP_HAL/AP_HAL_Namespace.h index 5633d3c902..5770d84634 100644 --- a/libraries/AP_HAL/AP_HAL_Namespace.h +++ b/libraries/AP_HAL/AP_HAL_Namespace.h @@ -28,6 +28,7 @@ namespace AP_HAL { class Scheduler; class Semaphore; class OpticalFlow; + class DSP; class CANProtocol; class CANManager; diff --git a/libraries/AP_HAL/DSP.cpp b/libraries/AP_HAL/DSP.cpp new file mode 100644 index 0000000000..c695c050f2 --- /dev/null +++ b/libraries/AP_HAL/DSP.cpp @@ -0,0 +1,230 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andy Piper + */ + +#include +#include "AP_HAL.h" +#include "DSP.h" + +#if HAL_WITH_DSP + +using namespace AP_HAL; + +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) + : _window_size(window_size), + _bin_count(window_size / 2), + _bin_resolution((float)sample_rate / (float)window_size) +{ + // 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); + _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); + + if (_freq_bins == nullptr || _hanning_window == nullptr || _rfft_data == nullptr) { + hal.util->free_type(_freq_bins, sizeof(float) * (_window_size), 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; + _hanning_window = nullptr; + _rfft_data = nullptr; + return; + } + + // create the Hanning window + // https://holometer.fnal.gov/GH_FFT.pdf - equation 19 + for (uint16_t i = 0; i < window_size; i++) { + _hanning_window[i] = (0.5f - 0.5f * cosf(2.0f * M_PI * i / ((float)window_size - 1))); + _window_scale += _hanning_window[i]; + } + // Calculate the inverse of the Effective Noise Bandwidth + _window_scale = 2.0f / sq(_window_scale); +} + +DSP::FFTWindowState::~FFTWindowState() +{ + hal.util->free_type(_freq_bins, sizeof(float) * (_window_size), DSP_MEM_REGION); + _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; +} + +// step 3: find the magnitudes of the complex data +void DSP::step_cmplx_mag(FFTWindowState* fft, uint16_t start_bin, uint16_t end_bin, uint8_t harmonics, float noise_att_cutoff) +{ + // find the maximum power in the range we are interested in + float max_value = 0, max_value2 = 0, max_value3 = 0; + uint16_t max_bin2 = 0, max_bin3 = 0; + uint16_t bin_range = (end_bin - start_bin) + 1; + // calculate highest two peaks in the range of interest. we cannot simply find the + // maximum in two halves since the primary peak could extend over multiple bins + // instead move outwards looking for the 3dB points and then search from there + + // first find the highest peak + vector_max_float(&fft->_freq_bins[start_bin], bin_range, &max_value, &fft->_max_energy_bin); + fft->_max_energy_bin += start_bin; + + // calculate the width of the peak + uint16_t top = 0, bottom = 0; + fft->_max_noise_width_hz = find_noise_width(fft, start_bin, end_bin, fft->_max_energy_bin, noise_att_cutoff, top, bottom); + + // if requested calculate another harmonic + if (harmonics > 1) { + // search for peaks above the 3db point + if (top < end_bin) { + vector_max_float(&fft->_freq_bins[top], end_bin - top + 1, &max_value2, &max_bin2); + } + max_bin2 += top; + // search for peaks below the 3db point + if (bottom > start_bin) { + vector_max_float(&fft->_freq_bins[start_bin], bottom - start_bin + 1, &max_value3, &max_bin3); + } + max_bin3 += start_bin; + + // pick the highest + if (fft->_freq_bins[max_bin2] > fft->_freq_bins[max_bin3]) { + fft->_second_energy_bin = max_bin2; + // calculate the noise width of the second bin + fft->_second_noise_width_hz = find_noise_width(fft, top, end_bin, fft->_second_energy_bin, noise_att_cutoff, top, bottom); + } else { + fft->_second_energy_bin = max_bin3; + // calculate the noise width of the second bin + fft->_second_noise_width_hz = find_noise_width(fft, start_bin, bottom, fft->_second_energy_bin, noise_att_cutoff, top, bottom); + } + } else { + fft->_second_energy_bin = 0; + fft->_second_noise_width_hz = 0; + } + + // scale the power to account for the input window + vector_scale_float(fft->_freq_bins, fft->_window_scale, fft->_freq_bins, fft->_bin_count); +} + +// 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 +{ + peak_top = peak_bottom = start_bin; + + if (max_energy_bin == 0) { + return fft->_bin_resolution; + } + + if (max_energy_bin == fft->_bin_count) { + peak_top = peak_bottom = fft->_bin_count; + return fft->_bin_resolution; + } + + // calculate the width of the peak + float noise_width_hz = 1; + + // -attenuation/2 dB point above the center 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) { + // 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; + break; + } + } + // -attenuation/2 dB point below the center 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) { + // 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; + break; + } + } + noise_width_hz *= fft->_bin_resolution; + + return noise_width_hz; +} + +// step 4: find the bin with the highest energy and interpolate the required frequency +uint16_t DSP::step_calc_frequencies(FFTWindowState* fft, uint16_t start_bin, uint16_t end_bin) +{ + if (is_zero(fft->_freq_bins[fft->_max_energy_bin])) { + fft->_max_bin_freq = start_bin * fft->_bin_resolution; + } else { + // 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. + fft->_max_bin_freq = (fft->_max_energy_bin + calculate_quinns_second_estimator(fft, fft->_rfft_data, fft->_max_energy_bin)) * fft->_bin_resolution; + } + + // calculate second frequency as required + if (fft->_second_energy_bin > 0) { + // find second highest bin frequency + if (is_zero(fft->_freq_bins[fft->_second_energy_bin])) { + fft->_second_bin_freq = start_bin * fft->_bin_resolution; + } else { + fft->_second_bin_freq = (fft->_second_energy_bin + calculate_quinns_second_estimator(fft, fft->_rfft_data, fft->_second_energy_bin)) * fft->_bin_resolution; + } + } + + return fft->_max_energy_bin; +} + +// 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 +{ + if (k_max <= 1 || k_max >= fft->_bin_count) { + return 0.0f; + } + + const uint16_t k_m1 = (k_max - 1) * 2; + const uint16_t k_p1 = (k_max + 1) * 2; + const uint16_t k = k_max * 2; + + const float divider = complex_fft[k] * complex_fft[k] + complex_fft[k+1] * complex_fft[k+1]; + const float ap = (complex_fft[k_p1] * complex_fft[k] + complex_fft[k_p1 + 1] * complex_fft[k+1]) / divider; + const float am = (complex_fft[k_m1] * complex_fft[k] + complex_fft[k_m1 + 1] * complex_fft[k + 1]) / divider; + + // sanity check + if (is_zero(ap) || is_zero(am)) { + return 0.0f; + } + + const float dp = -ap / (1.0f - ap); + const float dm = am / (1.0f - am); + + float d = (dp + dm) * 0.5f + tau(dp * dp) - tau(dm * dm); + + // -0.5 < d < 0.5 which is the fraction of the sample spacing about the center element + return constrain_float(d, -0.5f, 0.5f); +} + +static const float TAU_FACTOR = SQRT_6 / 24.0f; + +// Helper function used for Quinn's frequency estimation +float DSP::tau(const float x) const +{ + float p1 = logf(3.0f * sq(x) + 6.0f * x + 1.0f); + float part1 = x + 1.0f - SQRT_2_3; + float part2 = x + 1.0f + SQRT_2_3; + float p2 = logf(part1 / part2); + return (0.25f * p1 - TAU_FACTOR * p2); +} + +#endif // HAL_WITH_DSP diff --git a/libraries/AP_HAL/DSP.h b/libraries/AP_HAL/DSP.h new file mode 100644 index 0000000000..4c79aa6c1d --- /dev/null +++ b/libraries/AP_HAL/DSP.h @@ -0,0 +1,86 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andy Piper + */ +/* + interface to DSP device + */ +#pragma once + +#include +#include "AP_HAL_Namespace.h" + +#define DSP_MEM_REGION AP_HAL::Util::MEM_FAST + +class AP_HAL::DSP { +#if HAL_WITH_DSP +public: + typedef float* FFTSampleWindow; + class FFTWindowState { + public: + // frequency width of a FFT bin + const float _bin_resolution; + // number of FFT bins + const uint16_t _bin_count; + // size of the FFT window + const uint16_t _window_size; + // FFT data + float* _freq_bins; + // intermediate real FFT data + float* _rfft_data; + // estimate of FFT peak frequency + float _max_bin_freq; + // bin with maximum energy + uint16_t _max_energy_bin; + // width of the max energy peak + float _max_noise_width_hz; + // estimate of FFT second peak frequency + float _second_bin_freq; + // bin with second-most energy + uint16_t _second_energy_bin; + // width of the second energy peak + float _second_noise_width_hz; + // Hanning window for incoming samples, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window + float* _hanning_window; + // Use in calculating the PS of the signal [Heinz] equations (20) & (21) + float _window_scale; + + virtual ~FFTWindowState(); + FFTWindowState(uint16_t window_size, uint16_t sample_rate); + }; + // initialise an FFT instance + virtual FFTWindowState* fft_init(uint16_t window_size, uint16_t sample_rate) = 0; + // start an FFT analysis + virtual void fft_start(FFTWindowState* state, const float* samples, uint16_t buffer_index, uint16_t buffer_size) = 0; + // 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) = 0; + +protected: + // step 3: find the magnitudes of the complex data + void step_cmplx_mag(FFTWindowState* fft, uint16_t start_bin, uint16_t end_bin, uint8_t harmonics, 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; + // 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); + // find the maximum value in an vector of floats + virtual void vector_max_float(const float* vin, uint16_t len, float* max_value, uint16_t* max_index) const = 0; + // multiple an vector of floats by a scale factor + virtual void vector_scale_float(const float* vin, float scale, float* vout, uint16_t len) const = 0; + + // quinn's frequency interpolator + float calculate_quinns_second_estimator(const FFTWindowState* fft, const float* complex_fft, uint16_t k) const; + float tau(const float x) const; +#endif // HAL_WITH_DSP +}; diff --git a/libraries/AP_HAL/HAL.h b/libraries/AP_HAL/HAL.h index f77984f230..55d3c6e11a 100644 --- a/libraries/AP_HAL/HAL.h +++ b/libraries/AP_HAL/HAL.h @@ -13,6 +13,7 @@ class AP_Param; #include "UARTDriver.h" #include "system.h" #include "OpticalFlow.h" +#include "DSP.h" #if HAL_WITH_UAVCAN #include "CAN.h" #endif @@ -38,8 +39,9 @@ public: AP_HAL::RCOutput* _rcout, AP_HAL::Scheduler* _scheduler, AP_HAL::Util* _util, - AP_HAL::OpticalFlow *_opticalflow, - AP_HAL::Flash *_flash, + AP_HAL::OpticalFlow*_opticalflow, + AP_HAL::Flash* _flash, + AP_HAL::DSP* _dsp, #if HAL_WITH_UAVCAN AP_HAL::CANManager* _can_mgr[MAX_NUMBER_OF_CAN_DRIVERS]) #else @@ -65,7 +67,8 @@ public: scheduler(_scheduler), util(_util), opticalflow(_opticalflow), - flash(_flash) + flash(_flash), + dsp(_dsp) { #if HAL_WITH_UAVCAN if (_can_mgr == nullptr) { @@ -118,6 +121,7 @@ public: AP_HAL::Util *util; AP_HAL::OpticalFlow *opticalflow; AP_HAL::Flash *flash; + AP_HAL::DSP *dsp; #if HAL_WITH_UAVCAN AP_HAL::CANManager* can_mgr[MAX_NUMBER_OF_CAN_DRIVERS]; #else