Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_HAL/DSP.cpp

498 lines
19 KiB
C++
Raw Permalink Blame 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"
#ifndef HAL_NO_UARTDRIVER
#include <GCS_MAVLink/GCS.h>
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <assert.h>
#endif
#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, uint8_t sliding_window_size) :
_bin_resolution((float)sample_rate / (float)window_size),
_bin_count(window_size / 2),
_num_stored_freqs(window_size / 2 + 1),
_window_size(window_size),
_sliding_window_size(sliding_window_size),
_current_slice(0)
{
// 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);
_derivative_freq_bins = (float*)hal.util->malloc_type(sizeof(float) * _num_stored_freqs, 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);
// sliding window of frequency bin frames
if (_sliding_window_size > 0) {
_sliding_window = (float*)hal.util->malloc_type(sizeof(float) * _num_stored_freqs * _sliding_window_size, DSP_MEM_REGION);
_avg_freq_bins = (float*)hal.util->malloc_type(sizeof(float) * _num_stored_freqs, DSP_MEM_REGION);
// we can still fallback to non-averaging if there is not enough memory
if (_avg_freq_bins == nullptr) {
hal.util->free_type(_sliding_window, sizeof(float) * _num_stored_freqs * _sliding_window_size, DSP_MEM_REGION);
_sliding_window = nullptr;
}
}
if (_freq_bins == nullptr || _hanning_window == nullptr || _rfft_data == nullptr || _derivative_freq_bins == nullptr) {
free_data_structures();
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 - equation 24
_window_scale = 2.0f / sq(_window_scale);
}
DSP::FFTWindowState::~FFTWindowState()
{
free_data_structures();
}
void DSP::FFTWindowState::free_data_structures()
{
hal.util->free_type(_freq_bins, sizeof(float) * _window_size * _sliding_window_size, DSP_MEM_REGION);
_freq_bins = nullptr;
hal.util->free_type(_derivative_freq_bins, sizeof(float) * _num_stored_freqs, DSP_MEM_REGION);
_derivative_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;
hal.util->free_type(_avg_freq_bins, sizeof(float) * _num_stored_freqs, DSP_MEM_REGION);
_avg_freq_bins = nullptr;
hal.util->free_type(_sliding_window, sizeof(float) * _num_stored_freqs * _sliding_window_size, DSP_MEM_REGION);
_sliding_window = 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, float noise_att_cutoff)
{
// fft->_freq_bins is populated with the complex magnitude values of the fft data
// find the maximum power in the range we are interested in
// in order to see a peak in the last bin we need to allow all the way up to the nyquist
const uint16_t smoothwidth = 1;
float* freq_data = fft->_freq_bins;
if (fft->_sliding_window != nullptr) {
update_average_from_sliding_window(fft);
freq_data = fft->_avg_freq_bins;
} else {
// scale the power to account for the input window
vector_scale_float(fft->_freq_bins, fft->_window_scale, fft->_freq_bins, fft->_bin_count);
}
uint16_t bin_range = (MIN(end_bin + ((smoothwidth + 1) >> 1) + 2, fft->_bin_count) - start_bin) + 1;
// find the three highest peaks using a zero crossing algorithm
uint16_t peaks[MAX_TRACKED_PEAKS] {};
memset(fft->_peak_data, 0, sizeof(fft->_peak_data));
uint16_t numpeaks = find_peaks(&freq_data[start_bin], bin_range, fft->_derivative_freq_bins, peaks, MAX_TRACKED_PEAKS, 0.0f, -1.0f, smoothwidth, 2);
//hal.console->printf("found %d peaks\n", numpeaks);
for (uint16_t i = 0; i < MAX_TRACKED_PEAKS; i++) {
fft->_peak_data[i]._bin = peaks[i] + start_bin;
}
uint16_t top = 0, bottom = 0;
fft->_peak_data[CENTER]._noise_width_hz = find_noise_width(freq_data, start_bin, end_bin, fft->_peak_data[CENTER]._bin, noise_att_cutoff, fft->_bin_resolution, top, bottom);
if (numpeaks > 1) {
fft->_peak_data[LOWER_SHOULDER]._noise_width_hz = find_noise_width(freq_data, start_bin, end_bin, fft->_peak_data[LOWER_SHOULDER]._bin, noise_att_cutoff, fft->_bin_resolution, top, bottom);
}
if (numpeaks > 2) {
fft->_peak_data[UPPER_SHOULDER]._noise_width_hz = find_noise_width(freq_data, start_bin, end_bin, fft->_peak_data[UPPER_SHOULDER]._bin, noise_att_cutoff, fft->_bin_resolution, top, bottom);
}
// average the FFT data
if (fft->_averaging) {
vector_add_float(fft->_avg_freq_bins, fft->_freq_bins, fft->_avg_freq_bins, fft->_bin_count);
fft->_averaging_samples++;
}
}
// calculate the noise width of a peak based on the input parameters
// freq_bins can be scaled or unscaled for power
float DSP::find_noise_width(float* freq_bins, uint16_t start_bin, uint16_t end_bin, uint16_t max_energy_bin, float cutoff, float bin_resolution, uint16_t& peak_top, uint16_t& peak_bottom) const
{
// max_energy_bin is guaranteed to be between start_bin and end_bin
peak_top = end_bin;
peak_bottom = start_bin;
// calculate the width of the peak
float noise_width_hz = 1;
// -attenuation/2 dB point above the center bin
if (max_energy_bin < end_bin) {
for (uint16_t b = max_energy_bin + 1; b <= end_bin; b++) {
if (freq_bins[b] < 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
if (max_energy_bin > start_bin) {
for (uint16_t b = max_energy_bin - 1; b >= start_bin; b--) {
if (freq_bins[b] < 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 *= 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)
{
fft->_peak_data[CENTER]._freq_hz = calc_frequency(fft, start_bin, fft->_peak_data[CENTER]._bin, end_bin);
fft->_peak_data[UPPER_SHOULDER]._freq_hz = calc_frequency(fft, start_bin, fft->_peak_data[UPPER_SHOULDER]._bin, end_bin);
fft->_peak_data[LOWER_SHOULDER]._freq_hz = calc_frequency(fft, start_bin, fft->_peak_data[LOWER_SHOULDER]._bin, end_bin);
return fft->_peak_data[CENTER]._bin;
}
void DSP::update_average_from_sliding_window(FFTWindowState* fft)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#define ASSERT_MAX(v) assert((v)<(fft->_num_stored_freqs * fft->_sliding_window_size))
#else
#define ASSERT_MAX(v)
#endif
// copy and scale the new slice
const uint16_t slice_index = fft->_current_slice * fft->_num_stored_freqs;
ASSERT_MAX(slice_index);
float* slice = &fft->_sliding_window[slice_index];
const uint16_t old_slice_index = ((fft->_current_slice + 1) % fft->_sliding_window_size) * fft->_num_stored_freqs;
ASSERT_MAX(old_slice_index);
float* old_slice = &fft->_sliding_window[old_slice_index];
const float inv_ssize = 1.0f / fft->_sliding_window_size;
for (uint16_t i = 0; i < fft->_bin_count; i++) {
slice[i] = fft->_freq_bins[i] * fft->_window_scale * inv_ssize;
fft->_avg_freq_bins[i] = fft->_avg_freq_bins[i] + slice[i] - old_slice[i];
}
// advance the current slice
fft->_current_slice = (fft->_current_slice + 1) % fft->_sliding_window_size;
}
// calculate a single frequency
uint16_t DSP::calc_frequency(FFTWindowState* fft, uint16_t start_bin, uint16_t peak_bin, uint16_t end_bin)
{
if (peak_bin == 0 || is_zero(fft->get_freq_bin(peak_bin))) {
return start_bin * fft->_bin_resolution;
}
peak_bin = constrain_int16(peak_bin, start_bin, end_bin);
// 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.
if (fft->_sliding_window != nullptr) {
return (peak_bin + calculate_jains_estimator(fft, fft->_avg_freq_bins, peak_bin)) * fft->_bin_resolution;
} else {
return (peak_bin + calculate_quinns_second_estimator(fft, fft->_rfft_data, peak_bin)) * fft->_bin_resolution;
}
}
// 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);
}
// from https://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/
// Works on magnitudes only, which is useful when using averaged data
float DSP::calculate_jains_estimator(const FFTWindowState* fft, const float* real_fft, uint16_t k_max)
{
if (k_max <= 1 || k_max >= fft->_bin_count) {
return 0.0f;
}
float y1 = real_fft[k_max-1];
float y2 = real_fft[k_max];
float y3 = real_fft[k_max+1];
if (is_zero(y2) || is_zero(y1)) {
return 0.0f;
}
float d = 0.0f;
if (y1 > y3) {
float a = y2 / y1;
d = a / (1 + a) - 1;
} else {
float a = y3 / y2;
d = a / (1 + a);
}
return constrain_float(d, -0.5f, 0.5f);
}
// initialize averaging FFT windows as they are calculated
bool DSP::fft_init_average(FFTWindowState* fft)
{
if (fft->_avg_freq_bins == nullptr) {
fft->_avg_freq_bins = (float*)hal.util->malloc_type(sizeof(float) * fft->_num_stored_freqs, DSP_MEM_REGION);
if (fft->_avg_freq_bins == nullptr) {
return false;
}
}
return true;
}
// start averaging FFT windows as they are calculated
bool DSP::fft_start_average(FFTWindowState* fft)
{
if (fft->_averaging) {
return false;
}
if (!fft_init_average(fft)) {
return false;
}
fft->_averaging_samples = 0;
fft->_averaging = true;
return true;
}
// start averaging FFT windows as they are calculated
uint16_t DSP::fft_stop_average(FFTWindowState* fft, uint16_t start_bin, uint16_t end_bin, float* freqs)
{
// ensure the window has been allocated even if we do nothing else
if (!fft_init_average(fft)) {
return 0;
}
if (!fft->_averaging) {
return 0;
}
fft->_averaging = false;
// scale by the number of samples
vector_scale_float(fft->_avg_freq_bins, fft->_averaging_samples, fft->_avg_freq_bins, fft->_bin_count);
const uint16_t smoothwidth = 1;
uint16_t bin_range = (MIN(end_bin + ((smoothwidth + 1) >> 1) + 2, fft->_bin_count) - start_bin) + 1;
// find the three highest peaks using a zero crossing algorithm
// allocate the scratch space locally as we are in a different thread to the regular FFT
float* scratch_space = (float*)hal.util->malloc_type(sizeof(float) * fft->_num_stored_freqs, DSP_MEM_REGION);
if (scratch_space == nullptr) {
return false;
}
uint16_t peaks[MAX_TRACKED_PEAKS] {};
uint16_t numpeaks = find_peaks(&fft->_avg_freq_bins[start_bin], bin_range,
scratch_space, peaks, MAX_TRACKED_PEAKS, 0.0f, -1.0f, smoothwidth, 2);
hal.util->free_type(scratch_space, sizeof(float) * fft->_num_stored_freqs, DSP_MEM_REGION);
numpeaks = MIN(numpeaks, uint16_t(MAX_TRACKED_PEAKS));
// now try and find the lowest harmonic
for (uint16_t i = 0; i < numpeaks; i++) {
const uint16_t bin = peaks[i] + start_bin;
float d = calculate_jains_estimator(fft, fft->_avg_freq_bins, bin);
freqs[i] = (bin + d) * fft->_bin_resolution;
}
fft->_averaging_samples = 0;
return numpeaks;
}
// find all the peaks in the fft window using https://terpconnect.umd.edu/~toh/spectrum/PeakFindingandMeasurement.htm
// in general peakgrup > 2 is only good for very broad noisy peaks, <= 2 better for spikey peaks, although 1 will miss
// a true spike 50% of the time
uint16_t DSP::find_peaks(const float* input, uint16_t length, float* d, uint16_t* peaks, uint16_t peaklen,
float slopeThreshold, float ampThreshold, uint16_t smoothwidth, uint16_t peakgroup) const
{
if (smoothwidth > 1) {
derivative(input, d, length);
fastsmooth(d, length, smoothwidth);
} else {
derivative(input, d, length);
}
uint16_t n = ((peakgroup + 1) >> 1) + 1;
uint16_t halfw = (smoothwidth + 1) >> 1;
uint16_t numpeaks = 0;
uint16_t peakX = 0;
float peakY = 0;
uint16_t pindex;
uint16_t xx[peakgroup];
float yy[peakgroup];
memset(xx, 0, peakgroup * sizeof(uint16_t));
memset(yy, 0, peakgroup * sizeof(float));
for (uint16_t j = (halfw << 1) - 2; j < length - smoothwidth - 1; j++) {
if (d[j] >= 0 && d[j + 1] <= 0 && !is_equal(d[j], d[j + 1])) { // detect zero crossing
if ((d[j] - d[j + 1]) > slopeThreshold) {
for (uint16_t k = 0; k < peakgroup; k++) {
uint16_t groupIndex = j + k - n + 2;
groupIndex = constrain_int16(groupIndex, 0, length - 1);
xx[k] = groupIndex;
yy[k] = input[groupIndex];
}
if (peakgroup < 3) {
vector_max_float(yy, peakgroup, &peakY, &pindex);
} else {
peakY = vector_mean_float(yy, peakgroup);
pindex = val2index(yy, peakgroup, peakY);
}
peakX = xx[pindex];
//hal.console->printf("zero %d, gindex %d -> %d, index %d, val %f\n", j, j -n +2, j+peakgroup -1 - n +2, peakX, peakY);
// see if we have a valid peak
if (isfinite(peakY) && peakY >= ampThreshold) {
// record in amplitude order
for (int16_t i = 0; i < peaklen; i++) {
if (i >= numpeaks) {
peaks[i] = peakX;
break;
}
if (peakY > input[peaks[i]]) {
for (int16_t a = peaklen - 1; a > i; a--) {
peaks[a] = peaks[a - 1];
}
peaks[i] = peakX;
break;
}
}
numpeaks++;
}
}
}
}
return numpeaks;
}
// Returns the index and the value of the element of a vector that is closest to val
uint16_t DSP::val2index(const float* vector, uint16_t n, float val) const
{
float minval = FLT_MAX;
uint16_t minidx = 0;
for (uint16_t i = 0; i < n; i++) {
float dif = fabsf(vector[i] - val);
if (dif < minval) {
minval = dif;
minidx = i;
}
}
return minidx;
}
// First derivative of vector using 2-point central difference.
void DSP::derivative(const float* input, float* output, uint16_t n) const
{
output[0] = input[1] - input[0];
output[n - 1] = input[n - 1] - input[n - 2];
for (uint16_t i = 1; i < n - 1; i++) {
output[i] = (input[i + 1] - input[i - 1]) / 2.0f;
}
}
// smooth a vector in-place
void DSP::fastsmooth(float* input, uint16_t n, uint16_t smoothwidth) const
{
float window[smoothwidth];
memset(window, 0, smoothwidth * sizeof(float));
float sumpoints = 0.0f;
for (int i = 0; i < smoothwidth; i++) {
sumpoints += input[i];
}
uint16_t halfw = (smoothwidth + 1) >> 1;
for (int i = 0; i < n - smoothwidth; i++) {
window[i % smoothwidth] = sumpoints;
sumpoints -= input[i];
sumpoints += input[i + smoothwidth];
input[i] = window[(i + smoothwidth - 1) % smoothwidth] / smoothwidth;
}
uint16_t last = n - smoothwidth + halfw;
input[last] = 0.0f;
for (int i = last + 1; i < n; i++) {
input[last] += input[i];
}
input[n - smoothwidth + halfw] /= smoothwidth;
for (int i = last + 1; i < n; i++) {
input[i] = 0.0f;
}
}
#endif // HAL_WITH_DSP
Reference in New Issue View Git Blame Copy Permalink