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_Math/AP_Math.h>
19 : #include "AP_HAL.h"
20 : #include "DSP.h"
21 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
22 : #include <assert.h>
23 : #endif
24 :
25 : #if HAL_WITH_DSP
26 :
27 : using namespace AP_HAL;
28 :
29 : extern const AP_HAL::HAL &hal;
30 :
31 : #define SQRT_2_3 0.816496580927726f
32 : #define SQRT_6 2.449489742783178f
33 :
34 20 : DSP::FFTWindowState::FFTWindowState(uint16_t window_size, uint16_t sample_rate, uint8_t sliding_window_size) :
35 20 : _bin_resolution((float)sample_rate / (float)window_size),
36 20 : _bin_count(window_size / 2),
37 20 : _num_stored_freqs(window_size / 2 + 1),
38 20 : _window_size(window_size),
39 20 : _sliding_window_size(sliding_window_size),
40 20 : _current_slice(0)
41 : {
42 : // includes DC ad Nyquist components and needs to be large enough for intermediate steps
43 20 : _freq_bins = (float*)hal.util->malloc_type(sizeof(float) * _window_size, DSP_MEM_REGION);
44 20 : _derivative_freq_bins = (float*)hal.util->malloc_type(sizeof(float) * _num_stored_freqs, DSP_MEM_REGION);
45 20 : _hanning_window = (float*)hal.util->malloc_type(sizeof(float) * _window_size, DSP_MEM_REGION);
46 : // allocate workspace, including Nyquist component
47 20 : _rfft_data = (float*)hal.util->malloc_type(sizeof(float) * (_window_size + 2), DSP_MEM_REGION);
48 : // sliding window of frequency bin frames
49 20 : if (_sliding_window_size > 0) {
50 4 : _sliding_window = (float*)hal.util->malloc_type(sizeof(float) * _num_stored_freqs * _sliding_window_size, DSP_MEM_REGION);
51 4 : _avg_freq_bins = (float*)hal.util->malloc_type(sizeof(float) * _num_stored_freqs, DSP_MEM_REGION);
52 : // we can still fallback to non-averaging if there is not enough memory
53 4 : if (_avg_freq_bins == nullptr) {
54 0 : hal.util->free_type(_sliding_window, sizeof(float) * _num_stored_freqs * _sliding_window_size, DSP_MEM_REGION);
55 0 : _sliding_window = nullptr;
56 : }
57 : }
58 :
59 20 : if (_freq_bins == nullptr || _hanning_window == nullptr || _rfft_data == nullptr || _derivative_freq_bins == nullptr) {
60 0 : free_data_structures();
61 0 : return;
62 : }
63 :
64 : // create the Hanning window
65 : // https://holometer.fnal.gov/GH_FFT.pdf - equation 19
66 1492 : for (uint16_t i = 0; i < window_size; i++) {
67 1472 : _hanning_window[i] = (0.5f - 0.5f * cosf(2.0f * M_PI * i / ((float)window_size - 1)));
68 1472 : _window_scale += _hanning_window[i];
69 : }
70 : // Calculate the inverse of the Effective Noise Bandwidth - equation 24
71 20 : _window_scale = 2.0f / sq(_window_scale);
72 : }
73 :
74 0 : DSP::FFTWindowState::~FFTWindowState()
75 : {
76 0 : free_data_structures();
77 0 : }
78 :
79 0 : void DSP::FFTWindowState::free_data_structures()
80 : {
81 0 : hal.util->free_type(_freq_bins, sizeof(float) * _window_size * _sliding_window_size, DSP_MEM_REGION);
82 0 : _freq_bins = nullptr;
83 0 : hal.util->free_type(_derivative_freq_bins, sizeof(float) * _num_stored_freqs, DSP_MEM_REGION);
84 0 : _derivative_freq_bins = nullptr;
85 0 : hal.util->free_type(_hanning_window, sizeof(float) * (_window_size), DSP_MEM_REGION);
86 0 : _hanning_window = nullptr;
87 0 : hal.util->free_type(_rfft_data, sizeof(float) * (_window_size + 2), DSP_MEM_REGION);
88 0 : _rfft_data = nullptr;
89 0 : hal.util->free_type(_avg_freq_bins, sizeof(float) * _num_stored_freqs, DSP_MEM_REGION);
90 0 : _avg_freq_bins = nullptr;
91 0 : hal.util->free_type(_sliding_window, sizeof(float) * _num_stored_freqs * _sliding_window_size, DSP_MEM_REGION);
92 0 : _sliding_window = nullptr;
93 0 : }
94 :
95 : // step 3: find the magnitudes of the complex data
96 281246 : void DSP::step_cmplx_mag(FFTWindowState* fft, uint16_t start_bin, uint16_t end_bin, float noise_att_cutoff)
97 : {
98 : // fft->_freq_bins is populated with the complex magnitude values of the fft data
99 : // find the maximum power in the range we are interested in
100 : // in order to see a peak in the last bin we need to allow all the way up to the nyquist
101 281246 : const uint16_t smoothwidth = 1;
102 281246 : float* freq_data = fft->_freq_bins;
103 :
104 281246 : if (fft->_sliding_window != nullptr) {
105 72244 : update_average_from_sliding_window(fft);
106 72244 : freq_data = fft->_avg_freq_bins;
107 : } else {
108 : // scale the power to account for the input window
109 209002 : vector_scale_float(fft->_freq_bins, fft->_window_scale, fft->_freq_bins, fft->_bin_count);
110 : }
111 :
112 281246 : uint16_t bin_range = (MIN(end_bin + ((smoothwidth + 1) >> 1) + 2, fft->_bin_count) - start_bin) + 1;
113 : // find the three highest peaks using a zero crossing algorithm
114 281246 : uint16_t peaks[MAX_TRACKED_PEAKS] {};
115 281246 : memset(fft->_peak_data, 0, sizeof(fft->_peak_data));
116 281246 : 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);
117 : //hal.console->printf("found %d peaks\n", numpeaks);
118 :
119 1124984 : for (uint16_t i = 0; i < MAX_TRACKED_PEAKS; i++) {
120 843738 : fft->_peak_data[i]._bin = peaks[i] + start_bin;
121 : }
122 :
123 281246 : uint16_t top = 0, bottom = 0;
124 281246 : 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);
125 281246 : if (numpeaks > 1) {
126 258843 : 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);
127 : }
128 281246 : if (numpeaks > 2) {
129 223611 : 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);
130 : }
131 :
132 : // average the FFT data
133 281246 : if (fft->_averaging) {
134 11478 : vector_add_float(fft->_avg_freq_bins, fft->_freq_bins, fft->_avg_freq_bins, fft->_bin_count);
135 11478 : fft->_averaging_samples++;
136 : }
137 281246 : }
138 :
139 : // calculate the noise width of a peak based on the input parameters
140 : // freq_bins can be scaled or unscaled for power
141 763700 : 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
142 : {
143 : // max_energy_bin is guaranteed to be between start_bin and end_bin
144 763700 : peak_top = end_bin;
145 763700 : peak_bottom = start_bin;
146 :
147 : // calculate the width of the peak
148 763700 : float noise_width_hz = 1;
149 :
150 : // -attenuation/2 dB point above the center bin
151 763700 : if (max_energy_bin < end_bin) {
152 3901962 : for (uint16_t b = max_energy_bin + 1; b <= end_bin; b++) {
153 3786855 : if (freq_bins[b] < freq_bins[max_energy_bin] * cutoff) {
154 : // we assume that the 3dB point is in the middle of the final shoulder bin
155 615499 : noise_width_hz += (b - max_energy_bin - 0.5f);
156 615499 : peak_top = b;
157 615499 : break;
158 : }
159 : }
160 : }
161 : // -attenuation/2 dB point below the center bin
162 763700 : if (max_energy_bin > start_bin) {
163 4672103 : for (uint16_t b = max_energy_bin - 1; b >= start_bin; b--) {
164 4324495 : if (freq_bins[b] < freq_bins[max_energy_bin] * cutoff) {
165 : // we assume that the 3dB point is in the middle of the final shoulder bin
166 415962 : noise_width_hz += (max_energy_bin - b - 0.5f);
167 415962 : peak_bottom = b;
168 415962 : break;
169 : }
170 : }
171 : }
172 763700 : noise_width_hz *= bin_resolution;
173 :
174 763700 : return noise_width_hz;
175 : }
176 :
177 : // step 4: find the bin with the highest energy and interpolate the required frequency
178 281246 : uint16_t DSP::step_calc_frequencies(FFTWindowState* fft, uint16_t start_bin, uint16_t end_bin)
179 : {
180 281246 : fft->_peak_data[CENTER]._freq_hz = calc_frequency(fft, start_bin, fft->_peak_data[CENTER]._bin, end_bin);
181 281246 : fft->_peak_data[UPPER_SHOULDER]._freq_hz = calc_frequency(fft, start_bin, fft->_peak_data[UPPER_SHOULDER]._bin, end_bin);
182 281246 : fft->_peak_data[LOWER_SHOULDER]._freq_hz = calc_frequency(fft, start_bin, fft->_peak_data[LOWER_SHOULDER]._bin, end_bin);
183 :
184 281246 : return fft->_peak_data[CENTER]._bin;
185 : }
186 :
187 72244 : void DSP::update_average_from_sliding_window(FFTWindowState* fft)
188 : {
189 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
190 : #define ASSERT_MAX(v) assert((v)<(fft->_num_stored_freqs * fft->_sliding_window_size))
191 : #else
192 : #define ASSERT_MAX(v)
193 : #endif
194 :
195 : // copy and scale the new slice
196 72244 : const uint16_t slice_index = fft->_current_slice * fft->_num_stored_freqs;
197 72244 : ASSERT_MAX(slice_index);
198 72244 : float* slice = &fft->_sliding_window[slice_index];
199 :
200 72244 : const uint16_t old_slice_index = ((fft->_current_slice + 1) % fft->_sliding_window_size) * fft->_num_stored_freqs;
201 72244 : ASSERT_MAX(old_slice_index);
202 72244 : float* old_slice = &fft->_sliding_window[old_slice_index];
203 :
204 72244 : const float inv_ssize = 1.0f / fft->_sliding_window_size;
205 :
206 2384052 : for (uint16_t i = 0; i < fft->_bin_count; i++) {
207 2311808 : slice[i] = fft->_freq_bins[i] * fft->_window_scale * inv_ssize;
208 2311808 : fft->_avg_freq_bins[i] = fft->_avg_freq_bins[i] + slice[i] - old_slice[i];
209 : }
210 :
211 : // advance the current slice
212 72244 : fft->_current_slice = (fft->_current_slice + 1) % fft->_sliding_window_size;
213 72244 : }
214 :
215 : // calculate a single frequency
216 843738 : uint16_t DSP::calc_frequency(FFTWindowState* fft, uint16_t start_bin, uint16_t peak_bin, uint16_t end_bin)
217 : {
218 843738 : if (peak_bin == 0 || is_zero(fft->get_freq_bin(peak_bin))) {
219 8 : return start_bin * fft->_bin_resolution;
220 : }
221 :
222 843730 : peak_bin = constrain_int16(peak_bin, start_bin, end_bin);
223 :
224 : // It turns out that Jain is pretty good and works with only magnitudes, but Candan is significantly better
225 : // if you have access to the complex values and Quinn is a little better still. Quinn is computationally
226 : // more expensive, but compared to the overall FFT cost seems worth it.
227 843730 : if (fft->_sliding_window != nullptr) {
228 216732 : return (peak_bin + calculate_jains_estimator(fft, fft->_avg_freq_bins, peak_bin)) * fft->_bin_resolution;
229 : } else {
230 626998 : return (peak_bin + calculate_quinns_second_estimator(fft, fft->_rfft_data, peak_bin)) * fft->_bin_resolution;
231 : }
232 : }
233 :
234 : // Interpolate center frequency using https://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/
235 626998 : float DSP::calculate_quinns_second_estimator(const FFTWindowState* fft, const float* complex_fft, uint16_t k_max) const
236 : {
237 626998 : if (k_max <= 1 || k_max >= fft->_bin_count) {
238 18272 : return 0.0f;
239 : }
240 :
241 608726 : const uint16_t k_m1 = (k_max - 1) * 2;
242 608726 : const uint16_t k_p1 = (k_max + 1) * 2;
243 608726 : const uint16_t k = k_max * 2;
244 :
245 608726 : const float divider = complex_fft[k] * complex_fft[k] + complex_fft[k+1] * complex_fft[k+1];
246 608726 : const float ap = (complex_fft[k_p1] * complex_fft[k] + complex_fft[k_p1 + 1] * complex_fft[k+1]) / divider;
247 608726 : const float am = (complex_fft[k_m1] * complex_fft[k] + complex_fft[k_m1 + 1] * complex_fft[k + 1]) / divider;
248 :
249 : // sanity check
250 608726 : if (fabsf(1.0f - ap) < 0.01f || fabsf(1.0f - am) < 0.01f) {
251 177 : return 0.0f;
252 : }
253 :
254 608549 : const float dp = -ap / (1.0f - ap);
255 608549 : const float dm = am / (1.0f - am);
256 :
257 608549 : float d = (dp + dm) * 0.5f + tau(dp * dp) - tau(dm * dm);
258 :
259 : // -0.5 < d < 0.5 which is the fraction of the sample spacing about the center element
260 608549 : return constrain_float(d, -0.5f, 0.5f);
261 : }
262 :
263 : static const float TAU_FACTOR = SQRT_6 / 24.0f;
264 :
265 : // Helper function used for Quinn's frequency estimation
266 1217098 : float DSP::tau(const float x) const
267 : {
268 1217098 : float p1 = logf(3.0f * sq(x) + 6.0f * x + 1.0f);
269 1217098 : float part1 = x + 1.0f - SQRT_2_3;
270 1217098 : float part2 = x + 1.0f + SQRT_2_3;
271 1217098 : float p2 = logf(part1 / part2);
272 1217098 : return (0.25f * p1 - TAU_FACTOR * p2);
273 : }
274 :
275 : // from https://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/
276 : // Works on magnitudes only, which is useful when using averaged data
277 216738 : float DSP::calculate_jains_estimator(const FFTWindowState* fft, const float* real_fft, uint16_t k_max)
278 : {
279 216738 : if (k_max <= 1 || k_max >= fft->_bin_count) {
280 0 : return 0.0f;
281 : }
282 :
283 216738 : float y1 = real_fft[k_max-1];
284 216738 : float y2 = real_fft[k_max];
285 216738 : float y3 = real_fft[k_max+1];
286 :
287 216738 : if (is_zero(y2) || is_zero(y1)) {
288 0 : return 0.0f;
289 : }
290 :
291 216738 : float d = 0.0f;
292 :
293 216738 : if (y1 > y3) {
294 110888 : float a = y2 / y1;
295 110888 : d = a / (1 + a) - 1;
296 : } else {
297 105850 : float a = y3 / y2;
298 105850 : d = a / (1 + a);
299 : }
300 216738 : return constrain_float(d, -0.5f, 0.5f);
301 : }
302 :
303 : // initialize averaging FFT windows as they are calculated
304 6 : bool DSP::fft_init_average(FFTWindowState* fft)
305 : {
306 6 : if (fft->_avg_freq_bins == nullptr) {
307 2 : fft->_avg_freq_bins = (float*)hal.util->malloc_type(sizeof(float) * fft->_num_stored_freqs, DSP_MEM_REGION);
308 2 : if (fft->_avg_freq_bins == nullptr) {
309 0 : return false;
310 : }
311 : }
312 :
313 6 : return true;
314 : }
315 :
316 : // start averaging FFT windows as they are calculated
317 2 : bool DSP::fft_start_average(FFTWindowState* fft)
318 : {
319 2 : if (fft->_averaging) {
320 0 : return false;
321 : }
322 :
323 2 : if (!fft_init_average(fft)) {
324 0 : return false;
325 : }
326 :
327 2 : fft->_averaging_samples = 0;
328 2 : fft->_averaging = true;
329 2 : return true;
330 : }
331 :
332 : // start averaging FFT windows as they are calculated
333 4 : uint16_t DSP::fft_stop_average(FFTWindowState* fft, uint16_t start_bin, uint16_t end_bin, float* freqs)
334 : {
335 : // ensure the window has been allocated even if we do nothing else
336 4 : if (!fft_init_average(fft)) {
337 0 : return 0;
338 : }
339 :
340 4 : if (!fft->_averaging) {
341 2 : return 0;
342 : }
343 :
344 2 : fft->_averaging = false;
345 :
346 : // scale by the number of samples
347 2 : vector_scale_float(fft->_avg_freq_bins, fft->_averaging_samples, fft->_avg_freq_bins, fft->_bin_count);
348 :
349 2 : const uint16_t smoothwidth = 1;
350 2 : uint16_t bin_range = (MIN(end_bin + ((smoothwidth + 1) >> 1) + 2, fft->_bin_count) - start_bin) + 1;
351 :
352 : // find the three highest peaks using a zero crossing algorithm
353 : // allocate the scratch space locally as we are in a different thread to the regular FFT
354 2 : float* scratch_space = (float*)hal.util->malloc_type(sizeof(float) * fft->_num_stored_freqs, DSP_MEM_REGION);
355 2 : if (scratch_space == nullptr) {
356 0 : return false;
357 : }
358 2 : uint16_t peaks[MAX_TRACKED_PEAKS] {};
359 2 : uint16_t numpeaks = find_peaks(&fft->_avg_freq_bins[start_bin], bin_range,
360 2 : scratch_space, peaks, MAX_TRACKED_PEAKS, 0.0f, -1.0f, smoothwidth, 2);
361 2 : hal.util->free_type(scratch_space, sizeof(float) * fft->_num_stored_freqs, DSP_MEM_REGION);
362 :
363 2 : numpeaks = MIN(numpeaks, uint16_t(MAX_TRACKED_PEAKS));
364 :
365 : // now try and find the lowest harmonic
366 8 : for (uint16_t i = 0; i < numpeaks; i++) {
367 6 : const uint16_t bin = peaks[i] + start_bin;
368 6 : float d = calculate_jains_estimator(fft, fft->_avg_freq_bins, bin);
369 6 : freqs[i] = (bin + d) * fft->_bin_resolution;
370 : }
371 :
372 2 : fft->_averaging_samples = 0;
373 2 : return numpeaks;
374 : }
375 :
376 : // find all the peaks in the fft window using https://terpconnect.umd.edu/~toh/spectrum/PeakFindingandMeasurement.htm
377 : // in general peakgrup > 2 is only good for very broad noisy peaks, <= 2 better for spikey peaks, although 1 will miss
378 : // a true spike 50% of the time
379 281248 : uint16_t DSP::find_peaks(const float* input, uint16_t length, float* d, uint16_t* peaks, uint16_t peaklen,
380 : float slopeThreshold, float ampThreshold, uint16_t smoothwidth, uint16_t peakgroup) const
381 : {
382 281248 : if (smoothwidth > 1) {
383 0 : derivative(input, d, length);
384 0 : fastsmooth(d, length, smoothwidth);
385 : } else {
386 281248 : derivative(input, d, length);
387 : }
388 :
389 281248 : uint16_t n = ((peakgroup + 1) >> 1) + 1;
390 281248 : uint16_t halfw = (smoothwidth + 1) >> 1;
391 281248 : uint16_t numpeaks = 0;
392 281248 : uint16_t peakX = 0;
393 281248 : float peakY = 0;
394 : uint16_t pindex;
395 281248 : uint16_t xx[peakgroup];
396 281248 : float yy[peakgroup];
397 281248 : memset(xx, 0, peakgroup * sizeof(uint16_t));
398 281248 : memset(yy, 0, peakgroup * sizeof(float));
399 :
400 7442750 : for (uint16_t j = (halfw << 1) - 2; j < length - smoothwidth - 1; j++) {
401 7161502 : if (d[j] >= 0 && d[j + 1] <= 0 && !is_equal(d[j], d[j + 1])) { // detect zero crossing
402 1329731 : if ((d[j] - d[j + 1]) > slopeThreshold) {
403 3989193 : for (uint16_t k = 0; k < peakgroup; k++) {
404 2659462 : uint16_t groupIndex = j + k - n + 2;
405 2659462 : groupIndex = constrain_int16(groupIndex, 0, length - 1);
406 2659462 : xx[k] = groupIndex;
407 2659462 : yy[k] = input[groupIndex];
408 : }
409 1329731 : if (peakgroup < 3) {
410 1329731 : vector_max_float(yy, peakgroup, &peakY, &pindex);
411 : } else {
412 0 : peakY = vector_mean_float(yy, peakgroup);
413 0 : pindex = val2index(yy, peakgroup, peakY);
414 : }
415 1329731 : peakX = xx[pindex];
416 : //hal.console->printf("zero %d, gindex %d -> %d, index %d, val %f\n", j, j -n +2, j+peakgroup -1 - n +2, peakX, peakY);
417 : // see if we have a valid peak
418 1329731 : if (isfinite(peakY) && peakY >= ampThreshold) {
419 : // record in amplitude order
420 3251843 : for (int16_t i = 0; i < peaklen; i++) {
421 2800858 : if (i >= numpeaks) {
422 536101 : peaks[i] = peakX;
423 536101 : break;
424 : }
425 2264757 : if (peakY > input[peaks[i]]) {
426 846484 : for (int16_t a = peaklen - 1; a > i; a--) {
427 503839 : peaks[a] = peaks[a - 1];
428 : }
429 342645 : peaks[i] = peakX;
430 342645 : break;
431 : }
432 : }
433 1329731 : numpeaks++;
434 : }
435 : }
436 : }
437 : }
438 :
439 281248 : return numpeaks;
440 281248 : }
441 :
442 : // Returns the index and the value of the element of a vector that is closest to val
443 0 : uint16_t DSP::val2index(const float* vector, uint16_t n, float val) const
444 : {
445 0 : float minval = FLT_MAX;
446 0 : uint16_t minidx = 0;
447 0 : for (uint16_t i = 0; i < n; i++) {
448 0 : float dif = fabsf(vector[i] - val);
449 0 : if (dif < minval) {
450 0 : minval = dif;
451 0 : minidx = i;
452 : }
453 : }
454 0 : return minidx;
455 : }
456 :
457 : // First derivative of vector using 2-point central difference.
458 281248 : void DSP::derivative(const float* input, float* output, uint16_t n) const
459 : {
460 281248 : output[0] = input[1] - input[0];
461 281248 : output[n - 1] = input[n - 1] - input[n - 2];
462 7442750 : for (uint16_t i = 1; i < n - 1; i++) {
463 7161502 : output[i] = (input[i + 1] - input[i - 1]) / 2.0f;
464 : }
465 281248 : }
466 :
467 : // smooth a vector in-place
468 0 : void DSP::fastsmooth(float* input, uint16_t n, uint16_t smoothwidth) const
469 : {
470 0 : float window[smoothwidth];
471 0 : memset(window, 0, smoothwidth * sizeof(float));
472 0 : float sumpoints = 0.0f;
473 0 : for (int i = 0; i < smoothwidth; i++) {
474 0 : sumpoints += input[i];
475 : }
476 0 : uint16_t halfw = (smoothwidth + 1) >> 1;
477 0 : for (int i = 0; i < n - smoothwidth; i++) {
478 0 : window[i % smoothwidth] = sumpoints;
479 0 : sumpoints -= input[i];
480 0 : sumpoints += input[i + smoothwidth];
481 0 : input[i] = window[(i + smoothwidth - 1) % smoothwidth] / smoothwidth;
482 : }
483 0 : uint16_t last = n - smoothwidth + halfw;
484 0 : input[last] = 0.0f;
485 0 : for (int i = last + 1; i < n; i++) {
486 0 : input[last] += input[i];
487 : }
488 0 : input[n - smoothwidth + halfw] /= smoothwidth;
489 0 : for (int i = last + 1; i < n; i++) {
490 0 : input[i] = 0.0f;
491 : }
492 0 : }
493 :
494 : #endif // HAL_WITH_DSP
|
