mirror of https://github.com/ArduPilot/ardupilot
231 lines
9.2 KiB
C++
231 lines
9.2 KiB
C++
/*
|
|
* 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 (fabsf(1.0f - ap) < 0.01f || fabsf(1.0f - am) < 0.01f) {
|
|
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
|