ardupilot/libraries/AP_HAL/DSP.cpp

231 lines
9.2 KiB
C++
Raw Normal View History

/*
* 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