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 : #include <AP_HAL/AP_HAL.h>
19 :
20 : #if HAL_WITH_DSP
21 :
22 : #include "AP_HAL_SITL.h"
23 : #include <AP_Math/AP_Math.h>
24 : #include <GCS_MAVLink/GCS.h>
25 : #include "DSP.h"
26 : #include <cmath>
27 : #include <assert.h>
28 :
29 : using namespace HALSITL;
30 :
31 : extern const AP_HAL::HAL& hal;
32 :
33 : // The algorithms originally came from betaflight but are now substantially modified based on theory and experiment.
34 : // https://holometer.fnal.gov/GH_FFT.pdf "Spectrum and spectral density estimation by the Discrete Fourier transform (DFT),
35 : // including a comprehensive list of window functions and some new flat-top windows." - Heinzel et. al is a great reference
36 : // for understanding the underlying theory although we do not use spectral density here since time resolution is equally
37 : // important as frequency resolution. Referred to as [Heinz] throughout the code.
38 :
39 : // initialize the FFT state machine
40 20 : AP_HAL::DSP::FFTWindowState* DSP::fft_init(uint16_t window_size, uint16_t sample_rate, uint8_t sliding_window_size)
41 : {
42 20 : DSP::FFTWindowStateSITL* fft = NEW_NOTHROW DSP::FFTWindowStateSITL(window_size, sample_rate, sliding_window_size);
43 20 : if (fft == nullptr || fft->_hanning_window == nullptr || fft->_rfft_data == nullptr || fft->_freq_bins == nullptr || fft->_derivative_freq_bins == nullptr) {
44 0 : delete fft;
45 0 : return nullptr;
46 : }
47 20 : return fft;
48 : }
49 :
50 : // start an FFT analysis
51 281246 : void DSP::fft_start(AP_HAL::DSP::FFTWindowState* state, FloatBuffer& samples, uint16_t advance)
52 : {
53 281246 : step_hanning((FFTWindowStateSITL*)state, samples, advance);
54 281246 : }
55 :
56 : // perform remaining steps of an FFT analysis
57 281246 : uint16_t DSP::fft_analyse(AP_HAL::DSP::FFTWindowState* state, uint16_t start_bin, uint16_t end_bin, float noise_att_cutoff)
58 : {
59 281246 : FFTWindowStateSITL* fft = (FFTWindowStateSITL*)state;
60 281246 : step_fft(fft);
61 281246 : step_cmplx_mag(fft, start_bin, end_bin, noise_att_cutoff);
62 281246 : return step_calc_frequencies(fft, start_bin, end_bin);
63 : }
64 :
65 : // create an instance of the FFT state machine
66 20 : DSP::FFTWindowStateSITL::FFTWindowStateSITL(uint16_t window_size, uint16_t sample_rate, uint8_t sliding_window_size)
67 20 : : AP_HAL::DSP::FFTWindowState::FFTWindowState(window_size, sample_rate, sliding_window_size)
68 : {
69 20 : if (_freq_bins == nullptr || _hanning_window == nullptr || _rfft_data == nullptr || _derivative_freq_bins == nullptr) {
70 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to allocate window for DSP");
71 0 : return;
72 : }
73 :
74 1492 : buf = NEW_NOTHROW complexf[window_size];
75 : }
76 :
77 0 : DSP::FFTWindowStateSITL::~FFTWindowStateSITL()
78 : {
79 0 : delete[] buf;
80 0 : }
81 :
82 : // step 1: filter the incoming samples through a Hanning window
83 281246 : void DSP::step_hanning(FFTWindowStateSITL* fft, FloatBuffer& samples, uint16_t advance)
84 : {
85 : // 5us
86 : // apply hanning window to gyro samples and store result in _freq_bins
87 : // hanning starts and ends with 0, could be skipped for minor speed improvement
88 281246 : uint32_t read_window = samples.peek(&fft->_freq_bins[0], fft->_window_size);
89 281246 : if (read_window != fft->_window_size) {
90 0 : return;
91 : }
92 281246 : samples.advance(advance);
93 281246 : mult_f32(&fft->_freq_bins[0], &fft->_hanning_window[0], &fft->_freq_bins[0], fft->_window_size);
94 : }
95 :
96 : // step 2: perform an in-place FFT on the windowed data
97 281246 : void DSP::step_fft(FFTWindowStateSITL* fft)
98 : {
99 17341278 : for (uint16_t i = 0; i < fft->_window_size; i++) {
100 17060032 : fft->buf[i] = complexf(fft->_freq_bins[i], 0);
101 : }
102 :
103 281246 : calculate_fft(fft->buf, fft->_window_size);
104 :
105 8811262 : for (uint16_t i = 0; i < fft->_bin_count; i++) {
106 8530016 : fft->_freq_bins[i] = std::norm(fft->buf[i]);
107 : }
108 :
109 : // components at the nyquist frequency are real only
110 9092508 : for (uint16_t i = 0, j = 0; i <= fft->_bin_count; i++, j += 2) {
111 8811262 : fft->_rfft_data[j] = fft->buf[i].real();
112 8811262 : fft->_rfft_data[j+1] = fft->buf[i].imag();
113 : }
114 281246 : }
115 :
116 281246 : void DSP::mult_f32(const float* v1, const float* v2, float* vout, uint16_t len)
117 : {
118 17341278 : for (uint16_t i = 0; i < len; i++) {
119 17060032 : vout[i] = v1[i] * v2[i];
120 : }
121 281246 : }
122 :
123 1329731 : void DSP::vector_max_float(const float* vin, uint16_t len, float* maxValue, uint16_t* maxIndex) const
124 : {
125 1329731 : *maxValue = vin[0];
126 1329731 : *maxIndex = 0;
127 2659462 : for (uint16_t i = 1; i < len; i++) {
128 1329731 : if (vin[i] > *maxValue) {
129 648292 : *maxValue = vin[i];
130 648292 : *maxIndex = i;
131 : }
132 : }
133 1329731 : }
134 :
135 209004 : void DSP::vector_scale_float(const float* vin, float scale, float* vout, uint16_t len) const
136 : {
137 6427276 : for (uint16_t i = 0; i < len; i++) {
138 6218272 : vout[i] = vin[i] * scale;
139 : }
140 209004 : }
141 :
142 11478 : void DSP::vector_add_float(const float* vin1, const float* vin2, float* vout, uint16_t len) const
143 : {
144 378774 : for (uint16_t i = 0; i < len; i++) {
145 367296 : vout[i] = vin1[i] + vin2[i];
146 : }
147 11478 : }
148 :
149 0 : float DSP::vector_mean_float(const float* vin, uint16_t len) const
150 : {
151 0 : float mean_value = 0.0f;
152 0 : for (uint16_t i = 0; i < len; i++) {
153 0 : mean_value += vin[i];
154 : }
155 0 : mean_value /= len;
156 0 : return mean_value;
157 : }
158 :
159 : // simple integer log2
160 281246 : static uint16_t fft_log2(uint16_t n)
161 : {
162 281246 : uint16_t k = n, i = 0;
163 2194594 : while (k) {
164 1913348 : k >>= 1;
165 1913348 : i++;
166 : }
167 281246 : return i - 1;
168 : }
169 :
170 : // calculate the in-place FFT of the input using the Cooley–Tukey algorithm
171 : // this is a translation of Ron Nicholson's version in http://www.nicholson.com/dsp.fft1.html
172 281246 : void DSP::calculate_fft(complexf *samples, uint16_t fftlen)
173 : {
174 281246 : uint16_t m = fft_log2(fftlen);
175 : // shuffle data using bit reversed addressing ***
176 17341278 : for (uint16_t k = 0; k < fftlen; k++) {
177 : // generate a bit reversed address for samples[k] ***
178 17060032 : uint16_t ki = k, kr = 0;
179 120365248 : for (uint16_t i=1; i<=m; i++) {
180 103305216 : kr <<= 1; // left shift result kr by 1 bit
181 103305216 : if (ki % 2 == 1) {
182 51652608 : kr++;
183 : }
184 103305216 : ki >>= 1; // right shift temp ki by 1 bit
185 : }
186 : // swap data samples[k] to bit reversed address samples[kr]
187 17060032 : if (kr > k) {
188 7342292 : complexf t = samples[kr];
189 7342292 : samples[kr] = samples[k];
190 7342292 : samples[k] = t;
191 : }
192 : }
193 :
194 : // do fft butterflys in place
195 281246 : uint16_t istep = 2;
196 1913348 : while (istep <= fftlen) {// layers 2,4,8,16, ... ,n
197 1632102 : uint16_t is2 = istep / 2;
198 1632102 : uint16_t astep = fftlen / istep;
199 18410888 : for (uint16_t km = 0; km < is2; km++) { // outer row loop
200 16778786 : uint16_t a = km * astep; // twiddle angle index
201 16778786 : complexf w(sinf(2 * M_PI * (a+(fftlen/4)) / fftlen), sinf(2 * M_PI * a / fftlen));
202 68431394 : for (uint16_t ki = 0; ki <= (fftlen - istep); ki += istep) { // inner column loop
203 51652608 : uint16_t i = km + ki;
204 51652608 : uint16_t j = is2 + i;
205 51652608 : complexf t = w * samples[j];
206 51652608 : complexf q = samples[i];
207 51652608 : samples[j] = q - t;
208 51652608 : samples[i] = q + t;
209 : }
210 : }
211 1632102 : istep <<= 1;
212 : }
213 281246 : }
214 :
215 : #endif
|
