acc_algorithm.c
Go to the documentation of this file.
1 // Copyright (c) Acconeer AB, 2023
2 // All rights reserved
3 // This file is subject to the terms and conditions defined in the file
4 // 'LICENSES/license_acconeer.txt', (BSD 3-Clause License) which is part
5 // of this source code package.
6
7 #include <complex.h>
8 #include <math.h>
9 #include <stdint.h>
10 #include <stdlib.h>
11
12 #include "acc_alg_basic_utils.h"
13 #include "acc_algorithm.h"
14 #include "acc_definitions_a121.h"
15 #include "acc_definitions_common.h"
16
17 #define DOUBLE_BUFFERING_MEAN_ABS_DEV_OUTLIER_TH 5
18
19
20 static void filter_inplace_apply(uint16_t sample_idx,
21 const float *b,
22 const float *a,
23 float state[5],
24 float *data);
25
26
27 static float complex get_data_padded_f32_to_f32_complex(const float *data, uint16_t data_length, uint16_t index, uint16_t stride);
28
29
30 static float complex get_data_padded_f32_complex(const float complex *data, uint16_t data_length, uint16_t index, uint16_t stride);
31
32
33 static void small_rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride);
34
35
36 static void small_fft(const float complex *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride);
37
38
39 static void small_fft_transformation(uint16_t length_shift, float complex *output, uint16_t stride);
40
41
42 static void small_rfft_real_symmetry_conversion(float complex *output, uint16_t length_shift, uint16_t stride);
43
44
45 /**
46 * @brief Interpolate function for Double buffering
47 *
48 * This function will calculate the interpolated value from the sweep
49 * before and the sweep two positions ahead.
50 *
51 * @param[in, out] frame Data frame to where the filter is applied
52 * @param[in] sweeps_per_frame How many sweeps there are in the frame
53 * @param[in] num_points The number of points in the frame
54 * @param[in] sweep The sweep to generate with interpolatation
55 * @param[in] point The point to generate with interpolatation
56 */
57 static void double_buffering_interpolate(acc_int16_complex_t *frame,
58 const uint16_t sweeps_per_frame,
59 const uint16_t num_points,
60 const uint16_t sweep,
61 const uint16_t point);
62
63
64 /**
65 * @brief Median function for Double buffering
66 *
67 * This function will calculate the median value of four complex values.
68 *
69 * @param[in, out] frame Data frame to where the filter is applied
70 * @param[in] num_points The number of points in the frame
71 * @param[in] sweep The sweep to generate with median filter
72 * @param[in] point The point to generate with median filter
73 * @param[in] median_start_sweep The start sweep for the median calculation
74 */
75 static void double_buffering_median_filter(acc_int16_complex_t *frame,
76 const uint16_t num_points,
77 const uint16_t sweep,
78 const uint16_t point,
79 const uint16_t median_start_sweep);
80
81
82 /**
83 * @brief Merge peak cluster
84 *
85 * @param[in] start_idx Start index
86 * @param[in] num_peaks Number of peaks to merge
87 * @param[in] velocities Velocities array
88 * @param[in] energies Energies array
89 * @param[in] peak_idxs Peak indexes array
90 * @param[out] merged_velocities Merged velocities array
91 * @param[out] merged_energies Merged energies array
92 * @param[in] cluster_count Cluster count
93 */
94 static void merge_peak_cluster(uint16_t start_idx,
95 uint16_t num_peaks,
96 const float *velocities,
97 const float *energies,
98 const uint16_t *peak_idxs,
99 float *merged_velocities,
100 float *merged_energies,
101 uint16_t cluster_count);
102
103
104 /**
105 * @brief Get max measurable distance for some PRF
106 *
107 * @param[in] prf Pulse repetition frequency (PRF)
108 * @return Max measurable distance in meters
109 */
110 static float max_measurable_dist(acc_config_prf_t prf);
111
112
113 /**
114 * Get profile by value
115 *
116 * @param[in] value Integer value that corresponds to an ACC_CONFIG_PROFILE enum
117 * @return An ACC_CONFIG_PROFILE enum
118 */
119 static acc_config_profile_t get_profile(uint16_t value);
120
121
122 void acc_algorithm_roll_and_push(float *data, uint16_t data_length, float element)
123 {
124 for (uint16_t i = 1U; i < data_length; i++)
125 {
126 data[i - 1U] = data[i];
127 }
128
129 data[data_length - 1U] = element;
130 }
131
132
133 void acc_algorithm_roll_and_push_f32_matrix(float *data, uint16_t rows, uint16_t cols, const float *column, bool pos_shift)
134 {
135 if (pos_shift)
136 {
137 for (uint16_t r = rows - 1U; r > 0U; r--)
138 {
139 for (uint16_t c = 0U; c < cols; c++)
140 {
141 data[(r * cols) + c] = data[((r - 1U) * cols) + c];
142 }
143 }
144
145 for (uint16_t c = 0U; c < cols; c++)
146 {
147 data[c] = column[c];
148 }
149 }
150 else
151 {
152 for (uint16_t r = 1U; r < rows; r++)
153 {
154 for (uint16_t c = 0U; c < cols; c++)
155 {
156 data[((r - 1U) * cols) + c] = data[(r * cols) + c];
157 }
158 }
159
160 for (uint16_t c = 0U; c < cols; c++)
161 {
162 data[((rows - 1U) * cols) + c] = column[c];
163 }
164 }
165 }
166
167
168 void acc_algorithm_roll_and_push_f32_complex_matrix(float complex *data, uint16_t rows, uint16_t cols, const float complex *column,
169 bool pos_shift)
170 {
171 if (pos_shift)
172 {
173 for (uint16_t r = rows - 1U; r > 0U; r--)
174 {
175 for (uint16_t c = 0U; c < cols; c++)
176 {
177 data[(r * cols) + c] = data[((r - 1U) * cols) + c];
178 }
179 }
180
181 for (uint16_t c = 0U; c < cols; c++)
182 {
183 data[c] = column[c];
184 }
185 }
186 else
187 {
188 for (uint16_t r = 1U; r < rows; r++)
189 {
190 for (uint16_t c = 0U; c < cols; c++)
191 {
192 data[((r - 1U) * cols) + c] = data[(r * cols) + c];
193 }
194 }
195
196 for (uint16_t c = 0U; c < cols; c++)
197 {
198 data[((rows - 1U) * cols) + c] = column[c];
199 }
200 }
201 }
202
203
204 void acc_algorithm_unwrap(float *data, uint16_t data_length)
205 {
206 for (uint16_t i = 1U; i < data_length; i++)
207 {
208 float diff = data[i] - data[i - 1U];
209
210 while ((diff > (float)M_PI) || (diff < -(float)M_PI))
211 {
212 if (diff > (float)M_PI)
213 {
214 data[i] -= 2.0f * (float)M_PI;
215 }
216 else
217 {
218 data[i] += 2.0f * (float)M_PI;
219 }
220
221 diff = data[i] - data[i - 1U];
222 }
223 }
224 }
225
226
227 uint16_t acc_algorithm_argmax(const float *data, uint16_t data_length)
228 {
229 uint16_t idx = 0U;
230 float max = data[idx];
231
232 for (uint16_t i = 1U; i < data_length; i++)
233 {
234 if (data[i] > max)
235 {
236 idx = i;
237 max = data[i];
238 }
239 }
240
241 return idx;
242 }
243
244
245 float acc_algorithm_interpolate_peaks(const float *y, const float *x)
246 {
247 float a = ((x[0] * (y[2] - y[1])) + (x[1] * (y[0] - y[2])) + (x[2] * (y[1] - y[0]))) / (
248 (x[0] - x[1]) * (x[0] - x[2]) * (x[1] - x[2]));
249 float b = ((y[1] - y[0]) / (x[1] - x[0])) - (a * (x[0] + x[1]));
250
251 return -b / (2.0f * a);
252 }
253
254
255 float acc_algorithm_interpolate_peaks_equidistant(const float *y, float x_start, float x_delta, uint16_t peak_idx)
256 {
257 float peak_offset = (y[peak_idx - 1U] - y[peak_idx + 1U]) / ((2.0f * y[peak_idx - 1U]) - (4.0f * y[peak_idx]) + (2.0f * y[peak_idx + 1U]));
258
259 return x_start + (((float)peak_idx + peak_offset) * x_delta);
260 }
261
262
263 void acc_algorithm_butter_lowpass(float freq, float fs, float *b, float *a)
264 {
265 float factor = (2.0f * freq) / fs;
266
267 // Values are centered around 0 to ensure an exactly real pole
268 float complex p[2];
269
270 p[0] = -cexpf(((-1.0f * (float)M_PI) / 4.0f) * I);
271 p[1] = -cexpf(((1.0f * (float)M_PI) / 4.0f) * I);
272
273 // Pre-wrap frequencies for digital filter design
274 factor = 4.0f * tanf(((float)M_PI * factor) / 2.0f);
275
276 // Scale all points radially from origin to shift cutoff frequency
277 p[0] = (float complex)factor * p[0];
278 p[1] = (float complex)factor * p[1];
279
280 // Cancel out gain change from frequency scaling
281 float k = factor * factor;
282
283 // Compensate for gain change
284 float complex real_four = (float complex)4.0f;
285 float complex z_prod = 1.0f;
286 float complex p_prod = (real_four - p[0]) * (real_four - p[1]);
287
288 k = k * crealf(acc_algorithm_cdiv(z_prod, p_prod));
289
290 // Bilinear transform the poles and zeros
291 p[0] = acc_algorithm_cdiv(real_four + p[0], real_four - p[0]);
292 p[1] = acc_algorithm_cdiv(real_four + p[1], real_four - p[1]);
293
294 // Any zeroes that were at infinity get moved to the Nyquist Frequency
295 float z_z[2] = {-1.0f, -1.0f};
296
297 // Calculate polynomial transfer functions
298 a[0] = -(p[0] + p[1]);
299 a[1] = p[0] * p[1];
300 b[0] = k;
301 b[1] = -k * (z_z[0] + z_z[1]);
302 b[2] = k * (z_z[0] * z_z[1]);
303 }
304
305
306 void acc_algorithm_butter_bandpass(float min_freq, float max_freq, float fs, float *b, float *a)
307 {
308 float min_f = (2.0f * min_freq) / fs;
309 float max_f = (2.0f * max_freq) / fs;
310
311 // Values are centered around 0 to ensure an exactly real pole
312 float complex p[2];
313
314 p[0] = -cexpf(((-1.0f * (float)M_PI) / 4.0f) * I);
315 p[1] = -cexpf(((1.0f * (float)M_PI) / 4.0f) * I);
316 float k = 1.0f;
317
318 // Pre-wrap frequencies for digital filter design
319 min_f = 4.0f * tanf(((float)M_PI * min_f) / 2.0f);
320 max_f = 4.0f * tanf(((float)M_PI * max_f) / 2.0f);
321
322 // Transform lowpass filter prototype to a bandspass filter
323 float bw = max_f - min_f;
324 float complex w0 = (float complex)sqrtf(min_f * max_f);
325
326 // Scale poles and zeros to desired bandwidth
327 float complex scale = (float complex)(bw / 2.0f);
328
329 p[0] = scale * p[0];
330 p[1] = scale * p[1];
331
332 // Duplicate poles and zeros and shift from baseband to +w0 and -w0
333 float complex p_bp[4];
334
335 w0 *= w0;
336 p_bp[0] = p[0] + csqrtf((p[0] * p[0]) - w0);
337 p_bp[1] = p[1] + csqrtf((p[1] * p[1]) - w0);
338 p_bp[2] = p[0] - csqrtf((p[0] * p[0]) - w0);
339 p_bp[3] = p[1] - csqrtf((p[1] * p[1]) - w0);
340
341 // Cancel out gain change from frequency scaling
342 float k_bp = k * bw * bw;
343
344 // Bilinear transform the poles and zeros
345 float complex real_four = (float complex)4.0f;
346 float complex p_z[4];
347
348 p_z[0] = acc_algorithm_cdiv(real_four + p_bp[0], real_four - p_bp[0]);
349 p_z[1] = acc_algorithm_cdiv(real_four + p_bp[1], real_four - p_bp[1]);
350 p_z[2] = acc_algorithm_cdiv(real_four + p_bp[2], real_four - p_bp[2]);
351 p_z[3] = acc_algorithm_cdiv(real_four + p_bp[3], real_four - p_bp[3]);
352
353 // Any zeroes that were at infinity get moved to the Nyquist Frequency
354 float z_z[4] = {1.0f, 1.0f, -1.0f, -1.0f};
355
356 // Compensate for gain change
357 float complex z_prod = 16.0f;
358 float complex p_prod = (real_four - p_bp[0]) * (real_four - p_bp[1]) * (real_four - p_bp[2]) *
359 (real_four - p_bp[3]);
360 float k_z = k_bp * crealf(acc_algorithm_cdiv(z_prod, p_prod));
361
362 // Calculate polynomial transfer functions
363 a[0] = -(p_z[0] + p_z[1] + p_z[2] + p_z[3]);
364 a[1] = (p_z[0] * p_z[1]) + (p_z[0] * p_z[2]) + (p_z[0] * p_z[3]) + (p_z[1] * p_z[2]) + (p_z[1] * p_z[3]) + (p_z[2] * p_z[3]);
365 a[2] = -((p_z[0] * p_z[1] * p_z[2]) + (p_z[0] * p_z[1] * p_z[3]) + (p_z[0] * p_z[2] * p_z[3]) + (p_z[1] * p_z[2] * p_z[3]));
366 a[3] = p_z[0] * p_z[1] * p_z[2] * p_z[3];
367 b[0] = k_z;
368 b[1] = -k_z * (z_z[0] + z_z[1] + z_z[2] + z_z[3]);
369 b[2] = k_z * ((z_z[0] * z_z[1]) + (z_z[0] * z_z[2]) + (z_z[0] * z_z[3]) + (z_z[1] * z_z[2]) + (z_z[1] * z_z[3]) + (z_z[2] * z_z[3]));
370 b[3] = -k_z * ((z_z[0] * z_z[1] * z_z[2]) + (z_z[0] * z_z[1] * z_z[3]) + (z_z[0] * z_z[2] * z_z[3]) + (z_z[1] * z_z[2] * z_z[3]));
371 b[4] = k_z * (z_z[0] * z_z[1] * z_z[2] * z_z[3]);
372 }
373
374
375 void acc_algorithm_lfilter(const float *b, const float *a, float *data, uint16_t data_length)
376 {
377 float filter_states[5] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
378
379 for (uint16_t i = 0U; i < data_length; i++)
380 {
381 filter_inplace_apply(i, b, a, filter_states, data);
382 }
383 }
384
385
386 void acc_algorithm_lfilter_matrix(const float *b, const float *a, float *data, uint16_t rows, uint16_t cols)
387 {
388 for (uint16_t i = 0U; i < rows; i++)
389 {
390 acc_algorithm_lfilter(b, a, &data[i * cols], cols);
391 }
392 }
393
394
395 void acc_algorithm_apply_filter(const float *a, const float *filt_data, uint16_t filt_rows, uint16_t filt_cols, const float *b,
396 const float *data, uint16_t data_rows, uint16_t data_cols, float *output, uint16_t output_length)
397 {
398 for (uint16_t i = 0U; i < output_length; i++)
399 {
400 output[i] = 0.0f;
401
402 for (uint16_t r = 0U; r < filt_rows; r++)
403 {
404 output[i] -= a[r] * filt_data[i + (r * filt_cols)];
405 }
406
407 for (uint16_t r = 0U; r < data_rows; r++)
408 {
409 output[i] += b[r] * data[i + (r * data_cols)];
410 }
411 }
412 }
413
414
415 void acc_algorithm_apply_filter_complex(const float *a, const float complex *filt_data, uint16_t filt_rows, uint16_t filt_cols,
416 const float *b, const float complex *data, uint16_t data_rows, uint16_t data_cols,
417 float complex *output, uint16_t output_length)
418 {
419 for (uint16_t i = 0U; i < output_length; i++)
420 {
421 float real = 0.0f;
422 float imag = 0.0f;
423
424 for (uint16_t r = 0U; r < filt_rows; r++)
425 {
426 real -= a[r] * crealf(filt_data[i + (r * filt_cols)]);
427 imag -= a[r] * cimagf(filt_data[i + (r * filt_cols)]);
428 }
429
430 for (uint16_t r = 0U; r < data_rows; r++)
431 {
432 real += b[r] * crealf(data[i + (r * data_cols)]);
433 imag += b[r] * cimagf(data[i + (r * data_cols)]);
434 }
435
436 output[i] = real + (imag * I);
437 }
438 }
439
440
441 void acc_algorithm_mean_sweep(const acc_int16_complex_t *frame, uint16_t num_points, uint16_t sweeps_per_frame, uint16_t start_point,
442 uint16_t end_point, float complex *sweep)
443 {
444 for (uint16_t n = start_point; n < end_point; n++)
445 {
446 float real = 0.0f;
447 float imag = 0.0f;
448 for (uint16_t s = 0U; s < sweeps_per_frame; s++)
449 {
450 acc_int16_complex_t sample = frame[n + (s * num_points)];
451 real += (float)sample.real;
452 imag += (float)sample.imag;
453 }
454
455 real /= (float)sweeps_per_frame;
456 imag /= (float)sweeps_per_frame;
457
458 sweep[n - start_point] = real + (imag * I);
459 }
460 }
461
462
463 void acc_algorithm_rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride)
464 {
465 small_rfft(data, data_length, length_shift - 1U, output, stride);
466
467 small_rfft_real_symmetry_conversion(output, length_shift - 1U, stride);
468
469 output[(((uint16_t)1U) << (length_shift - 1U)) * stride] = cimagf(output[0]);
470 output[0] = crealf(output[0]);
471 }
472
473
474 void acc_algorithm_rfft_matrix(const float *data, uint16_t rows, uint16_t cols, uint16_t length_shift, float complex *output, uint16_t axis)
475 {
476 uint16_t full_cols = ((uint16_t)1U) << length_shift;
477
478 if (axis == 1U)
479 {
480 uint16_t output_cols = (full_cols / 2U) + 1U;
481 for (uint16_t i = 0U; i < rows; i++)
482 {
483 acc_algorithm_rfft(&data[i * cols], cols, length_shift, &output[i * output_cols], 1U);
484 }
485 }
486 else if (axis == 0U)
487 {
488 for (uint16_t i = 0U; i < cols; i++)
489 {
490 acc_algorithm_rfft(&data[i], rows, length_shift, &output[i], cols);
491 }
492 }
493 else
494 {
495 // Do nothing
496 }
497 }
498
499
500 void acc_algorithm_fft(const float complex *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride)
501 {
502 small_fft(data, data_length, length_shift, output, stride);
503 }
504
505
506 void acc_algorithm_fft_matrix(const float complex *data, uint16_t rows, uint16_t cols, uint16_t length_shift, float complex *output, uint16_t axis)
507 {
508 uint16_t full_cols = ((uint16_t)1U) << length_shift;
509
510 if (axis == 1U)
511 {
512 uint16_t output_cols = full_cols;
513 for (uint16_t i = 0U; i < rows; i++)
514 {
515 acc_algorithm_fft(&data[i * cols], cols, length_shift, &output[i * output_cols], 1U);
516 }
517 }
518 else if (axis == 0U)
519 {
520 for (uint16_t i = 0U; i < cols; i++)
521 {
522 acc_algorithm_fft(&data[i], rows, length_shift, &output[i], cols);
523 }
524 }
525 else
526 {
527 // Do nothing
528 }
529 }
530
531
532 float acc_algorithm_fftfreq_delta(uint16_t n, float d)
533 {
534 float df = NAN;
535
536 if ((n > 0U) && (d > 0.0f))
537 {
538 df = 1.0f / ((float)n * d);
539 }
540
541 return df;
542 }
543
544
545 void acc_algorithm_rfftfreq(uint16_t n, float d, float *freqs)
546 {
547 uint16_t n_freqs = (n / 2U) + 1U;
548 float df = acc_algorithm_fftfreq_delta(n, d);
549
550 for (uint16_t i = 0U; i < n_freqs; i++)
551 {
552 freqs[i] = (float)i * df;
553 }
554 }
555
556
557 void acc_algorithm_fftfreq(uint16_t n, float d, float *freqs)
558 {
559 float df = acc_algorithm_fftfreq_delta(n, d);
560 uint16_t mid = n / 2U;
561
562 for (uint16_t i = 0U; i < mid; i++)
563 {
564 freqs[i] = (float)i * df;
565 }
566
567 int32_t n_int32 = (int32_t)n;
568
569 for (int32_t i = (int32_t)mid; i < n_int32; i++)
570 {
571 int32_t diff = i - n_int32;
572 freqs[i] = (float)diff * df;
573 }
574 }
575
576
577 float acc_algorithm_exp_smoothing_coefficient(float fs, float tc)
578 {
579 float res = NAN;
580
581 if ((fs != 0.0f) && (tc != 0.0f))
582 {
583 float dt = 1.0f / fs;
584
585 res = expf(-dt / tc);
586 }
587
588 return res;
589 }
590
591
592 float complex acc_algorithm_cdiv(float complex num, float complex denom)
593 {
594 double a = (double)crealf(denom);
595 double b = (double)cimagf(denom);
596 double c = (double)crealf(num);
597 double d = (double)cimagf(num);
598
599 float real = (float)(((c * a) + (b * d)) / ((a * a) + (b * b)));
600 float imag = (float)(((a * d) - (c * b)) / ((a * a) + (b * b)));
601
602 return real + (imag * I);
603 }
604
605
606 void acc_algorithm_hamming(uint16_t n, float *window)
607 {
608 const float a = 0.54f;
609 const float b = 0.46f;
610 const float factor = (2.0f * (float)M_PI) / ((float)n - 1.0f);
611
612 for (uint16_t i = 0U; i < n; i++)
613 {
614 window[i] = a - (b * cosf((float)i * factor));
615 }
616 }
617
618
619 void acc_algorithm_hann(uint16_t n, float *window)
620 {
621 const float a = 0.5f;
622
623 const float factor = (2.0f * (float)M_PI) / (float)n;
624
625 for (uint16_t i = 0U; i < n; i++)
626 {
627 window[i] = a - (a * cosf((float)i * factor));
628 }
629 }
630
631
632 float acc_algorithm_get_fwhm(acc_config_profile_t profile)
633 {
634 float fwhm;
635
636 switch (profile)
637 {
638 case ACC_CONFIG_PROFILE_1:
639 fwhm = 0.04f;
640 break;
641 case ACC_CONFIG_PROFILE_2:
642 fwhm = 0.07f;
643 break;
644 case ACC_CONFIG_PROFILE_3:
645 fwhm = 0.14f;
646 break;
647 case ACC_CONFIG_PROFILE_4:
648 fwhm = 0.19f;
649 break;
650 case ACC_CONFIG_PROFILE_5:
651 fwhm = 0.32f;
652 break;
653 default:
654 fwhm = 0.0f;
655 break;
656 }
657
658 return fwhm;
659 }
660
661
662 void acc_algorithm_double_buffering_frame_filter(acc_int16_complex_t *frame,
663 const uint16_t sweeps_per_frame,
664 const uint16_t num_points,
665 int32_t *work_buffer)
666 {
667 int32_t first_diff_r[2U];
668 int32_t first_diff_i[2U];
669
670 if (sweeps_per_frame >= 32U)
671 {
672 for (uint16_t point = 0U; point < num_points; point++)
673 {
674 int32_t abs_mad_sum = 0;
675
676 for (uint16_t sweep = 0U; sweep < (sweeps_per_frame - 2U); sweep++)
677 {
678 /* Calculate 1st discrete difference */
679 for (uint16_t idx = 0U; idx < 2U; idx++)
680 {
681 uint16_t sweep_idx = (sweep + idx) * num_points;
682 uint16_t next_sweep_idx = (sweep + idx + 1U) * num_points;
683 int32_t diff1_r = frame[(next_sweep_idx + point)].real;
684 int32_t diff1_i = frame[(next_sweep_idx + point)].imag;
685 int32_t diff2_r = frame[(sweep_idx + point)].real;
686 int32_t diff2_i = frame[(sweep_idx + point)].imag;
687 first_diff_r[idx] = diff1_r - diff2_r;
688 first_diff_i[idx] = diff1_i - diff2_i;
689 }
690
691 /* Calculate 2nd discrete difference */
692 int32_t second_diff_r = first_diff_r[1] - first_diff_r[0U];
693 int32_t second_diff_i = first_diff_i[1] - first_diff_i[0U];
694
695 /* Estimating magnitude using: abs(real) + abs(imag) */
696 int32_t abs_r = (second_diff_r < 0) ? -second_diff_r : second_diff_r;
697 int32_t abs_i = (second_diff_i < 0) ? -second_diff_i : second_diff_i;
698
699 work_buffer[sweep] = abs_r + abs_i;
700
701 /* Sum mean absolute deviation */
702 abs_mad_sum += work_buffer[sweep];
703 }
704
705 /* Mean absolute deviation */
706 int32_t nof_of_abs = (int32_t)sweeps_per_frame;
707 nof_of_abs = nof_of_abs - 2;
708 int32_t diff_mad = abs_mad_sum / nof_of_abs;
709 int32_t threshold = DOUBLE_BUFFERING_MEAN_ABS_DEV_OUTLIER_TH * diff_mad;
710
711 for (uint16_t sweep = 1U; sweep < (sweeps_per_frame - 1U); sweep++)
712 {
713 if (work_buffer[sweep - 1U] <= threshold)
714 {
715 continue;
716 }
717
718 if (sweep == 1U)
719 {
720 /* First Sweep */
721 double_buffering_median_filter(frame, num_points, 1U, point, 0U);
722 }
723 else if (sweep == (sweeps_per_frame - 2U))
724 {
725 /* Last Sweep */
726 double_buffering_median_filter(frame, num_points, sweeps_per_frame - 2U, point, sweeps_per_frame - 4U - 1U);
727 }
728 else
729 {
730 double_buffering_interpolate(frame, sweeps_per_frame, num_points, sweep, point);
731 }
732 }
733 }
734 }
735 }
736
737
738 void acc_algorithm_fftshift_matrix(float *data, uint16_t rows, uint16_t cols)
739 {
740 for (uint16_t i = 0U; i < cols; i++)
741 {
742 acc_algorithm_fftshift(&(data[i]), rows, cols);
743 }
744 }
745
746
747 void acc_algorithm_fftshift(float *data, uint16_t data_length, uint16_t stride)
748 {
749 uint16_t half_data_length = (data_length + 1U) / 2U;
750
751 for (uint16_t i = 0U; i < half_data_length; i++)
752 {
753 float x = data[0];
754
755 for (uint16_t j = 0U; j < (data_length - 1U); j++)
756 {
757 data[j * stride] = data[(j * stride) + stride];
758 }
759
760 data[(data_length * stride) - stride] = x;
761 }
762 }
763
764
765 void acc_algorithm_welch_matrix(const float complex *data,
766 uint16_t rows,
767 uint16_t cols,
768 uint16_t segment_length,
769 float complex *data_buffer,
770 float complex *fft_out,
771 float *psds,
772 const float *window,
773 uint16_t length_shift,
774 float fs)
775 {
776 for (uint16_t i = 0U; i < cols; i++)
777 {
778 acc_algorithm_welch(&(data[i]), rows, segment_length, data_buffer, fft_out, &(psds[i]), window, length_shift, fs,
779 cols);
780 }
781 }
782
783
784 void acc_algorithm_welch(const float complex *data,
785 uint16_t data_length,
786 uint16_t segment_length,
787 float complex *data_buffer,
788 float complex *fft_out,
789 float *psd,
790 const float *window,
791 uint16_t length_shift,
792 float fs,
793 uint16_t stride)
794 {
795 uint16_t num_segments = data_length / segment_length;
796 float scale = 0.0f;
797
798 for (uint16_t i = 0U; i < num_segments; i++)
799 {
800 scale = 0.0f;
801 float complex mean = 0.0f;
802
803 for (uint16_t j = 0U; j < segment_length; j++)
804 {
805 mean += data[(i * segment_length * stride) + (j * stride)];
806 }
807
808 float complex adj_mean_real = crealf(mean) / (float)segment_length;
809 float complex adj_mean_imag = cimagf(mean) / (float)segment_length;
810 mean = adj_mean_real + (adj_mean_imag * I);
811
812 for (uint16_t j = 0U; j < segment_length; j++)
813 {
814 data_buffer[j] = data[(i * segment_length * stride) + (j * stride)] - mean;
815
816 float complex adj_buf_real = crealf(data_buffer[j]) * window[j];
817 float complex adj_buf_imag = cimagf(data_buffer[j]) * window[j];
818 data_buffer[j] = adj_buf_real + (adj_buf_imag * I);
819
820 scale += window[j] * window[j];
821 }
822
823 acc_algorithm_fft(data_buffer, segment_length, length_shift, fft_out, 1U);
824
825 for (uint16_t j = 0U; j < segment_length; j++)
826 {
827 psd[j * stride] += cabsf(fft_out[j]) *
828 cabsf(fft_out[j]);
829 }
830 }
831
832 if (scale != 0.0f)
833 {
834 scale = 1.0f / (scale * fs * (float)num_segments);
835 }
836
837 for (uint16_t i = 0U; i < segment_length; i++)
838 {
839 psd[i * stride] *= scale;
840 }
841 }
842
843
844 float acc_algorithm_calculate_cfar(const float *data,
845 uint16_t data_length,
846 uint16_t middle_idx,
847 uint16_t cfar_win_length,
848 uint16_t cfar_guard_length,
849 float cfar_sensitivity,
850 uint16_t idx)
851 {
852 uint16_t margin = cfar_win_length + cfar_guard_length;
853 uint16_t half_sweep_len_without_margin = (uint16_t)rint(((double)data_length / 2.0) - (double)margin);
854
855 float min = INFINITY;
856
857 for (uint16_t i = 0U; i < data_length; i++)
858 {
859 min = fminf(data[i], min);
860 }
861
862 float sum = 0.0f;
863
864 if (idx <= margin)
865 {
866 for (uint16_t j = 0U; j < cfar_win_length; j++)
867 {
868 sum += data[j];
869 }
870 }
871
872 if ((idx > margin) && (idx < middle_idx))
873 {
874 for (uint16_t j = 0U; j < cfar_win_length; j++)
875 {
876 sum += data[j + (idx - margin)];
877 }
878 }
879
880 if ((idx >= middle_idx) && (idx < (data_length - margin - 1U)))
881 {
882 for (uint16_t j = 0U; j < cfar_win_length; j++)
883 {
884 sum += data[data_length - half_sweep_len_without_margin - j + idx - middle_idx];
885 }
886 }
887
888 if (idx >= (data_length - margin - 1U))
889 {
890 for (uint16_t j = 0U; j < cfar_win_length; j++)
891 {
892 sum += data[data_length - j - 1U];
893 }
894 }
895
896 return ((sum / (float)cfar_win_length) + min) / cfar_sensitivity;
897 }
898
899
900 uint16_t acc_algorithm_get_distance_idx(const float *data, uint16_t cols, uint16_t rows, uint16_t middle_idx, uint16_t half_slow_zone)
901 {
902 float max = -INFINITY;
903
904 uint16_t idx = 0U;
905
906 for (uint16_t i = 0U; i < rows; i++)
907 {
908 if ((i < (middle_idx + half_slow_zone)) && (i >= (middle_idx - half_slow_zone)))
909 {
910 continue;
911 }
912
913 for (uint16_t j = 0U; j < cols; j++)
914 {
915 if (data[(i * cols) + j] > max)
916 {
917 max = data[(i * cols) + j];
918 idx = j;
919 }
920 }
921 }
922
923 return idx;
924 }
925
926
927 float acc_algorithm_get_peak_velocity(const float *velocities,
928 const float *energies,
929 const uint16_t *peak_idxs,
930 uint16_t num_peaks,
931 float limit)
932 {
933 float slow_vs = 0.0f;
934 float valid_vs = 0.0f;
935 bool has_valid = false;
936 float biggest_energy_slow = -INFINITY;
937 float biggest_energy_valid = -INFINITY;
938
939 for (uint16_t i = 0U; i < num_peaks; i++)
940 {
941 uint16_t idx = (peak_idxs != NULL) ? peak_idxs[i] : i;
942
943 if (energies[idx] > biggest_energy_slow)
944 {
945 if (fabsf(velocities[idx]) < limit)
946 {
947 slow_vs = velocities[idx];
948 biggest_energy_slow = energies[idx];
949 }
950 }
951
952 if (energies[i] > biggest_energy_valid)
953 {
954 if (fabsf(velocities[idx]) >= limit)
955 {
956 valid_vs = velocities[idx];
957 biggest_energy_valid = energies[idx];
958 has_valid = true;
959 }
960 }
961 }
962
963 return has_valid ? valid_vs : slow_vs;
964 }
965
966
967 bool acc_algorithm_merge_peaks(float max_peak_separation,
968 const float *velocities,
969 const float *energies,
970 const uint16_t *peak_idxs,
971 uint16_t num_peaks,
972 float *merged_velocities,
973 float *merged_energies,
974 uint16_t merged_peaks_length,
975 uint16_t *num_merged_peaks)
976 {
977 bool status = true;
978 uint16_t cluster_count = 0U;
979 uint16_t cluster_start_idx = 0U;
980
981 if (num_peaks > 1U)
982 {
983 for (uint16_t i = 0U; i < (num_peaks - 1U); i++)
984 {
985 uint16_t current_idx = peak_idxs[i];
986 uint16_t next_idx = peak_idxs[i + 1U];
987
988 uint16_t num_peaks_in_cluster = i - cluster_start_idx + 1U;
989
990 bool current_peak_is_in_cluster = (velocities[next_idx] - velocities[current_idx]) < max_peak_separation;
991
992 if (current_peak_is_in_cluster)
993 {
994 continue;
995 }
996
997 status = cluster_count < merged_peaks_length;
998
999 if (status)
1000 {
1001 merge_peak_cluster(cluster_start_idx, num_peaks_in_cluster, velocities, energies, peak_idxs,
1002 merged_velocities, merged_energies, cluster_count);
1003 }
1004
1005 if (!status)
1006 {
1007 break;
1008 }
1009
1010 cluster_count++;
1011 cluster_start_idx = i + 1U;
1012 }
1013 }
1014
1015 bool last_cluster_not_merged = cluster_start_idx < num_peaks;
1016
1017 if (status && last_cluster_not_merged)
1018 {
1019 status = cluster_count < merged_peaks_length;
1020
1021 if (status)
1022 {
1023 merge_peak_cluster(cluster_start_idx, num_peaks - cluster_start_idx, velocities,
1024 energies, peak_idxs,
1025 merged_velocities, merged_energies, cluster_count);
1026 }
1027
1028 if (status)
1029 {
1030 cluster_count++;
1031 }
1032 }
1033
1034 if (status)
1035 {
1036 *num_merged_peaks = cluster_count;
1037 }
1038
1039 return status;
1040 }
1041
1042
1043 float acc_algorithm_get_distance_m(uint16_t step_length, uint16_t start_point, float base_step_length_m, uint16_t idx)
1044 {
1045 uint16_t steps = (idx * step_length) + start_point;
1046
1047 return (float)steps * base_step_length_m;
1048 }
1049
1050
1051 acc_config_profile_t acc_algorithm_select_profile(int32_t start_point, float base_step_length)
1052 {
1053 acc_config_profile_t profile = ACC_CONFIG_PROFILE_1;
1054
1055 // Array with minimum start point for each profile without interference from direct leakage
1056 // Profile 1 has special case -1 to default to this if no other work
1057 float MIN_DIST_M[5] = {-1.0f, 0.07f * 2.0f, 0.14f * 2.0f, 0.19f * 2.0f, 0.32f * 2.0f};
1058
1059 for (uint16_t i = (uint16_t)ACC_CONFIG_PROFILE_1; i <= (uint16_t)ACC_CONFIG_PROFILE_5; i++)
1060 {
1061 if ((MIN_DIST_M[i - 1U] == -1.0f) || (MIN_DIST_M[i - 1U] <= ((float)start_point * base_step_length)))
1062 {
1063 profile = get_profile(i);
1064 }
1065 }
1066
1067 return profile;
1068 }
1069
1070
1071 acc_config_prf_t acc_algorithm_select_prf(int16_t breakpoint, acc_config_profile_t profile, float base_step_length)
1072 {
1073 float breakpoint_p = (float)breakpoint;
1074 float breakpoint_m = breakpoint_p * base_step_length;
1075 acc_config_prf_t prf;
1076
1077 if ((breakpoint_m < max_measurable_dist(ACC_CONFIG_PRF_19_5_MHZ))
1078 && (profile == ACC_CONFIG_PROFILE_1))
1079 {
1080 prf = ACC_CONFIG_PRF_19_5_MHZ;
1081 }
1082 else if (breakpoint_m < max_measurable_dist(ACC_CONFIG_PRF_15_6_MHZ))
1083 {
1084 prf = ACC_CONFIG_PRF_15_6_MHZ;
1085 }
1086 else if (breakpoint_m < max_measurable_dist(ACC_CONFIG_PRF_13_0_MHZ))
1087 {
1088 prf = ACC_CONFIG_PRF_13_0_MHZ;
1089 }
1090 else if (breakpoint_m < max_measurable_dist(ACC_CONFIG_PRF_8_7_MHZ))
1091 {
1092 prf = ACC_CONFIG_PRF_8_7_MHZ;
1093 }
1094 else if (breakpoint_m < max_measurable_dist(ACC_CONFIG_PRF_6_5_MHZ))
1095 {
1096 prf = ACC_CONFIG_PRF_6_5_MHZ;
1097 }
1098 else
1099 {
1100 prf = ACC_CONFIG_PRF_5_2_MHZ;
1101 }
1102
1103 return prf;
1104 }
1105
1106
1107 bool acc_algorithm_find_peaks(const float *abs_sweep, const uint16_t data_length, const uint32_t *threshold_check, uint16_t *peak_idxs,
1108 uint16_t peak_idxs_length, uint16_t *num_peaks)
1109 {
1110 bool success = true;
1111 uint16_t found_peaks = 0U;
1112 uint16_t i = 1U;
1113
1114 while (i < data_length)
1115 {
1116 /*
1117 * Find a peak candidate.
1118 */
1119
1120 if (!acc_alg_basic_utils_is_bit_set_bitarray_uint32(threshold_check, ((size_t)i - 1U)))
1121 {
1122 i++;
1123 continue;
1124 }
1125
1126 if (!acc_alg_basic_utils_is_bit_set_bitarray_uint32(threshold_check, i))
1127 {
1128 i += 2U;
1129 continue;
1130 }
1131
1132 if (abs_sweep[i - 1U] >= abs_sweep[i])
1133 {
1134 i++;
1135 continue;
1136 }
1137
1138 /*
1139 * Peak candidate found at abs_sweep[d].
1140 *
1141 * We have two consecutive peaks above threshold: abs_sweep[d-1] and
1142 * abs_sweep[d], where abs_sweep[d-1] < abs_sweep[d].
1143 *
1144 * Now search for an upper bound where abs_sweep[upper] < abs_sweep[d],
1145 * but still above threshold.
1146 */
1147
1148 uint16_t d_upper = i + 1U;
1149 bool upper_done = false;
1150
1151 while (!upper_done)
1152 {
1153 if (d_upper >= (data_length - 1U))
1154 {
1155 upper_done = true;
1156 }
1157 else if (!acc_alg_basic_utils_is_bit_set_bitarray_uint32(threshold_check, d_upper))
1158 {
1159 upper_done = true;
1160 }
1161 else if (abs_sweep[d_upper] > abs_sweep[i])
1162 {
1163 // Growing slope; reset the peak candidate to the new larger value.
1164 i = d_upper;
1165 d_upper++;
1166 }
1167 else if (abs_sweep[d_upper] < abs_sweep[i])
1168 {
1169 /*
1170 * Ensure that the value after a peak candidate (i) isn't below threshold; e.g:
1171 *
1172 * abs_sweep = [1, 2, 3, 4, 2, 2]
1173 * threshold = [2, 2, 2, 2, 2, 2]
1174 *
1175 * Candidate: abs_sweep[i] = 4
1176 */
1177 if (acc_alg_basic_utils_is_bit_set_bitarray_uint32(threshold_check, ((size_t)i + 1U)))
1178 {
1179 if (found_peaks < peak_idxs_length)
1180 {
1181 peak_idxs[found_peaks] = i;
1182 found_peaks++;
1183 }
1184 else
1185 {
1186 success = false;
1187 }
1188 }
1189
1190 upper_done = true;
1191 }
1192 else
1193 {
1194 d_upper++;
1195 }
1196 }
1197
1198 i = d_upper;
1199 }
1200
1201 *num_peaks = found_peaks;
1202
1203 return success;
1204 }
1205
1206
1207 static void filter_inplace_apply(uint16_t sample_idx,
1208 const float *b,
1209 const float *a,
1210 float state[5],
1211 float *data)
1212 {
1213 float x = data[sample_idx];
1214 float y;
1215
1216 y = state[0] + (b[0] * x);
1217
1218 state[0] = state[1] + (b[1] * x) - (a[0] * y);
1219 state[1] = state[2] + (b[2] * x) - (a[1] * y);
1220 state[2] = state[3] + (b[3] * x) - (a[2] * y);
1221 state[3] = (b[4] * x) - (a[3] * y);
1222
1223 data[sample_idx] = y;
1224 }
1225
1226
1227 static float complex get_data_padded_f32_to_f32_complex(const float *data, uint16_t data_length, uint16_t index, uint16_t stride)
1228 {
1229 float real = 0.0f;
1230 float imag = 0.0f;
1231 uint16_t i = index * 2U;
1232
1233 if (i < data_length)
1234 {
1235 real = data[i * stride];
1236 }
1237
1238 i++;
1239
1240 if (i < data_length)
1241 {
1242 imag = data[i * stride];
1243 }
1244
1245 return real + (imag * I);
1246 }
1247
1248
1249 static float complex get_data_padded_f32_complex(const float complex *data, uint16_t data_length, uint16_t index, uint16_t stride)
1250 {
1251 float complex res = 0.0f + 0.0f * I;
1252
1253 if (index < data_length)
1254 {
1255 res = data[index * stride];
1256 }
1257
1258 return res;
1259 }
1260
1261
1262 static void small_rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride)
1263 {
1264 uint16_t full_data_length = ((uint16_t)1U) << length_shift;
1265
1266 if (length_shift == 0U)
1267 {
1268 // Trivial 1-element FFT
1269 output[0] = get_data_padded_f32_to_f32_complex(data, data_length, 0U, stride);
1270 }
1271 else if (length_shift == 1U)
1272 {
1273 // 2-element FFT
1274 output[0] = get_data_padded_f32_to_f32_complex(data, data_length, 0U, stride) + get_data_padded_f32_to_f32_complex(data,
1275 data_length, 1U,
1276 stride);
1277 output[1U * stride] = get_data_padded_f32_to_f32_complex(data, data_length, 0U, stride) - get_data_padded_f32_to_f32_complex(data,
1278 data_length, 1U,
1279 stride);
1280 }
1281 else
1282 {
1283 // Perform element reordering
1284 uint16_t reverse_i = 0U;
1285 for (uint16_t i = 0U; i < full_data_length; i++)
1286 {
1287 if (i < reverse_i)
1288 {
1289 float complex tmp = get_data_padded_f32_to_f32_complex(data, data_length, i, stride);
1290 output[i*stride] = get_data_padded_f32_to_f32_complex(data, data_length, reverse_i, stride);
1291 output[reverse_i*stride] = tmp;
1292 }
1293 else if (i == reverse_i)
1294 {
1295 output[i*stride] = get_data_padded_f32_to_f32_complex(data, data_length, i, stride);
1296 }
1297 else
1298 {
1299 // Do nothing
1300 }
1301
1302 uint16_t bit = full_data_length >> 1U;
1303 while ((bit & reverse_i) != 0U)
1304 {
1305 reverse_i &= ~bit;
1306 bit >>= 1U;
1307 }
1308 reverse_i |= bit;
1309 }
1310
1311 small_fft_transformation(length_shift, output, stride);
1312 }
1313 }
1314
1315
1316 static void small_fft(const float complex *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride)
1317 {
1318 uint16_t full_data_length = ((uint16_t)1U) << length_shift;
1319
1320 if (length_shift == 0U)
1321 {
1322 // Trivial 1-element FFT
1323 output[0] = get_data_padded_f32_complex(data, data_length, 0U, stride);
1324 }
1325 else if (length_shift == 1U)
1326 {
1327 // 2-element FFT
1328 output[0] = get_data_padded_f32_complex(data, data_length, 0U, stride) + get_data_padded_f32_complex(data, data_length, 1U,
1329 stride);
1330 output[1U * stride] = get_data_padded_f32_complex(data, data_length, 0U, stride) - get_data_padded_f32_complex(data, data_length, 1U,
1331 stride);
1332 }
1333 else
1334 {
1335 // Perform element reordering
1336 uint16_t reverse_i = 0U;
1337 for (uint16_t i = 0U; i < full_data_length; i++)
1338 {
1339 if (i < reverse_i)
1340 {
1341 float complex tmp = get_data_padded_f32_complex(data, data_length, i, stride);
1342 output[i*stride] = get_data_padded_f32_complex(data, data_length, reverse_i, stride);
1343 output[reverse_i*stride] = tmp;
1344 }
1345 else if (i == reverse_i)
1346 {
1347 output[i*stride] = get_data_padded_f32_complex(data, data_length, i, stride);
1348 }
1349 else
1350 {
1351 // Do nothing
1352 }
1353
1354 uint16_t bit = full_data_length >> 1U;
1355 while ((bit & reverse_i) != 0U)
1356 {
1357 reverse_i &= ~bit;
1358 bit >>= 1U;
1359 }
1360 reverse_i |= bit;
1361 }
1362
1363 small_fft_transformation(length_shift, output, stride);
1364 }
1365 }
1366
1367
1368 static void small_fft_transformation(uint16_t length_shift, float complex *output, uint16_t stride)
1369 {
1370 uint16_t full_data_length = ((uint16_t)1U) << length_shift;
1371
1372 // Perform 4-element base transformations
1373 for (uint16_t i = 0U; i < full_data_length; i += 4U)
1374 {
1375 float complex s0 = output[i * stride] + output[(i + 1U) * stride];
1376 float complex d0 = output[i * stride] - output[(i + 1U) * stride];
1377 float complex s1 = output[(i + 2U) * stride] + output[(i + 3U) * stride];
1378 float complex d1 = output[(i + 2U) * stride] - output[(i + 3U) * stride];
1379
1380 d1 = cimagf(d1) - (I * crealf(d1)); // d1 = -I*d1;
1381
1382 output[(i + 0U) * stride] = s0 + s1;
1383 output[(i + 2U) * stride] = s0 - s1;
1384 output[(i + 1U) * stride] = d0 + d1;
1385 output[(i + 3U) * stride] = d0 - d1;
1386 }
1387
1388 // Main part of the FFT computation
1389 uint16_t block_length = 4U;
1390 float complex phase_incr = -I;
1391
1392 while (block_length < full_data_length)
1393 {
1394 // Update the phase_incr unit vector to have half phase angle
1395 phase_incr = (phase_incr + 1.0f) / cabsf(phase_incr + 1.0f);
1396
1397 float complex phase = 1.0f;
1398 for (uint16_t m = 0U; m < block_length; m++)
1399 {
1400 for (uint16_t i = m; i < full_data_length; i += block_length << 1U)
1401 {
1402 float complex delta = output[(i + block_length) * stride] * phase;
1403
1404 output[(i + block_length) * stride] = output[i*stride] - delta;
1405
1406 output[i*stride] += delta;
1407 }
1408
1409 // This phase increment is the leading error source for large transforms
1410 phase = phase * phase_incr;
1411 }
1412
1413 block_length <<= 1U;
1414 }
1415 }
1416
1417
1418 static void small_rfft_real_symmetry_conversion(float complex *output, uint16_t length_shift, uint16_t stride)
1419 {
1420 uint16_t full_data_length = ((uint16_t)1U) << length_shift;
1421
1422 output[0] = (1.0f + (1.0f * I)) * conjf(output[0]);
1423
1424 if (length_shift > 0U)
1425 {
1426 float complex phase_incr = I;
1427 float complex z1_factor = 0.5f * phase_incr;
1428
1429 for (uint16_t i = 1U; i < length_shift; i++)
1430 {
1431 phase_incr = (phase_incr + 1.0f) / cabsf(phase_incr + 1.0f);
1432 }
1433
1434 uint16_t mid = full_data_length / 2U;
1435 for (uint16_t i = 1U; i < mid; i++)
1436 {
1437 float complex t;
1438 float complex z0 = output[i * stride];
1439 float complex z1 = output[(full_data_length - i) * stride];
1440
1441 t = z0 + conjf(z1);
1442 z1 = conjf(z0) - z1;
1443 z0 = t;
1444
1445 z0 *= 0.5f;
1446 z1_factor = z1_factor * phase_incr;
1447 z1 = z1 * z1_factor;
1448
1449 t = z0 + conjf(z1);
1450 z1 = conjf(z0) - z1;
1451 z0 = t;
1452
1453 output[i * stride] = z0;
1454 output[(full_data_length - i) * stride] = z1;
1455 }
1456
1457 output[mid * stride] = conjf(output[mid * stride]);
1458 }
1459 }
1460
1461
1462 static void double_buffering_median_filter(acc_int16_complex_t *frame,
1463 const uint16_t num_points,
1464 const uint16_t sweep,
1465 const uint16_t point,
1466 const uint16_t median_start_sweep)
1467 {
1468 /* Get the complex median value over an array of length 4 */
1469 int32_t point_r[4U];
1470 int32_t point_i[4U];
1471 int32_t point_abs[4U];
1472
1473 /* Calculate abs value */
1474 for (uint16_t idx = 0U; idx < 4U; idx++)
1475 {
1476 point_r[idx] = frame[((median_start_sweep + idx)*num_points) + point].real;
1477 point_i[idx] = frame[((median_start_sweep + idx)*num_points) + point].imag;
1478 point_abs[idx] = (point_r[idx]*point_r[idx]) + (point_i[idx]*point_i[idx]);
1479 }
1480
1481 uint16_t high_index = 0U;
1482 uint16_t low_index = 0U;
1483 int32_t high_val = INT32_MIN;
1484 int32_t low_val = INT32_MAX;
1485
1486 /* Find highest/lowest abs index */
1487 for (uint16_t idx = 0; idx < 4U; idx++)
1488 {
1489 if (point_abs[idx] > high_val)
1490 {
1491 high_val = point_abs[idx];
1492 high_index = idx;
1493 }
1494
1495 if (point_abs[idx] < low_val)
1496 {
1497 low_val = point_abs[idx];
1498 low_index = idx;
1499 }
1500 }
1501
1502 /* Clear highest and lowest */
1503 point_r[high_index] = 0;
1504 point_i[high_index] = 0;
1505 point_r[low_index] = 0;
1506 point_i[low_index] = 0;
1507
1508 int32_t median_real = 0;
1509 int32_t median_imag = 0;
1510
1511 /* Sum complex points */
1512 for (uint16_t idx = 0U; idx < 4U; idx++)
1513 {
1514 median_real += point_r[idx];
1515 median_imag += point_i[idx];
1516 }
1517
1518 /* Update frame with median filtered value */
1519 median_real = median_real / 2;
1520 median_imag = median_imag / 2;
1521
1522 frame[(sweep*num_points) + point].real = (int16_t)median_real;
1523 frame[(sweep*num_points) + point].imag = (int16_t)median_imag;
1524 }
1525
1526
1527 static void double_buffering_interpolate(acc_int16_complex_t *frame,
1528 const uint16_t sweeps_per_frame,
1529 const uint16_t num_points,
1530 const uint16_t sweep,
1531 const uint16_t point)
1532 {
1533 /* 2/3 of the sweep value before */
1534 int32_t interpolate_real_i32 = frame[((sweep - 1U)*num_points) + point].real;
1535 int32_t interpolate_imag_i32 = frame[((sweep - 1U)*num_points) + point].imag;
1536
1537 interpolate_real_i32 = interpolate_real_i32 * 2;
1538 interpolate_imag_i32 = interpolate_imag_i32 * 2;
1539
1540 /* 1/3 of the sweep value two positions ahead */
1541 uint16_t sweep_idx = sweep + 2U;
1542
1543 if (sweep_idx > (sweeps_per_frame - 1U))
1544 {
1545 sweep_idx = sweeps_per_frame - 1U;
1546 }
1547
1548 interpolate_real_i32 += frame[((sweep_idx)*num_points) + point].real;
1549 interpolate_imag_i32 += frame[((sweep_idx)*num_points) + point].imag;
1550
1551 interpolate_real_i32 = interpolate_real_i32 / 3;
1552 interpolate_imag_i32 = interpolate_imag_i32 / 3;
1553
1554 /* Update frame with interpolated value */
1555 frame[(sweep*num_points) + point].real = (int16_t)interpolate_real_i32;
1556 frame[(sweep*num_points) + point].imag = (int16_t)interpolate_imag_i32;
1557 }
1558
1559
1560 static void merge_peak_cluster(uint16_t start_idx,
1561 uint16_t num_peaks,
1562 const float *velocities,
1563 const float *energies,
1564 const uint16_t *peak_idxs,
1565 float *merged_velocities,
1566 float *merged_energies,
1567 uint16_t cluster_count)
1568 {
1569 float min = INFINITY;
1570 float max = -INFINITY;
1571
1572 for (uint16_t i = 0U; i < num_peaks; i++)
1573 {
1574 merged_velocities[cluster_count] +=
1575 velocities[peak_idxs[start_idx + i]];
1576 merged_energies[cluster_count] +=
1577 energies[peak_idxs[start_idx + i]];
1578
1579 min = fminf(
1580 velocities[peak_idxs[start_idx + i]], min);
1581
1582 max = fmaxf(velocities[peak_idxs[start_idx + i]], max);
1583 }
1584
1585 merged_velocities[cluster_count] /= (float)num_peaks;
1586 merged_energies[cluster_count] /= (float)num_peaks;
1587 }
1588
1589
1590 static float max_measurable_dist(acc_config_prf_t prf)
1591 {
1592 float mmd;
1593
1594 switch (prf)
1595 {
1596 case ACC_CONFIG_PRF_19_5_MHZ:
1597 mmd = 3.1f;
1598 break;
1599 case ACC_CONFIG_PRF_15_6_MHZ:
1600 mmd = 5.1f;
1601 break;
1602 case ACC_CONFIG_PRF_13_0_MHZ:
1603 mmd = 7.0f;
1604 break;
1605 case ACC_CONFIG_PRF_8_7_MHZ:
1606 mmd = 12.7f;
1607 break;
1608 case ACC_CONFIG_PRF_6_5_MHZ:
1609 mmd = 18.5f;
1610 break;
1611 case ACC_CONFIG_PRF_5_2_MHZ:
1612 mmd = 24.2f;
1613 break;
1614 default:
1615 mmd = 0.0f;
1616 break;
1617 }
1618
1619 return mmd;
1620 }
1621
1622
1623 static acc_config_profile_t get_profile(uint16_t value)
1624 {
1625 acc_config_profile_t profile = ACC_CONFIG_PROFILE_3;
1626
1627 switch (value)
1628 {
1629 case 1U:
1630 profile = ACC_CONFIG_PROFILE_1;
1631 break;
1632
1633 case 2U:
1634 profile = ACC_CONFIG_PROFILE_2;
1635 break;
1636
1637 case 4U:
1638 profile = ACC_CONFIG_PROFILE_4;
1639 break;
1640
1641 case 5U:
1642 profile = ACC_CONFIG_PROFILE_5;
1643 break;
1644
1645 default:
1646 break;
1647 }
1648
1649 return profile;
1650 }
acc_algorithm_rfftfreq
void acc_algorithm_rfftfreq(uint16_t n, float d, float *freqs)
Calculate the real Fast Fourier Transform sample frequencies.
Definition: acc_algorithm.c:545
filter_inplace_apply
static void filter_inplace_apply(uint16_t sample_idx, const float *b, const float *a, float state[5], float *data)
Definition: acc_algorithm.c:1207
acc_algorithm_fft
void acc_algorithm_fft(const float complex *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride)
1D Fast Fourier Transform for complex input
Definition: acc_algorithm.c:500
DOUBLE_BUFFERING_MEAN_ABS_DEV_OUTLIER_TH
#define DOUBLE_BUFFERING_MEAN_ABS_DEV_OUTLIER_TH
Definition: acc_algorithm.c:17
acc_algorithm_argmax
uint16_t acc_algorithm_argmax(const float *data, uint16_t data_length)
Find index of largest element in the array.
Definition: acc_algorithm.c:227
acc_algorithm_cdiv
float complex acc_algorithm_cdiv(float complex num, float complex denom)
Divide complex number num / denum.
Definition: acc_algorithm.c:592
acc_algorithm_apply_filter
void acc_algorithm_apply_filter(const float *a, const float *filt_data, uint16_t filt_rows, uint16_t filt_cols, const float *b, const float *data, uint16_t data_rows, uint16_t data_cols, float *output, uint16_t output_length)
Apply filter coefficients to filtered data matrix and data matrix.
Definition: acc_algorithm.c:395
acc_algorithm_hann
void acc_algorithm_hann(uint16_t n, float *window)
Calculate non-symmetrical hann window for a specified number of points.
Definition: acc_algorithm.c:619
acc_algorithm_select_prf
acc_config_prf_t acc_algorithm_select_prf(int16_t breakpoint, acc_config_profile_t profile, float base_step_length)
Select a suitable PRF given a breakpoint and profile.
Definition: acc_algorithm.c:1071
acc_algorithm_calculate_cfar
float acc_algorithm_calculate_cfar(const float *data, uint16_t data_length, uint16_t middle_idx, uint16_t cfar_win_length, uint16_t cfar_guard_length, float cfar_sensitivity, uint16_t idx)
Calculate CFAR threshold.
Definition: acc_algorithm.c:844
get_data_padded_f32_to_f32_complex
static float complex get_data_padded_f32_to_f32_complex(const float *data, uint16_t data_length, uint16_t index, uint16_t stride)
Definition: acc_algorithm.c:1227
acc_config_profile_t
acc_config_profile_t
Profile.
Definition: acc_definitions_a121.h:39
acc_algorithm_roll_and_push_f32_complex_matrix
void acc_algorithm_roll_and_push_f32_complex_matrix(float complex *data, uint16_t rows, uint16_t cols, const float complex *column, bool pos_shift)
Roll row elements and push a new column.
Definition: acc_algorithm.c:168
acc_alg_basic_utils_is_bit_set_bitarray_uint32
static bool acc_alg_basic_utils_is_bit_set_bitarray_uint32(const uint32_t *bitarray, size_t bit_index)
Check if bit is set in bit array.
Definition: acc_alg_basic_utils.h:77
acc_algorithm_lfilter_matrix
void acc_algorithm_lfilter_matrix(const float *b, const float *a, float *data, uint16_t rows, uint16_t cols)
Filter data along row dimension.
Definition: acc_algorithm.c:386
acc_algorithm_roll_and_push_f32_matrix
void acc_algorithm_roll_and_push_f32_matrix(float *data, uint16_t rows, uint16_t cols, const float *column, bool pos_shift)
Roll row elements and push a new column.
Definition: acc_algorithm.c:133
acc_algorithm_fftshift_matrix
void acc_algorithm_fftshift_matrix(float *data, uint16_t rows, uint16_t cols)
Shift the zero-frequency component to the center along row dimensions.
Definition: acc_algorithm.c:738
acc_alg_basic_utils.h
acc_int16_complex_t
Data type for interger-based representation of complex numbers.
Definition: acc_definitions_common.h:43
ACC_CONFIG_PRF_15_6_MHZ
@ ACC_CONFIG_PRF_15_6_MHZ
Definition: acc_definitions_a121.h:108
ACC_CONFIG_PRF_6_5_MHZ
@ ACC_CONFIG_PRF_6_5_MHZ
Definition: acc_definitions_a121.h:114
acc_algorithm_get_fwhm
float acc_algorithm_get_fwhm(acc_config_profile_t profile)
Get the envelope Full Width Half Maximum in meters given a profile.
Definition: acc_algorithm.c:632
small_rfft_real_symmetry_conversion
static void small_rfft_real_symmetry_conversion(float complex *output, uint16_t length_shift, uint16_t stride)
Definition: acc_algorithm.c:1418
acc_algorithm_exp_smoothing_coefficient
float acc_algorithm_exp_smoothing_coefficient(float fs, float tc)
Calculate exponential smoothing coefficient.
Definition: acc_algorithm.c:577
double_buffering_median_filter
static void double_buffering_median_filter(acc_int16_complex_t *frame, const uint16_t num_points, const uint16_t sweep, const uint16_t point, const uint16_t median_start_sweep)
Median function for Double buffering.
Definition: acc_algorithm.c:1462
acc_algorithm_rfft_matrix
void acc_algorithm_rfft_matrix(const float *data, uint16_t rows, uint16_t cols, uint16_t length_shift, float complex *output, uint16_t axis)
1D Fast Fourier Transform for real input matrix
Definition: acc_algorithm.c:474
acc_algorithm_lfilter
void acc_algorithm_lfilter(const float *b, const float *a, float *data, uint16_t data_length)
Filter data with a digital filter.
Definition: acc_algorithm.c:375
ACC_CONFIG_PROFILE_4
@ ACC_CONFIG_PROFILE_4
Definition: acc_definitions_a121.h:45
acc_algorithm_apply_filter_complex
void acc_algorithm_apply_filter_complex(const float *a, const float complex *filt_data, uint16_t filt_rows, uint16_t filt_cols, const float *b, const float complex *data, uint16_t data_rows, uint16_t data_cols, float complex *output, uint16_t output_length)
Apply filter coefficients to filtered data matrix and data matrix.
Definition: acc_algorithm.c:415
ACC_CONFIG_PRF_8_7_MHZ
@ ACC_CONFIG_PRF_8_7_MHZ
Definition: acc_definitions_a121.h:112
acc_config_prf_t
acc_config_prf_t
Pulse Repetition Frequency.
Definition: acc_definitions_a121.h:103
acc_algorithm_merge_peaks
bool acc_algorithm_merge_peaks(float max_peak_separation, const float *velocities, const float *energies, const uint16_t *peak_idxs, uint16_t num_peaks, float *merged_velocities, float *merged_energies, uint16_t merged_peaks_length, uint16_t *num_merged_peaks)
Merges peaks.
Definition: acc_algorithm.c:967
ACC_CONFIG_PRF_5_2_MHZ
@ ACC_CONFIG_PRF_5_2_MHZ
Definition: acc_definitions_a121.h:116
acc_algorithm_get_distance_m
float acc_algorithm_get_distance_m(uint16_t step_length, uint16_t start_point, float base_step_length_m, uint16_t idx)
Calculate distance for a point at an index.
Definition: acc_algorithm.c:1043
acc_algorithm_mean_sweep
void acc_algorithm_mean_sweep(const acc_int16_complex_t *frame, uint16_t num_points, uint16_t sweeps_per_frame, uint16_t start_point, uint16_t end_point, float complex *sweep)
Calculate mean sweep of a frame from start_point to end_point.
Definition: acc_algorithm.c:441
acc_algorithm_fftfreq_delta
float acc_algorithm_fftfreq_delta(uint16_t n, float d)
Calculate delta between frequency bins in rfft.
Definition: acc_algorithm.c:532
ACC_CONFIG_PRF_13_0_MHZ
@ ACC_CONFIG_PRF_13_0_MHZ
Definition: acc_definitions_a121.h:110
acc_algorithm_fft_matrix
void acc_algorithm_fft_matrix(const float complex *data, uint16_t rows, uint16_t cols, uint16_t length_shift, float complex *output, uint16_t axis)
1D Fast Fourier Transform for input matrix
Definition: acc_algorithm.c:506
acc_algorithm_roll_and_push
void acc_algorithm_roll_and_push(float *data, uint16_t data_length, float element)
Roll array elements and push new element last.
Definition: acc_algorithm.c:122
acc_algorithm_select_profile
acc_config_profile_t acc_algorithm_select_profile(int32_t start_point, float base_step_length)
Select the highest possible profile without interference of direct leakage.
Definition: acc_algorithm.c:1051
acc_algorithm_hamming
void acc_algorithm_hamming(uint16_t n, float *window)
Calculate hamming window for a specified number of points.
Definition: acc_algorithm.c:606
small_fft_transformation
static void small_fft_transformation(uint16_t length_shift, float complex *output, uint16_t stride)
Definition: acc_algorithm.c:1368
get_data_padded_f32_complex
static float complex get_data_padded_f32_complex(const float complex *data, uint16_t data_length, uint16_t index, uint16_t stride)
Definition: acc_algorithm.c:1249
acc_int16_complex_t::real
int16_t real
Definition: acc_definitions_common.h:45
ACC_CONFIG_PROFILE_5
@ ACC_CONFIG_PROFILE_5
Definition: acc_definitions_a121.h:47
merge_peak_cluster
static void merge_peak_cluster(uint16_t start_idx, uint16_t num_peaks, const float *velocities, const float *energies, const uint16_t *peak_idxs, float *merged_velocities, float *merged_energies, uint16_t cluster_count)
Merge peak cluster.
Definition: acc_algorithm.c:1560
M_PI
#define M_PI
Definition: acc_alg_basic_utils.h:14
acc_algorithm_rfft
void acc_algorithm_rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride)
1D Fast Fourier Transform for real input
Definition: acc_algorithm.c:463
acc_algorithm_interpolate_peaks_equidistant
float acc_algorithm_interpolate_peaks_equidistant(const float *y, float x_start, float x_delta, uint16_t peak_idx)
Interpolate equidistant peaks.
Definition: acc_algorithm.c:255
acc_algorithm_butter_bandpass
void acc_algorithm_butter_bandpass(float min_freq, float max_freq, float fs, float *b, float *a)
Design a 2nd order digital Butterworth bandpass filter.
Definition: acc_algorithm.c:306
acc_algorithm_find_peaks
bool acc_algorithm_find_peaks(const float *abs_sweep, const uint16_t data_length, const uint32_t *threshold_check, uint16_t *peak_idxs, uint16_t peak_idxs_length, uint16_t *num_peaks)
Find peaks above threshold.
Definition: acc_algorithm.c:1107
acc_algorithm_interpolate_peaks
float acc_algorithm_interpolate_peaks(const float *y, const float *x)
Interpolate peak.
Definition: acc_algorithm.c:245
acc_algorithm_fftshift
void acc_algorithm_fftshift(float *data, uint16_t data_length, uint16_t stride)
Shift the zero-frequency component to the center.
Definition: acc_algorithm.c:747
acc_algorithm_unwrap
void acc_algorithm_unwrap(float *data, uint16_t data_length)
Unwraps a signal by changing elements which have an absolute difference from their predecessor of mor...
Definition: acc_algorithm.c:204
small_fft
static void small_fft(const float complex *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride)
Definition: acc_algorithm.c:1316
acc_algorithm_fftfreq
void acc_algorithm_fftfreq(uint16_t n, float d, float *freqs)
Calculate the Fast Fourier Transform sample frequencies.
Definition: acc_algorithm.c:557
get_profile
static acc_config_profile_t get_profile(uint16_t value)
Definition: acc_algorithm.c:1623
small_rfft
static void small_rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride)
Definition: acc_algorithm.c:1262
acc_algorithm.h
acc_int16_complex_t::imag
int16_t imag
Definition: acc_definitions_common.h:46
acc_definitions_common.h
ACC_CONFIG_PROFILE_2
@ ACC_CONFIG_PROFILE_2
Definition: acc_definitions_a121.h:43
acc_algorithm_butter_lowpass
void acc_algorithm_butter_lowpass(float freq, float fs, float *b, float *a)
Design a 2nd order digital Butterworth lowpass filter.
Definition: acc_algorithm.c:263
double_buffering_interpolate
static void double_buffering_interpolate(acc_int16_complex_t *frame, const uint16_t sweeps_per_frame, const uint16_t num_points, const uint16_t sweep, const uint16_t point)
Interpolate function for Double buffering.
Definition: acc_algorithm.c:1527
acc_algorithm_get_peak_velocity
float acc_algorithm_get_peak_velocity(const float *velocities, const float *energies, const uint16_t *peak_idxs, uint16_t num_peaks, float limit)
Find the velocity of the peak with the largest amplitude, prioritizing peaks with a velocity over the...
Definition: acc_algorithm.c:927
max_measurable_dist
static float max_measurable_dist(acc_config_prf_t prf)
Get max measurable distance for some PRF.
Definition: acc_algorithm.c:1590
ACC_CONFIG_PROFILE_3
@ ACC_CONFIG_PROFILE_3
Definition: acc_definitions_a121.h:44
ACC_CONFIG_PRF_19_5_MHZ
@ ACC_CONFIG_PRF_19_5_MHZ
Definition: acc_definitions_a121.h:106
ACC_CONFIG_PROFILE_1
@ ACC_CONFIG_PROFILE_1
Definition: acc_definitions_a121.h:42
acc_algorithm_welch_matrix
void acc_algorithm_welch_matrix(const float complex *data, uint16_t rows, uint16_t cols, uint16_t segment_length, float complex *data_buffer, float complex *fft_out, float *psds, const float *window, uint16_t length_shift, float fs)
Estimate power spectral density (PSD) using Welch’s method along row dimensions.
Definition: acc_algorithm.c:765
acc_algorithm_get_distance_idx
uint16_t acc_algorithm_get_distance_idx(const float *data, uint16_t cols, uint16_t rows, uint16_t middle_idx, uint16_t half_slow_zone)
Find the index of the distance column containing the largest amplitude, disregarding amplitudes prese...
Definition: acc_algorithm.c:900
acc_algorithm_double_buffering_frame_filter
void acc_algorithm_double_buffering_frame_filter(acc_int16_complex_t *frame, const uint16_t sweeps_per_frame, const uint16_t num_points, int32_t *work_buffer)
Double buffering frame filter.
Definition: acc_algorithm.c:662
acc_algorithm_welch
void acc_algorithm_welch(const float complex *data, uint16_t data_length, uint16_t segment_length, float complex *data_buffer, float complex *fft_out, float *psd, const float *window, uint16_t length_shift, float fs, uint16_t stride)
Estimate power spectral density using Welch’s method.
Definition: acc_algorithm.c:784
acc_definitions_a121.h