mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-10 01:44:00 -04:00
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
This commit is contained in:
Andy Piper
Andrew Tridgell
parent
3d9776dd6d
commit
f4a99a1589
@ -19,6 +19,7 @@
|
||||
#include "Util.h"
|
||||
#include "OpticalFlow.h"
|
||||
#include "Flash.h"
|
||||
#include "DSP.h"
|
||||
|
||||
#if HAL_WITH_UAVCAN
|
||||
#include "CAN.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
|
||||
|
||||
@ -28,6 +28,7 @@ namespace AP_HAL {
|
||||
class Scheduler;
|
||||
class Semaphore;
|
||||
class OpticalFlow;
|
||||
class DSP;
|
||||
|
||||
class CANProtocol;
|
||||
class CANManager;
|
||||
|
||||
230
libraries/AP_HAL/DSP.cpp
Normal file
230
libraries/AP_HAL/DSP.cpp
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* Code by Andy Piper
|
||||
*/
|
||||
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#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
|
||||
86
libraries/AP_HAL/DSP.h
Normal file
86
libraries/AP_HAL/DSP.h
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* Code by Andy Piper
|
||||
*/
|
||||
/*
|
||||
interface to DSP device
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#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
|
||||
};
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user