Line data Source code
1 : /*
2 : * This file is free software: you can redistribute it and/or modify it
3 : * under the terms of the GNU General Public License as published by the
4 : * Free Software Foundation, either version 3 of the License, or
5 : * (at your option) any later version.
6 : *
7 : * This file is distributed in the hope that it will be useful, but
8 : * WITHOUT ANY WARRANTY; without even the implied warranty of
9 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
10 : * See the GNU General Public License for more details.
11 : *
12 : * You should have received a copy of the GNU General Public License along
13 : * with this program. If not, see <http://www.gnu.org/licenses/>.
14 : *
15 : * Code by Andy Piper
16 : */
17 : /*
18 : interface to DSP device
19 : */
20 : #pragma once
21 :
22 : #include <stdint.h>
23 : #include "AP_HAL_Namespace.h"
24 : #include <AP_HAL/utility/RingBuffer.h>
25 :
26 : #define DSP_MEM_REGION AP_HAL::Util::MEM_FAST
27 : // Maximum tolerated number of cycles with missing signal
28 : #define FFT_MAX_MISSED_UPDATES 5
29 :
30 : class AP_HAL::DSP {
31 : #if HAL_WITH_DSP
32 : public:
33 : enum FrequencyPeak : uint8_t {
34 : CENTER = 0,
35 : LOWER_SHOULDER = 1,
36 : UPPER_SHOULDER = 2,
37 : MAX_TRACKED_PEAKS = 3,
38 : NONE = 4
39 : };
40 :
41 : struct FrequencyPeakData {
42 : // estimate of FFT peak frequency
43 : float _freq_hz;
44 : // FFT bin with maximum energy
45 : uint16_t _bin;
46 : // width of the peak
47 : float _noise_width_hz;
48 : };
49 :
50 : static const uint8_t MAX_SLIDING_WINDOW_SIZE = 8;
51 :
52 : class FFTWindowState {
53 : public:
54 : // frequency width of a FFT bin
55 : const float _bin_resolution;
56 : // number of FFT bins
57 : const uint16_t _bin_count;
58 : // number of stored frequencies (_bin_count + DC)
59 : const uint16_t _num_stored_freqs;
60 : // size of the FFT window
61 : const uint16_t _window_size;
62 : // size of the FFT sliding window
63 : const uint8_t _sliding_window_size;
64 : // FFT data
65 : float* _freq_bins;
66 : // derivative real data scratch space
67 : float* _derivative_freq_bins;
68 : // intermediate real FFT data
69 : float* _rfft_data;
70 : // averaged frequency data via Welch's method
71 : float* _avg_freq_bins;
72 : // sliding window of _bin_count frames
73 : float* _sliding_window;
74 : // three highest peaks
75 : FrequencyPeakData _peak_data[MAX_TRACKED_PEAKS];
76 : // Hanning window for incoming samples, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
77 : float* _hanning_window;
78 : // Use in calculating the PS of the signal [Heinz] equations (20) & (21)
79 : float _window_scale;
80 : // averaging is ongoing
81 : bool _averaging;
82 : // number of samples in the average
83 : uint32_t _averaging_samples;
84 : // current sliding window slice
85 : uint8_t _current_slice;
86 : // get a frequency bin from an arbitrary slice
87 3290504 : float get_freq_bin(uint16_t idx) { return _sliding_window == nullptr ? _freq_bins[idx] : _avg_freq_bins[idx]; }
88 :
89 : void free_data_structures();
90 : virtual ~FFTWindowState();
91 : FFTWindowState(uint16_t window_size, uint16_t sample_rate, uint8_t sliding_window_size);
92 : };
93 : // initialise an FFT instance
94 : virtual FFTWindowState* fft_init(uint16_t window_size, uint16_t sample_rate, uint8_t sliding_window_size = 0) = 0;
95 : // start an FFT analysis with an ObjectBuffer
96 : virtual void fft_start(FFTWindowState* state, FloatBuffer& samples, uint16_t advance) = 0;
97 : // perform remaining steps of an FFT analysis
98 : virtual uint16_t fft_analyse(FFTWindowState* state, uint16_t start_bin, uint16_t end_bin, float noise_att_cutoff) = 0;
99 : // start averaging FFT data
100 : bool fft_start_average(FFTWindowState* fft);
101 : // finish the averaging process
102 : uint16_t fft_stop_average(FFTWindowState* fft, uint16_t start_bin, uint16_t end_bin, float* peaks);
103 :
104 : protected:
105 : // step 3: find the magnitudes of the complex data
106 : void step_cmplx_mag(FFTWindowState* fft, uint16_t start_bin, uint16_t end_bin, float noise_att_cutoff);
107 : // calculate the noise width of a peak based on the input parameters
108 : float find_noise_width(float* freq_bins, uint16_t start_bin, uint16_t end_bin, uint16_t max_energy_bin, float cutoff,
109 : float bin_resolution, uint16_t& peak_top, uint16_t& peak_bottom) const;
110 : // step 4: find the bin with the highest energy and interpolate the required frequency
111 : uint16_t step_calc_frequencies(FFTWindowState* fft, uint16_t start_bin, uint16_t end_bin);
112 : // calculate the final average output
113 : void update_average_from_sliding_window(FFTWindowState* fft);
114 : // calculate a single frequency
115 : uint16_t calc_frequency(FFTWindowState* fft, uint16_t start_bin, uint16_t peak_bin, uint16_t end_bin);
116 : // find the maximum value in an vector of floats
117 : virtual void vector_max_float(const float* vin, uint16_t len, float* max_value, uint16_t* max_index) const = 0;
118 : // find the mean value in an vector of floats
119 : virtual float vector_mean_float(const float* vin, uint16_t len) const = 0;
120 : // multiply an vector of floats by a scale factor
121 : virtual void vector_scale_float(const float* vin, float scale, float* vout, uint16_t len) const = 0;
122 : // add two vectors together
123 : virtual void vector_add_float(const float* vin1, const float* vin2, float* vout, uint16_t len) const = 0;
124 : // algorithm for finding peaks in noisy data as per https://terpconnect.umd.edu/~toh/spectrum/PeakFindingandMeasurement.htm
125 : uint16_t find_peaks(const float* input, uint16_t length, float* output, uint16_t* peaks, uint16_t peaklen,
126 : float slopeThreshold, float ampThreshold, uint16_t smoothwidth, uint16_t peakgroup) const;
127 : uint16_t val2index(const float* vector, uint16_t n, float val) const;
128 : void derivative(const float* input, float* output, uint16_t n) const;
129 : void fastsmooth(float* input, uint16_t n, uint16_t smoothwidth) const;
130 :
131 : // Quinn's frequency interpolator
132 : float calculate_quinns_second_estimator(const FFTWindowState* fft, const float* complex_fft, uint16_t k) const;
133 : float tau(const float x) const;
134 : // Jain's estimator
135 : float calculate_jains_estimator(const FFTWindowState* fft, const float* real_fft, uint16_t k_max);
136 : // init averaging FFT data
137 : bool fft_init_average(FFTWindowState* fft);
138 :
139 : #endif // HAL_WITH_DSP
140 : };
|
