mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_HAL: collect data for three largest peaks
new dsp peak detection algorithm add DSP sketch with frequency ascii art tool to generate gyro data frames from batch sampled DF logs add generated data from real Y6B flight allow fft_start() to use ObjectBuffer<float> for lock-free access allow ObjectBuffer to be resized
This commit is contained in:
Andy Piper
Andrew Tridgell
parent
605672b631
commit
e2ef0bd36e
@ -28,23 +28,27 @@ 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)
|
||||
DSP::FFTWindowState::FFTWindowState(uint16_t window_size, uint16_t sample_rate, uint8_t harmonics)
|
||||
: _window_size(window_size),
|
||||
_bin_count(window_size / 2),
|
||||
_bin_resolution((float)sample_rate / (float)window_size)
|
||||
_bin_resolution((float)sample_rate / (float)window_size),
|
||||
_harmonics(harmonics)
|
||||
{
|
||||
// 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) * (_bin_count + 1), 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) {
|
||||
if (_freq_bins == nullptr || _hanning_window == nullptr || _rfft_data == nullptr || _derivative_freq_bins == nullptr) {
|
||||
hal.util->free_type(_freq_bins, sizeof(float) * (_window_size), DSP_MEM_REGION);
|
||||
hal.util->free_type(_derivative_freq_bins, sizeof(float) * (_bin_count), 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;
|
||||
_derivative_freq_bins = nullptr;
|
||||
_hanning_window = nullptr;
|
||||
_rfft_data = nullptr;
|
||||
return;
|
||||
@ -64,6 +68,8 @@ DSP::FFTWindowState::~FFTWindowState()
|
||||
{
|
||||
hal.util->free_type(_freq_bins, sizeof(float) * (_window_size), DSP_MEM_REGION);
|
||||
_freq_bins = nullptr;
|
||||
hal.util->free_type(_derivative_freq_bins, sizeof(float) * (_bin_count), 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);
|
||||
@ -71,50 +77,30 @@ DSP::FFTWindowState::~FFTWindowState()
|
||||
}
|
||||
|
||||
// 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)
|
||||
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
|
||||
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
|
||||
// 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;
|
||||
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(&fft->_freq_bins[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);
|
||||
|
||||
// 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;
|
||||
for (uint16_t i = 0; i < MAX_TRACKED_PEAKS; i++) {
|
||||
fft->_peak_data[i]._bin = peaks[i] + 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);
|
||||
fft->_peak_data[CENTER]._noise_width_hz = find_noise_width(fft, start_bin, end_bin, fft->_peak_data[CENTER]._bin, noise_att_cutoff, top, bottom);
|
||||
if (numpeaks > 1) {
|
||||
fft->_peak_data[LOWER_SHOULDER]._noise_width_hz = find_noise_width(fft, start_bin, end_bin, fft->_peak_data[LOWER_SHOULDER]._bin, noise_att_cutoff, top, bottom);
|
||||
}
|
||||
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;
|
||||
if (numpeaks > 2) {
|
||||
fft->_peak_data[UPPER_SHOULDER]._noise_width_hz = find_noise_width(fft, start_bin, end_bin, fft->_peak_data[UPPER_SHOULDER]._bin, noise_att_cutoff, top, bottom);
|
||||
}
|
||||
|
||||
// scale the power to account for the input window
|
||||
@ -124,21 +110,15 @@ void DSP::step_cmplx_mag(FFTWindowState* fft, uint16_t start_bin, uint16_t end_b
|
||||
// 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;
|
||||
}
|
||||
// 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 (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
|
||||
@ -147,7 +127,9 @@ float DSP::find_noise_width(FFTWindowState* fft, uint16_t start_bin, uint16_t en
|
||||
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 (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
|
||||
@ -156,6 +138,7 @@ float DSP::find_noise_width(FFTWindowState* fft, uint16_t start_bin, uint16_t en
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
noise_width_hz *= fft->_bin_resolution;
|
||||
|
||||
return noise_width_hz;
|
||||
@ -164,26 +147,26 @@ float DSP::find_noise_width(FFTWindowState* fft, uint16_t start_bin, uint16_t en
|
||||
// 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 {
|
||||
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;
|
||||
}
|
||||
|
||||
// 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->_freq_bins[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.
|
||||
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;
|
||||
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/
|
||||
@ -227,4 +210,122 @@ float DSP::tau(const float x) const
|
||||
return (0.25f * p1 - TAU_FACTOR * p2);
|
||||
}
|
||||
|
||||
// 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
|
||||
|
||||
@ -21,13 +21,32 @@
|
||||
|
||||
#include <stdint.h>
|
||||
#include "AP_HAL_Namespace.h"
|
||||
#include <AP_HAL/utility/RingBuffer.h>
|
||||
|
||||
#define DSP_MEM_REGION AP_HAL::Util::MEM_FAST
|
||||
// Maximum tolerated number of cycles with missing signal
|
||||
#define FFT_MAX_MISSED_UPDATES 5
|
||||
|
||||
class AP_HAL::DSP {
|
||||
#if HAL_WITH_DSP
|
||||
public:
|
||||
typedef float* FFTSampleWindow;
|
||||
enum FrequencyPeak {
|
||||
CENTER = 0,
|
||||
LOWER_SHOULDER = 1,
|
||||
UPPER_SHOULDER = 2,
|
||||
MAX_TRACKED_PEAKS = 3,
|
||||
NONE = 4
|
||||
};
|
||||
|
||||
struct FrequencyPeakData {
|
||||
// estimate of FFT peak frequency
|
||||
float _freq_hz;
|
||||
// FFT bin with maximum energy
|
||||
uint16_t _bin;
|
||||
// width of the peak
|
||||
float _noise_width_hz;
|
||||
};
|
||||
|
||||
class FFTWindowState {
|
||||
public:
|
||||
// frequency width of a FFT bin
|
||||
@ -36,48 +55,52 @@ public:
|
||||
const uint16_t _bin_count;
|
||||
// size of the FFT window
|
||||
const uint16_t _window_size;
|
||||
// number of harmonics of interest
|
||||
const uint8_t _harmonics;
|
||||
// FFT data
|
||||
float* _freq_bins;
|
||||
// derivative real data scratch space
|
||||
float* _derivative_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;
|
||||
// three highest peaks
|
||||
FrequencyPeakData _peak_data[MAX_TRACKED_PEAKS];
|
||||
// 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);
|
||||
FFTWindowState(uint16_t window_size, uint16_t sample_rate, uint8_t harmonics);
|
||||
};
|
||||
// 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;
|
||||
virtual FFTWindowState* fft_init(uint16_t window_size, uint16_t sample_rate, uint8_t harmonics) = 0;
|
||||
// start an FFT analysis with an ObjectBuffer
|
||||
virtual void fft_start(FFTWindowState* state, FloatBuffer& samples, uint16_t advance) = 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;
|
||||
virtual uint16_t fft_analyse(FFTWindowState* state, uint16_t start_bin, uint16_t end_bin, 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);
|
||||
void step_cmplx_mag(FFTWindowState* fft, uint16_t start_bin, uint16_t end_bin, 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);
|
||||
// calculate a single frequency
|
||||
uint16_t calc_frequency(FFTWindowState* fft, uint16_t start_bin, uint16_t peak_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;
|
||||
// find the mean value in an vector of floats
|
||||
virtual float vector_mean_float(const float* vin, uint16_t len) 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;
|
||||
// algorithm for finding peaks in noisy data as per https://terpconnect.umd.edu/~toh/spectrum/PeakFindingandMeasurement.htm
|
||||
uint16_t find_peaks(const float* input, uint16_t length, float* output, uint16_t* peaks, uint16_t peaklen,
|
||||
float slopeThreshold, float ampThreshold, uint16_t smoothwidth, uint16_t peakgroup) const;
|
||||
uint16_t val2index(const float* vector, uint16_t n, float val) const;
|
||||
void derivative(const float* input, float* output, uint16_t n) const;
|
||||
void fastsmooth(float* input, uint16_t n, uint16_t smoothwidth) const;
|
||||
|
||||
// quinn's frequency interpolator
|
||||
float calculate_quinns_second_estimator(const FFTWindowState* fft, const float* complex_fft, uint16_t k) const;
|
||||
|
||||
175
libraries/AP_HAL/examples/DSP_test/DSP_test.cpp
Normal file
175
libraries/AP_HAL/examples/DSP_test/DSP_test.cpp
Normal file
@ -0,0 +1,175 @@
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL_Empty/AP_HAL_Empty.h>
|
||||
#include <GCS_MAVLink/GCS_Dummy.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
#include "GyroFrame.h"
|
||||
|
||||
#if HAL_WITH_DSP
|
||||
const AP_HAL::HAL &hal = AP_HAL::get_HAL();
|
||||
|
||||
static const uint16_t WINDOW_SIZE = 128;
|
||||
static const uint16_t FRAME_SIZE = 1024;
|
||||
static const float max_hz = 350;
|
||||
static const float attenuation_power_db = 15;
|
||||
static const float frequency1 = 120;
|
||||
static const float frequency2 = 50;
|
||||
static const float frequency3 = 350;
|
||||
static float attenuation_cutoff;
|
||||
static FloatBuffer fft_window {WINDOW_SIZE};
|
||||
|
||||
static const uint16_t last_bin = MIN(ceilf(max_hz / ((float)SAMPLE_RATE/ WINDOW_SIZE)), WINDOW_SIZE/2);
|
||||
|
||||
static AP_HAL::DSP::FFTWindowState* fft;
|
||||
|
||||
void setup();
|
||||
void loop();
|
||||
void update();
|
||||
void do_fft(const float* data);
|
||||
|
||||
static AP_SerialManager serial_manager;
|
||||
static AP_BoardConfig board_config;
|
||||
static AP_InertialSensor ins;
|
||||
AP_Int32 logger_bitmask;
|
||||
static AP_Logger logger{logger_bitmask};
|
||||
|
||||
class DummyVehicle {
|
||||
public:
|
||||
};
|
||||
|
||||
class DSPTest : public AP_HAL::DSP {
|
||||
public:
|
||||
virtual FFTWindowState* fft_init(uint16_t w, uint16_t sample_rate, uint8_t harmonics) override { return nullptr; }
|
||||
virtual void fft_start(FFTWindowState* state, FloatBuffer& samples, uint16_t advance) override {}
|
||||
virtual uint16_t fft_analyse(FFTWindowState* state, uint16_t start_bin, uint16_t end_bin, float noise_att_cutoff) override { return 0; }
|
||||
protected:
|
||||
virtual void vector_max_float(const float* vin, uint16_t len, float* maxValue, uint16_t* maxIndex) const override {}
|
||||
virtual void vector_scale_float(const float* vin, float scale, float* vout, uint16_t len) const override {}
|
||||
virtual float vector_mean_float(const float* vin, uint16_t len) const override { return 0.0f; };
|
||||
public:
|
||||
void run_tests();
|
||||
} dsptest;
|
||||
|
||||
//static DummyVehicle vehicle;
|
||||
// create fake gcs object
|
||||
GCS_Dummy _gcs;
|
||||
|
||||
const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
uint32_t frame_num = 0;
|
||||
|
||||
void setup()
|
||||
{
|
||||
hal.console->printf("DSP test\n");
|
||||
board_config.init();
|
||||
serial_manager.init();
|
||||
fft = hal.dsp->fft_init(WINDOW_SIZE, SAMPLE_RATE, 3);
|
||||
attenuation_cutoff = powf(10.0f, -attenuation_power_db / 10.0f);
|
||||
|
||||
for(uint16_t i = 0; i < WINDOW_SIZE; i++) {
|
||||
float sample = sinf(2.0f * M_PI * frequency1 * i / SAMPLE_RATE) * ToRad(20) * 2000;
|
||||
sample += sinf(2.0f * M_PI * frequency2 * i / SAMPLE_RATE) * ToRad(10) * 2000;
|
||||
sample += sinf(2.0f * M_PI * frequency3 * i / SAMPLE_RATE) * ToRad(10) * 2000;
|
||||
fft_window.push(sample);
|
||||
}
|
||||
|
||||
dsptest.run_tests();
|
||||
|
||||
}
|
||||
|
||||
void DSPTest::run_tests() {
|
||||
float vals[] = {1, 1, 1, 10, 10, 10, 1, 1, 1, 1};
|
||||
fastsmooth(vals, 10, 3);
|
||||
for (int i=0; i < 10; i++) {
|
||||
hal.console->printf("%.f ", vals[i]);
|
||||
}
|
||||
hal.console->printf("\n");
|
||||
// fastsmooth([1 1 1 10 10 10 1 1 1 1],3) => [0 1 4 7 10 7 4 1 1 0]
|
||||
}
|
||||
|
||||
|
||||
void do_fft(const float* data)
|
||||
{
|
||||
fft_window.push(data, WINDOW_SIZE);
|
||||
hal.dsp->fft_start(fft, fft_window, WINDOW_SIZE);
|
||||
uint16_t max_bin = hal.dsp->fft_analyse(fft, 1, last_bin, attenuation_cutoff);
|
||||
|
||||
if (max_bin <= 0) {
|
||||
hal.console->printf("FFT: could not detect frequency %.1f\n", frequency1);
|
||||
}
|
||||
|
||||
const float max_energy = fft->_freq_bins[fft->_peak_data[AP_HAL::DSP::CENTER]._bin];
|
||||
|
||||
for (uint16_t i = 0; i < 32; i++) {
|
||||
const uint16_t height = uint16_t(roundf(80.0f * fft->_freq_bins[i] / max_energy));
|
||||
hal.console->printf("[%3.f]", i * fft->_bin_resolution);
|
||||
for (uint16_t j = 0; j < height; j++) {
|
||||
hal.console->printf("\u2588");
|
||||
}
|
||||
hal.console->printf("\n");
|
||||
}
|
||||
|
||||
hal.console->printf("FFT: detected frequencies %.1f/%d/[%.1f-%.1f] %.1f/%d/[%.1f-%.1f] %.1f/%d/[%.1f-%.1f]\n",
|
||||
fft->_peak_data[AP_HAL::DSP::CENTER]._freq_hz,
|
||||
fft->_peak_data[AP_HAL::DSP::CENTER]._bin,
|
||||
(fft->_peak_data[AP_HAL::DSP::CENTER]._bin - 0.5) * fft->_bin_resolution,
|
||||
(fft->_peak_data[AP_HAL::DSP::CENTER]._bin + 0.5) * fft->_bin_resolution,
|
||||
fft->_peak_data[AP_HAL::DSP::LOWER_SHOULDER]._freq_hz,
|
||||
fft->_peak_data[AP_HAL::DSP::LOWER_SHOULDER]._bin,
|
||||
(fft->_peak_data[AP_HAL::DSP::LOWER_SHOULDER]._bin - 0.5) * fft->_bin_resolution,
|
||||
(fft->_peak_data[AP_HAL::DSP::LOWER_SHOULDER]._bin + 0.5) * fft->_bin_resolution,
|
||||
fft->_peak_data[AP_HAL::DSP::UPPER_SHOULDER]._freq_hz,
|
||||
fft->_peak_data[AP_HAL::DSP::UPPER_SHOULDER]._bin,
|
||||
(fft->_peak_data[AP_HAL::DSP::UPPER_SHOULDER]._bin - 0.5) * fft->_bin_resolution,
|
||||
(fft->_peak_data[AP_HAL::DSP::UPPER_SHOULDER]._bin + 0.5) * fft->_bin_resolution);
|
||||
}
|
||||
|
||||
void update()
|
||||
{
|
||||
for (uint16_t i = 0; i < FRAME_SIZE / WINDOW_SIZE; i++) {
|
||||
do_fft(&gyro_frames[frame_num].x[i * WINDOW_SIZE]);
|
||||
}
|
||||
if (++frame_num > NUM_FRAMES) {
|
||||
exit(0);
|
||||
};
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
if (!hal.console->is_initialized()) {
|
||||
return;
|
||||
}
|
||||
uint32_t reference_time, run_time;
|
||||
|
||||
hal.console->printf("--------------------\n");
|
||||
|
||||
reference_time = AP_HAL::micros();
|
||||
update();
|
||||
run_time = AP_HAL::micros() - reference_time;
|
||||
if (run_time > 1000) {
|
||||
hal.console->printf("ran for %d\n", unsigned(run_time));
|
||||
}
|
||||
|
||||
// delay before next display
|
||||
hal.scheduler->delay(1e3); // 1 second
|
||||
}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
||||
#else
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
||||
|
||||
static void loop() { }
|
||||
static void setup()
|
||||
{
|
||||
printf("Board not currently supported\n");
|
||||
}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
||||
#endif
|
||||
75
libraries/AP_HAL/examples/DSP_test/GyroFrame.cpp
Normal file
75
libraries/AP_HAL/examples/DSP_test/GyroFrame.cpp
Normal file
File diff suppressed because one or more lines are too long
11
libraries/AP_HAL/examples/DSP_test/GyroFrame.h
Normal file
11
libraries/AP_HAL/examples/DSP_test/GyroFrame.h
Normal file
@ -0,0 +1,11 @@
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
struct GyroFrame {
|
||||
float x[1024];
|
||||
float y[1024];
|
||||
float z[1024];
|
||||
};
|
||||
|
||||
extern const GyroFrame gyro_frames[];
|
||||
extern const uint32_t NUM_FRAMES;
|
||||
extern const uint16_t SAMPLE_RATE;
|
||||
133
libraries/AP_HAL/examples/DSP_test/gyro_isb_reader.py
Normal file
133
libraries/AP_HAL/examples/DSP_test/gyro_isb_reader.py
Normal file
@ -0,0 +1,133 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
'''
|
||||
extract ISBH and ISBD messages from AP_Logging files and produce C++ arrays for consumption by the DSP subsystem
|
||||
'''
|
||||
from __future__ import print_function
|
||||
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
import copy
|
||||
import numpy
|
||||
|
||||
from argparse import ArgumentParser
|
||||
|
||||
parser = ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--condition", default=None, help="select packets by condition")
|
||||
parser.add_argument("logs", metavar="LOG", nargs="+")
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
from pymavlink import mavutil
|
||||
|
||||
def isb_parser(logfile):
|
||||
'''display fft for raw ACC data in logfile'''
|
||||
|
||||
'''object to store data about a single FFT plot'''
|
||||
class ISBData(object):
|
||||
def __init__(self, ffth):
|
||||
self.seqno = -1
|
||||
self.fftnum = ffth.N
|
||||
self.sensor_type = ffth.type
|
||||
self.instance = ffth.instance
|
||||
self.sample_rate_hz = ffth.smp_rate
|
||||
self.multiplier = ffth.mul
|
||||
self.data = {}
|
||||
self.data["X"] = []
|
||||
self.data["Y"] = []
|
||||
self.data["Z"] = []
|
||||
self.holes = False
|
||||
self.freq = None
|
||||
|
||||
def add_isb(self, fftd):
|
||||
if fftd.N != self.fftnum:
|
||||
print("Skipping ISBD with wrong fftnum (%u vs %u)\n" % (fftd.fftnum, self.fftnum), file=sys.stderr)
|
||||
return
|
||||
if self.holes:
|
||||
print("Skipping ISBD(%u) for ISBH(%u) with holes in it" % (fftd.seqno, self.fftnum), file=sys.stderr)
|
||||
return
|
||||
if fftd.seqno != self.seqno+1:
|
||||
print("ISBH(%u) has holes in it" % fftd.N, file=sys.stderr)
|
||||
self.holes = True
|
||||
return
|
||||
self.seqno += 1
|
||||
self.data["X"].extend(fftd.x)
|
||||
self.data["Y"].extend(fftd.y)
|
||||
self.data["Z"].extend(fftd.z)
|
||||
|
||||
def prefix(self):
|
||||
if self.sensor_type == 0:
|
||||
return "Accel"
|
||||
elif self.sensor_type == 1:
|
||||
return "Gyro"
|
||||
else:
|
||||
return "?Unknown Sensor Type?"
|
||||
|
||||
def tag(self):
|
||||
return str(self)
|
||||
|
||||
def __str__(self):
|
||||
return "%s[%u]" % (self.prefix(), self.instance)
|
||||
|
||||
mlog = mavutil.mavlink_connection(logfile)
|
||||
|
||||
isb_frames = []
|
||||
isbdata = None
|
||||
msgcount = 0
|
||||
start_time = time.time()
|
||||
while True:
|
||||
m = mlog.recv_match(condition=args.condition)
|
||||
if m is None:
|
||||
break
|
||||
msgcount += 1
|
||||
if msgcount % 1000 == 0:
|
||||
sys.stderr.write(".")
|
||||
msg_type = m.get_type()
|
||||
if msg_type == "ISBH":
|
||||
if isbdata is not None:
|
||||
sensor = isbdata.tag()
|
||||
isb_frames.append(isbdata)
|
||||
isbdata = ISBData(m)
|
||||
continue
|
||||
|
||||
if msg_type == "ISBD":
|
||||
if isbdata is None:
|
||||
sys.stderr.write("?(fftnum=%u)" % m.N)
|
||||
continue
|
||||
isbdata.add_isb(m)
|
||||
|
||||
print("", file=sys.stderr)
|
||||
time_delta = time.time() - start_time
|
||||
print("Extracted %u fft data sets" % len(isb_frames), file=sys.stderr)
|
||||
|
||||
sample_rates = {}
|
||||
counts = {}
|
||||
|
||||
print("#include \"GyroFrame.h\"")
|
||||
print("const uint32_t NUM_FRAMES = %d;" % len(isb_frames))
|
||||
print("const GyroFrame gyro_frames[] = {")
|
||||
|
||||
sample_rate = None
|
||||
for isb_frame in isb_frames:
|
||||
if isb_frame.sensor_type == 1 and isb_frame.instance == 0: # gyro data
|
||||
print(" {")
|
||||
|
||||
for axis in [ "X","Y","Z" ]:
|
||||
# normalize data
|
||||
d = numpy.array(isb_frame.data[axis]) # / float(isb_frame.multiplier)
|
||||
print(" { " + ", ".join(d.astype(numpy.dtype(str))) + " },")
|
||||
|
||||
if len(d) == 0:
|
||||
print("No data?!?!?!", file=sys.stderr)
|
||||
if sample_rate is None:
|
||||
sample_rate = isb_frame.sample_rate_hz
|
||||
print(" },")
|
||||
|
||||
print("};")
|
||||
print("const uint16_t SAMPLE_RATE = %d;" % sample_rate)
|
||||
|
||||
|
||||
|
||||
for filename in args.logs:
|
||||
isb_parser(filename)
|
||||
7
libraries/AP_HAL/examples/DSP_test/wscript
Normal file
7
libraries/AP_HAL/examples/DSP_test/wscript
Normal file
@ -0,0 +1,7 @@
|
||||
#!/usr/bin/env python
|
||||
# encoding: utf-8
|
||||
|
||||
def build(bld):
|
||||
bld.ap_example(
|
||||
use='ap',
|
||||
)
|
||||
@ -2,7 +2,9 @@
|
||||
|
||||
#include <atomic>
|
||||
#include <stdint.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL/AP_HAL_Boards.h>
|
||||
#include <AP_HAL/AP_HAL_Macros.h>
|
||||
#include <AP_HAL/Semaphores.h>
|
||||
|
||||
/*
|
||||
* Circular buffer of bytes.
|
||||
@ -99,7 +101,7 @@ private:
|
||||
template <class T>
|
||||
class ObjectBuffer {
|
||||
public:
|
||||
ObjectBuffer(uint32_t _size) {
|
||||
ObjectBuffer(uint32_t _size = 0) {
|
||||
// we set size to 1 more than requested as the byte buffer
|
||||
// gives one less byte than requested. We round up to a full
|
||||
// multiple of the object size so that we always get aligned
|
||||
@ -110,6 +112,15 @@ public:
|
||||
delete buffer;
|
||||
}
|
||||
|
||||
// return size of ringbuffer
|
||||
uint32_t get_size(void) const { return buffer->get_size() / sizeof(T); }
|
||||
|
||||
// set size of ringbuffer, caller responsible for locking
|
||||
bool set_size(uint32_t size) { return buffer->set_size(((size+1) * sizeof(T))); }
|
||||
|
||||
// read len objects without advancing the read pointer
|
||||
uint32_t peek(T *data, uint32_t len) { return buffer->peekbytes((uint8_t*)data, len * sizeof(T)) / sizeof(T); }
|
||||
|
||||
// Discards the buffer content, emptying it.
|
||||
// !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
|
||||
void clear(void)
|
||||
@ -244,7 +255,7 @@ private:
|
||||
template <class T>
|
||||
class ObjectBuffer_TS {
|
||||
public:
|
||||
ObjectBuffer_TS(uint32_t _size) {
|
||||
ObjectBuffer_TS(uint32_t _size = 0) {
|
||||
// we set size to 1 more than requested as the byte buffer
|
||||
// gives one less byte than requested. We round up to a full
|
||||
// multiple of the object size so that we always get aligned
|
||||
@ -255,6 +266,25 @@ public:
|
||||
delete buffer;
|
||||
}
|
||||
|
||||
// return size of ringbuffer
|
||||
uint32_t get_size(void) const {
|
||||
WITH_SEMAPHORE(sem);
|
||||
return buffer->get_size() / sizeof(T);
|
||||
}
|
||||
|
||||
// set size of ringbuffer, caller responsible for locking
|
||||
bool set_size(uint32_t size) {
|
||||
WITH_SEMAPHORE(sem);
|
||||
return buffer->set_size(((size+1) * sizeof(T)));
|
||||
}
|
||||
|
||||
// read len objects without advancing the read pointer
|
||||
uint32_t peek(T *data, uint32_t len) {
|
||||
WITH_SEMAPHORE(sem);
|
||||
return buffer->peekbytes((uint8_t*)data, len * sizeof(T)) / sizeof(T);
|
||||
}
|
||||
|
||||
|
||||
// Discards the buffer content, emptying it.
|
||||
// !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
|
||||
void clear(void)
|
||||
@ -523,3 +553,7 @@ private:
|
||||
uint16_t _count; // number in buffer now
|
||||
uint16_t _head; // first element
|
||||
};
|
||||
|
||||
typedef ObjectBuffer<float> FloatBuffer;
|
||||
typedef ObjectBuffer_TS<float> FloatBuffer_TS;
|
||||
typedef ObjectArray<float> FloatArray;
|
||||
Loading…
Reference in New Issue
Block a user