acc_algorithm.c
Go to the documentation of this file.
1 // Copyright (c) Acconeer AB, 2023-2024
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 <float.h>
9 #include <math.h>
10 #include <stdint.h>
11 #include <stdlib.h>
12
13 #include "acc_alg_basic_utils.h"
14 #include "acc_algorithm.h"
15 #include "acc_definitions_a121.h"
16 #include "acc_definitions_common.h"
17
18 #define DOUBLE_BUFFERING_MEAN_ABS_DEV_OUTLIER_TH 5
19
20 //-----------------------------
21 // Private declarations
22 //-----------------------------
23
24 /**
25 * @brief Calculates the sum of elements of data located at (i * stride) for i in [0, num_steps)
26 *
27 * @param[in] data The data
28 * @param[in] num_steps How many steps of length 'stride' should be taken
29 * @param[out] out Single output location
30 * @param[in] stride How many elements one step should "jump".
31 * E.g. 1U for contiguous iteration.
32 */
33 static void sum_i16_complex(const acc_int16_complex_t *data, uint16_t num_steps, float complex *out, uint16_t stride);
34
35 /**
36 * @brief Calculates the mean of elements of data located at (i * stride) for i in [0, num_steps)
37 *
38 * @param[in] data The data
39 * @param[in] num_steps How many steps of length 'stride' should be taken
40 * @param[out] out Single output location
41 * @param[in] stride How many elements one step should "jump".
42 * E.g. 1U for contiguous iteration.
43 */
44 static void mean_i16_complex(const acc_int16_complex_t *data, uint16_t num_steps, float complex *out, uint16_t stride);
45
46 static void rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride);
47
48 static void fftshift(float *data, uint16_t data_length, uint16_t stride);
49
50 static void welch(const float complex *data,
51 uint16_t data_length,
52 uint16_t segment_length,
53 float complex *data_buffer,
54 float complex *fft_out,
55 float *psd,
56 const float *window,
57 uint16_t length_shift,
58 float fs,
59 uint16_t stride);
60
61 static void filter_inplace_apply(uint16_t sample_idx, const float *b, const float *a, float state[5], float *data);
62
63 static float complex get_data_padded_f32_to_f32_complex(const float *data, uint16_t data_length, uint16_t index, uint16_t stride);
64
65 static float complex get_data_padded_f32_complex(const float complex *data, uint16_t data_length, uint16_t index, uint16_t stride);
66
67 static void small_rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride);
68
69 static void small_fft(const float complex *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride);
70
71 static void small_fft_transformation(uint16_t length_shift, float complex *output, uint16_t stride);
72
73 static void small_rfft_real_symmetry_conversion(float complex *output, uint16_t length_shift, uint16_t stride);
74
75 static void sort_i16(int16_t *array, uint16_t array_length);
76
77 static void swap_i16(int16_t *array, uint16_t idx_a, uint16_t idx_b);
78
79 static void sort_f32(float *array, uint16_t array_length);
80
81 static void swap_f32(float *array, uint16_t idx_a, uint16_t idx_b);
82
83 /**
84 * @brief Interpolate function for Double buffering
85 *
86 * This function will calculate the interpolated value from the sweep
87 * before and the sweep two positions ahead.
88 *
89 * @param[in, out] frame Data frame to where the filter is applied
90 * @param[in] sweeps_per_frame How many sweeps there are in the frame
91 * @param[in] num_points The number of points in the frame
92 * @param[in] sweep The sweep to generate with interpolatation
93 * @param[in] point The point to generate with interpolatation
94 */
95 static void double_buffering_interpolate(acc_int16_complex_t *frame,
96 const uint16_t sweeps_per_frame,
97 const uint16_t num_points,
98 const uint16_t sweep,
99 const uint16_t point);
100
101 /**
102 * @brief Median function for Double buffering
103 *
104 * This function will calculate the median value of four complex values.
105 *
106 * @param[in, out] frame Data frame to where the filter is applied
107 * @param[in] num_points The number of points in the frame
108 * @param[in] sweep The sweep to generate with median filter
109 * @param[in] point The point to generate with median filter
110 * @param[in] median_start_sweep The start sweep for the median calculation
111 */
112 static void double_buffering_median_filter(acc_int16_complex_t *frame,
113 const uint16_t num_points,
114 const uint16_t sweep,
115 const uint16_t point,
116 const uint16_t median_start_sweep);
117
118 /**
119 * @brief Merge peak cluster
120 *
121 * @param[in] start_idx Start index
122 * @param[in] num_peaks Number of peaks to merge
123 * @param[in] velocities Velocities array
124 * @param[in] energies Energies array
125 * @param[in] peak_idxs Peak indexes array
126 * @param[out] merged_velocities Merged velocities array
127 * @param[out] merged_energies Merged energies array
128 * @param[in] cluster_count Cluster count
129 */
130 static void merge_peak_cluster(uint16_t start_idx,
131 uint16_t num_peaks,
132 const float *velocities,
133 const float *energies,
134 const uint16_t *peak_idxs,
135 float *merged_velocities,
136 float *merged_energies,
137 uint16_t cluster_count);
138
139 /**
140 * @brief Get max measurable distance for some PRF
141 *
142 * @param[in] prf Pulse repetition frequency (PRF)
143 * @return Max measurable distance in meters
144 */
145 static float max_measurable_dist(acc_config_prf_t prf);
146
147 /**
148 * Get profile by value
149 *
150 * @param[in] value Integer value that corresponds to an ACC_CONFIG_PROFILE enum
151 * @return An ACC_CONFIG_PROFILE enum
152 */
153 static acc_config_profile_t get_profile(uint16_t value);
154
155 //-----------------------------
156 // Public definitions
157 //-----------------------------
158
159 void acc_algorithm_roll_and_push(float *data, uint16_t data_length, float element)
160 {
161 for (uint16_t i = 1U; i < data_length; i++)
162 {
163 data[i - 1U] = data[i];
164 }
165
166 data[data_length - 1U] = element;
167 }
168
169 void acc_algorithm_roll_and_push_matrix_f32(float *data, uint16_t rows, uint16_t cols, const float *column, 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 void acc_algorithm_roll_and_push_matrix_f32_complex(float complex *data, uint16_t rows, uint16_t cols, const float complex *column, bool pos_shift)
204 {
205 if (pos_shift)
206 {
207 for (uint16_t r = rows - 1U; r > 0U; r--)
208 {
209 for (uint16_t c = 0U; c < cols; c++)
210 {
211 data[(r * cols) + c] = data[((r - 1U) * cols) + c];
212 }
213 }
214
215 for (uint16_t c = 0U; c < cols; c++)
216 {
217 data[c] = column[c];
218 }
219 }
220 else
221 {
222 for (uint16_t r = 1U; r < rows; r++)
223 {
224 for (uint16_t c = 0U; c < cols; c++)
225 {
226 data[((r - 1U) * cols) + c] = data[(r * cols) + c];
227 }
228 }
229
230 for (uint16_t c = 0U; c < cols; c++)
231 {
232 data[((rows - 1U) * cols) + c] = column[c];
233 }
234 }
235 }
236
237 void acc_algorithm_roll_and_push_mult_matrix_i16_complex(acc_int16_complex_t *data,
238 uint16_t data_rows,
239 uint16_t cols,
240 const acc_int16_complex_t *matrix,
241 uint16_t matrix_rows,
242 bool pos_shift)
243 {
244 for (uint16_t m_rows = 0U; m_rows < matrix_rows; m_rows++)
245 {
246 if (pos_shift)
247 {
248 for (uint16_t r = data_rows - 1U; r > 0U; r--)
249 {
250 for (uint16_t c = 0U; c < cols; c++)
251 {
252 data[(r * cols) + c].real = data[((r - 1U) * cols) + c].real;
253 data[(r * cols) + c].imag = data[((r - 1U) * cols) + c].imag;
254 }
255 }
256
257 for (uint16_t c = 0U; c < cols; c++)
258 {
259 data[c].real = matrix[(m_rows * cols) + c].real;
260 data[c].imag = matrix[(m_rows * cols) + c].imag;
261 }
262 }
263 else
264 {
265 for (uint16_t r = 1U; r < data_rows; r++)
266 {
267 for (uint16_t c = 0U; c < cols; c++)
268 {
269 data[((r - 1U) * cols) + c].real = data[(r * cols) + c].real;
270 data[((r - 1U) * cols) + c].imag = data[(r * cols) + c].imag;
271 }
272 }
273
274 for (uint16_t c = 0U; c < cols; c++)
275 {
276 data[((data_rows - 1U) * cols) + c].real = matrix[(m_rows * cols) + c].real;
277 data[((data_rows - 1U) * cols) + c].imag = matrix[(m_rows * cols) + c].imag;
278 }
279 }
280 }
281 }
282
283 void acc_algorithm_unwrap(float *data, uint16_t data_length)
284 {
285 for (uint16_t i = 1U; i < data_length; i++)
286 {
287 float diff = data[i] - data[i - 1U];
288
289 while ((diff > (float)M_PI) || (diff < -(float)M_PI))
290 {
291 if (diff > (float)M_PI)
292 {
293 data[i] -= 2.0f * (float)M_PI;
294 }
295 else
296 {
297 data[i] += 2.0f * (float)M_PI;
298 }
299
300 diff = data[i] - data[i - 1U];
301 }
302 }
303 }
304
305 uint16_t acc_algorithm_argmax(const float *data, uint16_t data_length)
306 {
307 uint16_t idx = 0U;
308 float max = data[idx];
309
310 for (uint16_t i = 1U; i < data_length; i++)
311 {
312 if (data[i] > max)
313 {
314 idx = i;
315 max = data[i];
316 }
317 }
318
319 return idx;
320 }
321
322 float acc_algorithm_interpolate_peaks(const float *y, const float *x)
323 {
324 float a = ((x[0] * (y[2] - y[1])) + (x[1] * (y[0] - y[2])) + (x[2] * (y[1] - y[0]))) / ((x[0] - x[1]) * (x[0] - x[2]) * (x[1] - x[2]));
325 float b = ((y[1] - y[0]) / (x[1] - x[0])) - (a * (x[0] + x[1]));
326
327 return -b / (2.0f * a);
328 }
329
330 float acc_algorithm_interpolate_peaks_equidistant(const float *y, float x_start, float x_delta, uint16_t peak_idx)
331 {
332 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]));
333
334 return x_start + (((float)peak_idx + peak_offset) * x_delta);
335 }
336
337 void acc_algorithm_butter_lowpass(float freq, float fs, float *b, float *a)
338 {
339 float factor = (2.0f * freq) / fs;
340
341 // Values are centered around 0 to ensure an exactly real pole
342 float complex p[2];
343
344 p[0] = -cexpf(((-1.0f * (float)M_PI) / 4.0f) * I);
345 p[1] = -cexpf(((1.0f * (float)M_PI) / 4.0f) * I);
346
347 // Pre-wrap frequencies for digital filter design
348 factor = 4.0f * tanf(((float)M_PI * factor) / 2.0f);
349
350 // Scale all points radially from origin to shift cutoff frequency
351 p[0] = (float complex)factor * p[0];
352 p[1] = (float complex)factor * p[1];
353
354 // Cancel out gain change from frequency scaling
355 float k = factor * factor;
356
357 // Compensate for gain change
358 float complex real_four = (float complex)4.0f;
359 float complex z_prod = 1.0f;
360 float complex p_prod = (real_four - p[0]) * (real_four - p[1]);
361
362 k = k * crealf(acc_algorithm_cdiv(z_prod, p_prod));
363
364 // Bilinear transform the poles and zeros
365 p[0] = acc_algorithm_cdiv(real_four + p[0], real_four - p[0]);
366 p[1] = acc_algorithm_cdiv(real_four + p[1], real_four - p[1]);
367
368 // Any zeroes that were at infinity get moved to the Nyquist Frequency
369 float z_z[2] = {-1.0f, -1.0f};
370
371 // Calculate polynomial transfer functions
372 a[0] = -(p[0] + p[1]);
373 a[1] = p[0] * p[1];
374 b[0] = k;
375 b[1] = -k * (z_z[0] + z_z[1]);
376 b[2] = k * (z_z[0] * z_z[1]);
377 }
378
379 void acc_algorithm_butter_bandpass(float min_freq, float max_freq, float fs, float *b, float *a)
380 {
381 float min_f = (2.0f * min_freq) / fs;
382 float max_f = (2.0f * max_freq) / fs;
383
384 // Values are centered around 0 to ensure an exactly real pole
385 float complex p[2];
386
387 p[0] = -cexpf(((-1.0f * (float)M_PI) / 4.0f) * I);
388 p[1] = -cexpf(((1.0f * (float)M_PI) / 4.0f) * I);
389 float k = 1.0f;
390
391 // Pre-wrap frequencies for digital filter design
392 min_f = 4.0f * tanf(((float)M_PI * min_f) / 2.0f);
393 max_f = 4.0f * tanf(((float)M_PI * max_f) / 2.0f);
394
395 // Transform lowpass filter prototype to a bandspass filter
396 float bw = max_f - min_f;
397 float complex w0 = (float complex)sqrtf(min_f * max_f);
398
399 // Scale poles and zeros to desired bandwidth
400 float complex scale = (float complex)(bw / 2.0f);
401
402 p[0] = scale * p[0];
403 p[1] = scale * p[1];
404
405 // Duplicate poles and zeros and shift from baseband to +w0 and -w0
406 float complex p_bp[4];
407
408 w0 *= w0;
409 p_bp[0] = p[0] + csqrtf((p[0] * p[0]) - w0);
410 p_bp[1] = p[1] + csqrtf((p[1] * p[1]) - w0);
411 p_bp[2] = p[0] - csqrtf((p[0] * p[0]) - w0);
412 p_bp[3] = p[1] - csqrtf((p[1] * p[1]) - w0);
413
414 // Cancel out gain change from frequency scaling
415 float k_bp = k * bw * bw;
416
417 // Bilinear transform the poles and zeros
418 float complex real_four = (float complex)4.0f;
419 float complex p_z[4];
420
421 p_z[0] = acc_algorithm_cdiv(real_four + p_bp[0], real_four - p_bp[0]);
422 p_z[1] = acc_algorithm_cdiv(real_four + p_bp[1], real_four - p_bp[1]);
423 p_z[2] = acc_algorithm_cdiv(real_four + p_bp[2], real_four - p_bp[2]);
424 p_z[3] = acc_algorithm_cdiv(real_four + p_bp[3], real_four - p_bp[3]);
425
426 // Any zeroes that were at infinity get moved to the Nyquist Frequency
427 float z_z[4] = {1.0f, 1.0f, -1.0f, -1.0f};
428
429 // Compensate for gain change
430 float complex z_prod = 16.0f;
431 float complex p_prod = (real_four - p_bp[0]) * (real_four - p_bp[1]) * (real_four - p_bp[2]) * (real_four - p_bp[3]);
432 float k_z = k_bp * crealf(acc_algorithm_cdiv(z_prod, p_prod));
433
434 // Calculate polynomial transfer functions
435 a[0] = -(p_z[0] + p_z[1] + p_z[2] + p_z[3]);
436 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]);
437 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]));
438 a[3] = p_z[0] * p_z[1] * p_z[2] * p_z[3];
439 b[0] = k_z;
440 b[1] = -k_z * (z_z[0] + z_z[1] + z_z[2] + z_z[3]);
441 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]));
442 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]));
443 b[4] = k_z * (z_z[0] * z_z[1] * z_z[2] * z_z[3]);
444 }
445
446 void acc_algorithm_lfilter(const float *b, const float *a, float *data, uint16_t data_length)
447 {
448 float filter_states[5] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
449
450 for (uint16_t i = 0U; i < data_length; i++)
451 {
452 filter_inplace_apply(i, b, a, filter_states, data);
453 }
454 }
455
456 void acc_algorithm_lfilter_matrix(const float *b, const float *a, float *data, uint16_t rows, uint16_t cols)
457 {
458 for (uint16_t i = 0U; i < rows; i++)
459 {
460 acc_algorithm_lfilter(b, a, &data[i * cols], cols);
461 }
462 }
463
464 void acc_algorithm_apply_filter_f32(const float *a,
465 const float *filt_data,
466 uint16_t filt_rows,
467 uint16_t filt_cols,
468 const float *b,
469 const float *data,
470 uint16_t data_rows,
471 uint16_t data_cols,
472 float *output,
473 uint16_t output_length)
474 {
475 for (uint16_t i = 0U; i < output_length; i++)
476 {
477 output[i] = 0.0f;
478
479 for (uint16_t r = 0U; r < filt_rows; r++)
480 {
481 output[i] -= a[r] * filt_data[i + (r * filt_cols)];
482 }
483
484 for (uint16_t r = 0U; r < data_rows; r++)
485 {
486 output[i] += b[r] * data[i + (r * data_cols)];
487 }
488 }
489 }
490
491 void acc_algorithm_apply_filter_f32_complex(const float *a,
492 const float complex *filt_data,
493 uint16_t filt_rows,
494 uint16_t filt_cols,
495 const float *b,
496 const float complex *data,
497 uint16_t data_rows,
498 uint16_t data_cols,
499 float complex *output,
500 uint16_t output_length)
501 {
502 for (uint16_t i = 0U; i < output_length; i++)
503 {
504 float real = 0.0f;
505 float imag = 0.0f;
506
507 for (uint16_t r = 0U; r < filt_rows; r++)
508 {
509 real -= a[r] * crealf(filt_data[i + (r * filt_cols)]);
510 imag -= a[r] * cimagf(filt_data[i + (r * filt_cols)]);
511 }
512
513 for (uint16_t r = 0U; r < data_rows; r++)
514 {
515 real += b[r] * crealf(data[i + (r * data_cols)]);
516 imag += b[r] * cimagf(data[i + (r * data_cols)]);
517 }
518
519 output[i] = real + (imag * I);
520 }
521 }
522
523 void acc_algorithm_mean_sweep(const acc_int16_complex_t *frame,
524 uint16_t num_points,
525 uint16_t sweeps_per_frame,
526 uint16_t start_point,
527 uint16_t end_point,
528 float complex *sweep)
529 {
530 for (uint16_t n = start_point; n < end_point; n++)
531 {
532 mean_i16_complex(&frame[n], sweeps_per_frame, &sweep[n - start_point], num_points);
533 }
534 }
535
536 void acc_algorithm_sum_sweep(const acc_int16_complex_t *frame,
537 uint16_t num_points,
538 uint16_t sweeps_per_frame,
539 uint16_t start_point,
540 uint16_t end_point,
541 float complex *sum_sweep)
542 {
543 for (uint16_t n = start_point; n < end_point; n++)
544 {
545 sum_i16_complex(&frame[n], sweeps_per_frame, &sum_sweep[n - start_point], num_points);
546 }
547 }
548
549 void acc_algorithm_mean_i16_complex(const acc_int16_complex_t *data, uint16_t data_length, float complex *out)
550 {
551 mean_i16_complex(data, data_length, out, 1U);
552 }
553
554 void acc_algorithm_mean_matrix_i16_complex(const acc_int16_complex_t *matrix, uint16_t rows, uint16_t cols, float complex *out, uint16_t axis)
555 {
556 if (axis == 1U)
557 {
558 for (uint16_t i = 0U; i < rows; i++)
559 {
560 mean_i16_complex(&matrix[i * cols], cols, &out[i], 1U);
561 }
562 }
563 else if (axis == 0U)
564 {
565 for (uint16_t i = 0U; i < cols; i++)
566 {
567 mean_i16_complex(&matrix[i], rows, &out[i], cols);
568 }
569 }
570 else
571 {
572 // Do nothing
573 }
574 }
575
576 void acc_algorithm_conj_f32(float complex *data, uint16_t data_length)
577 {
578 for (uint16_t i = 0U; i < data_length; i++)
579 {
580 data[i] = conjf(data[i]);
581 }
582 }
583
584 void acc_algorithm_normalize_f32_complex(float complex *data, uint16_t data_length)
585 {
586 for (uint16_t i = 0U; i < data_length; i++)
587 {
588 data[i] = data[i] / cabsf(data[i]);
589 }
590 }
591
592 void acc_algorithm_rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output)
593 {
594 rfft(data, data_length, length_shift, output, 1U);
595 }
596
597 void acc_algorithm_rfft_matrix(const float *data, uint16_t rows, uint16_t cols, uint16_t length_shift, float complex *output, uint16_t axis)
598 {
599 uint16_t full_cols = ((uint16_t)1U) << length_shift;
600
601 if (axis == 1U)
602 {
603 uint16_t output_cols = (full_cols / 2U) + 1U;
604 for (uint16_t i = 0U; i < rows; i++)
605 {
606 rfft(&data[i * cols], cols, length_shift, &output[i * output_cols], 1U);
607 }
608 }
609 else if (axis == 0U)
610 {
611 for (uint16_t i = 0U; i < cols; i++)
612 {
613 rfft(&data[i], rows, length_shift, &output[i], cols);
614 }
615 }
616 else
617 {
618 // Do nothing
619 }
620 }
621
622 void acc_algorithm_fft(const float complex *data, uint16_t data_length, uint16_t length_shift, float complex *output)
623 {
624 small_fft(data, data_length, length_shift, output, 1U);
625 }
626
627 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)
628 {
629 uint16_t full_cols = ((uint16_t)1U) << length_shift;
630
631 if (axis == 1U)
632 {
633 uint16_t output_cols = full_cols;
634 for (uint16_t i = 0U; i < rows; i++)
635 {
636 small_fft(&data[i * cols], cols, length_shift, &output[i * output_cols], 1U);
637 }
638 }
639 else if (axis == 0U)
640 {
641 for (uint16_t i = 0U; i < cols; i++)
642 {
643 small_fft(&data[i], rows, length_shift, &output[i], cols);
644 }
645 }
646 else
647 {
648 // Do nothing
649 }
650 }
651
652 float acc_algorithm_fftfreq_delta(uint16_t n, float d)
653 {
654 float df = NAN;
655
656 if ((n > 0U) && (d > 0.0f))
657 {
658 df = 1.0f / ((float)n * d);
659 }
660
661 return df;
662 }
663
664 void acc_algorithm_rfftfreq(uint16_t n, float d, float *freqs)
665 {
666 uint16_t n_freqs = (n / 2U) + 1U;
667 float df = acc_algorithm_fftfreq_delta(n, d);
668
669 for (uint16_t i = 0U; i < n_freqs; i++)
670 {
671 freqs[i] = (float)i * df;
672 }
673 }
674
675 void acc_algorithm_fftfreq(uint16_t n, float d, float *freqs)
676 {
677 float df = acc_algorithm_fftfreq_delta(n, d);
678 uint16_t mid = n / 2U;
679
680 for (uint16_t i = 0U; i < mid; i++)
681 {
682 freqs[i] = (float)i * df;
683 }
684
685 int32_t n_int32 = (int32_t)n;
686
687 for (int32_t i = (int32_t)mid; i < n_int32; i++)
688 {
689 int32_t diff = i - n_int32;
690 freqs[i] = (float)diff * df;
691 }
692 }
693
694 float acc_algorithm_exp_smoothing_coefficient(float fs, float tc)
695 {
696 float res = NAN;
697
698 if ((fs != 0.0f) && (tc != 0.0f))
699 {
700 float dt = 1.0f / fs;
701
702 res = expf(-dt / tc);
703 }
704
705 return res;
706 }
707
708 float complex acc_algorithm_cdiv(float complex num, float complex denom)
709 {
710 double a = (double)crealf(denom);
711 double b = (double)cimagf(denom);
712 double c = (double)crealf(num);
713 double d = (double)cimagf(num);
714
715 float real = (float)(((c * a) + (b * d)) / ((a * a) + (b * b)));
716 float imag = (float)(((a * d) - (c * b)) / ((a * a) + (b * b)));
717
718 return real + (imag * I);
719 }
720
721 void acc_algorithm_hamming(uint16_t n, float *window)
722 {
723 const float a = 0.54f;
724 const float b = 0.46f;
725 const float factor = (2.0f * (float)M_PI) / ((float)n - 1.0f);
726
727 for (uint16_t i = 0U; i < n; i++)
728 {
729 window[i] = a - (b * cosf((float)i * factor));
730 }
731 }
732
733 void acc_algorithm_hann(uint16_t n, float *window)
734 {
735 const float a = 0.5f;
736
737 const float factor = (2.0f * (float)M_PI) / (float)n;
738
739 for (uint16_t i = 0U; i < n; i++)
740 {
741 window[i] = a - (a * cosf((float)i * factor));
742 }
743 }
744
745 float acc_algorithm_get_fwhm(acc_config_profile_t profile)
746 {
747 float fwhm;
748
749 switch (profile)
750 {
751 case ACC_CONFIG_PROFILE_1:
752 fwhm = 0.04f;
753 break;
754 case ACC_CONFIG_PROFILE_2:
755 fwhm = 0.07f;
756 break;
757 case ACC_CONFIG_PROFILE_3:
758 fwhm = 0.14f;
759 break;
760 case ACC_CONFIG_PROFILE_4:
761 fwhm = 0.19f;
762 break;
763 case ACC_CONFIG_PROFILE_5:
764 fwhm = 0.32f;
765 break;
766 default:
767 fwhm = 0.0f;
768 break;
769 }
770
771 return fwhm;
772 }
773
774 void acc_algorithm_double_buffering_frame_filter(acc_int16_complex_t *frame,
775 const uint16_t sweeps_per_frame,
776 const uint16_t num_points,
777 int32_t *work_buffer)
778 {
779 int32_t first_diff_r[2U];
780 int32_t first_diff_i[2U];
781
782 if (sweeps_per_frame >= 32U)
783 {
784 for (uint16_t point = 0U; point < num_points; point++)
785 {
786 int32_t abs_mad_sum = 0;
787
788 for (uint16_t sweep = 0U; sweep < (sweeps_per_frame - 2U); sweep++)
789 {
790 /* Calculate 1st discrete difference */
791 for (uint16_t idx = 0U; idx < 2U; idx++)
792 {
793 uint16_t sweep_idx = (sweep + idx) * num_points;
794 uint16_t next_sweep_idx = (sweep + idx + 1U) * num_points;
795 int32_t diff1_r = frame[(next_sweep_idx + point)].real;
796 int32_t diff1_i = frame[(next_sweep_idx + point)].imag;
797 int32_t diff2_r = frame[(sweep_idx + point)].real;
798 int32_t diff2_i = frame[(sweep_idx + point)].imag;
799 first_diff_r[idx] = diff1_r - diff2_r;
800 first_diff_i[idx] = diff1_i - diff2_i;
801 }
802
803 /* Calculate 2nd discrete difference */
804 int32_t second_diff_r = first_diff_r[1] - first_diff_r[0U];
805 int32_t second_diff_i = first_diff_i[1] - first_diff_i[0U];
806
807 /* Estimating magnitude using: abs(real) + abs(imag) */
808 int32_t abs_r = (second_diff_r < 0) ? -second_diff_r : second_diff_r;
809 int32_t abs_i = (second_diff_i < 0) ? -second_diff_i : second_diff_i;
810
811 work_buffer[sweep] = abs_r + abs_i;
812
813 /* Sum mean absolute deviation */
814 abs_mad_sum += work_buffer[sweep];
815 }
816
817 /* Mean absolute deviation */
818 int32_t nof_of_abs = (int32_t)sweeps_per_frame;
819 nof_of_abs = nof_of_abs - 2;
820 int32_t diff_mad = abs_mad_sum / nof_of_abs;
821 int32_t threshold = DOUBLE_BUFFERING_MEAN_ABS_DEV_OUTLIER_TH * diff_mad;
822
823 for (uint16_t sweep = 1U; sweep < (sweeps_per_frame - 1U); sweep++)
824 {
825 if (work_buffer[sweep - 1U] <= threshold)
826 {
827 continue;
828 }
829
830 if (sweep == 1U)
831 {
832 /* First Sweep */
833 double_buffering_median_filter(frame, num_points, 1U, point, 0U);
834 }
835 else if (sweep == (sweeps_per_frame - 2U))
836 {
837 /* Last Sweep */
838 double_buffering_median_filter(frame, num_points, sweeps_per_frame - 2U, point, sweeps_per_frame - 4U - 1U);
839 }
840 else
841 {
842 double_buffering_interpolate(frame, sweeps_per_frame, num_points, sweep, point);
843 }
844 }
845 }
846 }
847 }
848
849 void acc_algorithm_fftshift_matrix(float *data, uint16_t rows, uint16_t cols)
850 {
851 for (uint16_t i = 0U; i < cols; i++)
852 {
853 fftshift(&(data[i]), rows, cols);
854 }
855 }
856
857 void acc_algorithm_fftshift(float *data, uint16_t data_length)
858 {
859 fftshift(data, data_length, 1U);
860 }
861
862 void acc_algorithm_welch_matrix(const float complex *data,
863 uint16_t rows,
864 uint16_t cols,
865 uint16_t segment_length,
866 float complex *data_buffer,
867 float complex *fft_out,
868 float *psds,
869 const float *window,
870 uint16_t length_shift,
871 float fs)
872 {
873 for (uint16_t i = 0U; i < cols; i++)
874 {
875 welch(&(data[i]), rows, segment_length, data_buffer, fft_out, &(psds[i]), window, length_shift, fs, cols);
876 }
877 }
878
879 void acc_algorithm_welch(const float complex *data,
880 uint16_t data_length,
881 uint16_t segment_length,
882 float complex *data_buffer,
883 float complex *fft_out,
884 float *psd,
885 const float *window,
886 uint16_t length_shift,
887 float fs)
888 {
889 welch(data, data_length, segment_length, data_buffer, fft_out, psd, window, length_shift, fs, 1U);
890 }
891
892 float acc_algorithm_calculate_cfar(const float *data,
893 uint16_t data_length,
894 uint16_t window_length,
895 uint16_t half_guard_length,
896 float sensitivity,
897 uint16_t idx)
898 {
899 const uint16_t start_idx = window_length + half_guard_length;
900 const uint16_t end_idx = data_length - start_idx;
901 float threshold = 0.0f;
902
903 if ((idx < start_idx) || (idx >= end_idx))
904 {
905 threshold = FLT_MAX;
906 }
907 else
908 {
909 float samples_sum = 0.0f;
910 uint16_t sample_count = 0U;
911 uint16_t take_window_close_start = idx - half_guard_length - window_length;
912 uint16_t take_window_far_start = idx + half_guard_length + 1U;
913
914 for (uint16_t k = 0; k < window_length; k++)
915 {
916 samples_sum += data[take_window_close_start + k];
917 samples_sum += data[take_window_far_start + k];
918 sample_count += 2U;
919 }
920
921 threshold = (sample_count > 0U) ? (samples_sum / (float)sample_count) : 0.0f;
922 threshold += sensitivity;
923 }
924
925 return threshold;
926 }
927
928 float acc_algorithm_calculate_mirrored_one_sided_cfar(const float *data,
929 uint16_t data_length,
930 uint16_t middle_idx,
931 uint16_t window_length,
932 uint16_t half_guard_length,
933 float sensitivity,
934 uint16_t idx)
935 {
936 uint16_t margin = window_length + half_guard_length;
937 uint16_t half_sweep_len_without_margin = (uint16_t)rint(((double)data_length / 2.0) - (double)margin);
938
939 float min = INFINITY;
940
941 for (uint16_t i = 0U; i < data_length; i++)
942 {
943 min = fminf(data[i], min);
944 }
945
946 float sum = 0.0f;
947
948 if (idx <= margin)
949 {
950 for (uint16_t j = 0U; j < window_length; j++)
951 {
952 sum += data[j];
953 }
954 }
955
956 if ((idx > margin) && (idx < middle_idx))
957 {
958 for (uint16_t j = 0U; j < window_length; j++)
959 {
960 sum += data[j + (idx - margin)];
961 }
962 }
963
964 if ((idx >= middle_idx) && (idx < (data_length - margin - 1U)))
965 {
966 for (uint16_t j = 0U; j < window_length; j++)
967 {
968 sum += data[data_length - half_sweep_len_without_margin - j + idx - middle_idx];
969 }
970 }
971
972 if (idx >= (data_length - margin - 1U))
973 {
974 for (uint16_t j = 0U; j < window_length; j++)
975 {
976 sum += data[data_length - j - 1U];
977 }
978 }
979
980 return ((sum / (float)window_length) + min) / sensitivity;
981 }
982
983 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)
984 {
985 float max = -INFINITY;
986
987 uint16_t idx = 0U;
988
989 for (uint16_t i = 0U; i < rows; i++)
990 {
991 if ((i < (middle_idx + half_slow_zone)) && (i >= (middle_idx - half_slow_zone)))
992 {
993 continue;
994 }
995
996 for (uint16_t j = 0U; j < cols; j++)
997 {
998 if (data[(i * cols) + j] > max)
999 {
1000 max = data[(i * cols) + j];
1001 idx = j;
1002 }
1003 }
1004 }
1005
1006 return idx;
1007 }
1008
1009 float acc_algorithm_get_peak_velocity(const float *velocities, const float *energies, const uint16_t *peak_idxs, uint16_t num_peaks, float limit)
1010 {
1011 float slow_vs = 0.0f;
1012 float valid_vs = 0.0f;
1013 bool has_valid = false;
1014 float biggest_energy_slow = -INFINITY;
1015 float biggest_energy_valid = -INFINITY;
1016
1017 for (uint16_t i = 0U; i < num_peaks; i++)
1018 {
1019 uint16_t idx = (peak_idxs != NULL) ? peak_idxs[i] : i;
1020
1021 if (energies[idx] > biggest_energy_slow)
1022 {
1023 if (fabsf(velocities[idx]) < limit)
1024 {
1025 slow_vs = velocities[idx];
1026 biggest_energy_slow = energies[idx];
1027 }
1028 }
1029
1030 if (energies[i] > biggest_energy_valid)
1031 {
1032 if (fabsf(velocities[idx]) >= limit)
1033 {
1034 valid_vs = velocities[idx];
1035 biggest_energy_valid = energies[idx];
1036 has_valid = true;
1037 }
1038 }
1039 }
1040
1041 return has_valid ? valid_vs : slow_vs;
1042 }
1043
1044 bool acc_algorithm_merge_peaks(float max_peak_separation,
1045 const float *velocities,
1046 const float *energies,
1047 const uint16_t *peak_idxs,
1048 uint16_t num_peaks,
1049 float *merged_velocities,
1050 float *merged_energies,
1051 uint16_t merged_peaks_length,
1052 uint16_t *num_merged_peaks)
1053 {
1054 bool status = true;
1055 uint16_t cluster_count = 0U;
1056 uint16_t cluster_start_idx = 0U;
1057
1058 if (num_peaks > 1U)
1059 {
1060 for (uint16_t i = 0U; i < (num_peaks - 1U); i++)
1061 {
1062 uint16_t current_idx = peak_idxs[i];
1063 uint16_t next_idx = peak_idxs[i + 1U];
1064
1065 uint16_t num_peaks_in_cluster = i - cluster_start_idx + 1U;
1066
1067 bool current_peak_is_in_cluster = (velocities[next_idx] - velocities[current_idx]) < max_peak_separation;
1068
1069 if (current_peak_is_in_cluster)
1070 {
1071 continue;
1072 }
1073
1074 status = cluster_count < merged_peaks_length;
1075
1076 if (status)
1077 {
1078 merge_peak_cluster(
1079 cluster_start_idx, num_peaks_in_cluster, velocities, energies, peak_idxs, merged_velocities, merged_energies, cluster_count);
1080 }
1081
1082 if (!status)
1083 {
1084 break;
1085 }
1086
1087 cluster_count++;
1088 cluster_start_idx = i + 1U;
1089 }
1090 }
1091
1092 bool last_cluster_not_merged = cluster_start_idx < num_peaks;
1093
1094 if (status && last_cluster_not_merged)
1095 {
1096 status = cluster_count < merged_peaks_length;
1097
1098 if (status)
1099 {
1100 merge_peak_cluster(
1101 cluster_start_idx, num_peaks - cluster_start_idx, velocities, energies, peak_idxs, merged_velocities, merged_energies, cluster_count);
1102 }
1103
1104 if (status)
1105 {
1106 cluster_count++;
1107 }
1108 }
1109
1110 if (status)
1111 {
1112 *num_merged_peaks = cluster_count;
1113 }
1114
1115 return status;
1116 }
1117
1118 float acc_algorithm_get_distance_m(uint16_t step_length, uint16_t start_point, float base_step_length_m, uint16_t idx)
1119 {
1120 uint16_t steps = (idx * step_length) + start_point;
1121
1122 return (float)steps * base_step_length_m;
1123 }
1124
1125 acc_config_profile_t acc_algorithm_select_profile(int32_t start_point, float base_step_length)
1126 {
1127 acc_config_profile_t profile = ACC_CONFIG_PROFILE_1;
1128
1129 // Array with minimum start point for each profile without interference from direct leakage
1130 // Profile 1 has special case -1 to default to this if no other work
1131 float MIN_DIST_M[5] = {-1.0f, 0.07f * 2.0f, 0.14f * 2.0f, 0.19f * 2.0f, 0.32f * 2.0f};
1132
1133 for (uint16_t i = (uint16_t)ACC_CONFIG_PROFILE_1; i <= (uint16_t)ACC_CONFIG_PROFILE_5; i++)
1134 {
1135 if ((MIN_DIST_M[i - 1U] == -1.0f) || (MIN_DIST_M[i - 1U] <= ((float)start_point * base_step_length)))
1136 {
1137 profile = get_profile(i);
1138 }
1139 }
1140
1141 return profile;
1142 }
1143
1144 acc_config_prf_t acc_algorithm_select_prf(int16_t breakpoint, acc_config_profile_t profile, float base_step_length)
1145 {
1146 float breakpoint_p = (float)breakpoint;
1147 float breakpoint_m = breakpoint_p * base_step_length;
1148 acc_config_prf_t prf;
1149
1150 if ((breakpoint_m < max_measurable_dist(ACC_CONFIG_PRF_19_5_MHZ)) && (profile == ACC_CONFIG_PROFILE_1))
1151 {
1152 prf = ACC_CONFIG_PRF_19_5_MHZ;
1153 }
1154 else if (breakpoint_m < max_measurable_dist(ACC_CONFIG_PRF_15_6_MHZ))
1155 {
1156 prf = ACC_CONFIG_PRF_15_6_MHZ;
1157 }
1158 else if (breakpoint_m < max_measurable_dist(ACC_CONFIG_PRF_13_0_MHZ))
1159 {
1160 prf = ACC_CONFIG_PRF_13_0_MHZ;
1161 }
1162 else if (breakpoint_m < max_measurable_dist(ACC_CONFIG_PRF_8_7_MHZ))
1163 {
1164 prf = ACC_CONFIG_PRF_8_7_MHZ;
1165 }
1166 else if (breakpoint_m < max_measurable_dist(ACC_CONFIG_PRF_6_5_MHZ))
1167 {
1168 prf = ACC_CONFIG_PRF_6_5_MHZ;
1169 }
1170 else
1171 {
1172 prf = ACC_CONFIG_PRF_5_2_MHZ;
1173 }
1174
1175 return prf;
1176 }
1177
1178 bool acc_algorithm_find_peaks(const float *abs_sweep,
1179 const uint16_t data_length,
1180 const uint32_t *threshold_check,
1181 uint16_t *peak_idxs,
1182 uint16_t peak_idxs_length,
1183 uint16_t *num_peaks)
1184 {
1185 bool success = true;
1186 uint16_t found_peaks = 0U;
1187 uint16_t i = 1U;
1188
1189 while (i < data_length)
1190 {
1191 /*
1192 * Find a peak candidate.
1193 */
1194
1195 if (!acc_alg_basic_utils_is_bit_set_bitarray_uint32(threshold_check, ((size_t)i - 1U)))
1196 {
1197 i++;
1198 continue;
1199 }
1200
1201 if (!acc_alg_basic_utils_is_bit_set_bitarray_uint32(threshold_check, i))
1202 {
1203 i += 2U;
1204 continue;
1205 }
1206
1207 if (abs_sweep[i - 1U] >= abs_sweep[i])
1208 {
1209 i++;
1210 continue;
1211 }
1212
1213 /*
1214 * Peak candidate found at abs_sweep[d].
1215 *
1216 * We have two consecutive peaks above threshold: abs_sweep[d-1] and
1217 * abs_sweep[d], where abs_sweep[d-1] < abs_sweep[d].
1218 *
1219 * Now search for an upper bound where abs_sweep[upper] < abs_sweep[d],
1220 * but still above threshold.
1221 */
1222
1223 uint16_t d_upper = i + 1U;
1224 bool upper_done = false;
1225
1226 while (!upper_done)
1227 {
1228 if (d_upper >= (data_length - 1U))
1229 {
1230 upper_done = true;
1231 }
1232 else if (!acc_alg_basic_utils_is_bit_set_bitarray_uint32(threshold_check, d_upper))
1233 {
1234 upper_done = true;
1235 }
1236 else if (abs_sweep[d_upper] > abs_sweep[i])
1237 {
1238 // Growing slope; reset the peak candidate to the new larger value.
1239 i = d_upper;
1240 d_upper++;
1241 }
1242 else if (abs_sweep[d_upper] < abs_sweep[i])
1243 {
1244 /*
1245 * Ensure that the value after a peak candidate (i) isn't below threshold; e.g:
1246 *
1247 * abs_sweep = [1, 2, 3, 4, 2, 2]
1248 * threshold = [2, 2, 2, 2, 2, 2]
1249 *
1250 * Candidate: abs_sweep[i] = 4
1251 */
1252 if (acc_alg_basic_utils_is_bit_set_bitarray_uint32(threshold_check, ((size_t)i + 1U)))
1253 {
1254 if (found_peaks < peak_idxs_length)
1255 {
1256 peak_idxs[found_peaks] = i;
1257 found_peaks++;
1258 }
1259 else
1260 {
1261 success = false;
1262 }
1263 }
1264
1265 upper_done = true;
1266 }
1267 else
1268 {
1269 d_upper++;
1270 }
1271 }
1272
1273 i = d_upper;
1274 }
1275
1276 *num_peaks = found_peaks;
1277
1278 return success;
1279 }
1280
1281 void acc_algorithm_count_points_above_threshold(const float *matrix,
1282 uint16_t rows,
1283 uint16_t cols,
1284 const float threshold,
1285 uint16_t *count,
1286 uint16_t offset,
1287 uint16_t threshold_check_length,
1288 uint16_t axis)
1289 {
1290 if (axis == 0U)
1291 {
1292 for (uint16_t r = offset; r < (threshold_check_length + offset); r++)
1293 {
1294 count[r] = 0U;
1295 for (uint16_t c = 0U; c < cols; c++)
1296 {
1297 if (matrix[c + (r * cols)] > threshold)
1298 {
1299 count[r]++;
1300 }
1301 }
1302 }
1303 }
1304 else if (axis == 1U)
1305 {
1306 for (uint16_t c = offset; c < (threshold_check_length + offset); c++)
1307 {
1308 count[c] = 0U;
1309 for (uint16_t r = 0U; r < rows; r++)
1310 {
1311 if (matrix[c + (r * cols)] > threshold)
1312 {
1313 count[c]++;
1314 }
1315 }
1316 }
1317 }
1318 else
1319 {
1320 // Do nothing
1321 }
1322 }
1323
1324 int16_t acc_algorithm_median_i16(int16_t *data, uint16_t length)
1325 {
1326 sort_i16(data, length);
1327
1328 int16_t result;
1329
1330 if ((length % 2U) == 0U)
1331 {
1332 result = (data[(length / 2U) - 1U] + data[length / 2U]) / 2;
1333 }
1334 else
1335 {
1336 result = data[length / 2U];
1337 }
1338
1339 return result;
1340 }
1341
1342 float acc_algorithm_median_f32(float *data, uint16_t length)
1343 {
1344 sort_f32(data, length);
1345
1346 float result;
1347
1348 if ((length % 2U) == 0U)
1349 {
1350 result = (data[(length / 2U) - 1U] + data[length / 2U]) / 2.0f;
1351 }
1352 else
1353 {
1354 result = data[length / 2U];
1355 }
1356
1357 return result;
1358 }
1359
1360 float acc_algorithm_max_f32(const float *data, uint16_t length)
1361 {
1362 float max_value = 0.0f;
1363
1364 for (uint16_t i = 0; i < length; i++)
1365 {
1366 if (data[i] > max_value)
1367 {
1368 max_value = data[i];
1369 }
1370 }
1371
1372 return max_value;
1373 }
1374
1375 float acc_algorithm_weighted_mean(const float *data, const float *weights, uint16_t length)
1376 {
1377 float weight_sum = 0.0f;
1378 float weighted_data_sum = 0.0f;
1379 float weighted_mean = 0.0f;
1380
1381 for (uint16_t i = 0; i < length; i++)
1382 {
1383 weight_sum += weights[i];
1384 weighted_data_sum += weights[i] * data[i];
1385 }
1386
1387 if (weight_sum > 0.0f)
1388 {
1389 weighted_mean = weighted_data_sum / weight_sum;
1390 }
1391 else
1392 {
1393 weighted_mean = data[0];
1394 }
1395
1396 return weighted_mean;
1397 }
1398
1399 float acc_algorithm_variance_f32(const float *data, uint16_t length)
1400 {
1401 float sum = 0.0f;
1402 float sq_err_sum = 0.0f;
1403 float variance = 0.0f;
1404
1405 if (length > 1U)
1406 {
1407 for (uint16_t i = 0U; i < length; i++)
1408 {
1409 sum += data[i];
1410 }
1411
1412 float mean = sum / (float)length;
1413
1414 for (uint16_t i = 0U; i < length; i++)
1415 {
1416 float error = data[i] - mean;
1417 sq_err_sum += error * error;
1418 }
1419
1420 variance = sq_err_sum / (float)length;
1421 }
1422
1423 return variance;
1424 }
1425
1426 float acc_algorithm_stddev_f32(const float *data, uint16_t length)
1427 {
1428 return sqrtf(acc_algorithm_variance_f32(data, length));
1429 }
1430
1431 float acc_algorithm_clip_f32(float value, float min, float max)
1432 {
1433 float res;
1434
1435 if (value > max)
1436 {
1437 res = max;
1438 }
1439 else if (value < min)
1440 {
1441 res = min;
1442 }
1443 else
1444 {
1445 res = value;
1446 }
1447
1448 return res;
1449 }
1450
1451 //-----------------------------
1452 // Private definitions
1453 //-----------------------------
1454
1455 static void sum_i16_complex(const acc_int16_complex_t *data, uint16_t num_steps, float complex *out, uint16_t stride)
1456 {
1457 float real_sum = 0.0f;
1458 float imag_sum = 0.0f;
1459
1460 for (uint16_t i = 0U; i < num_steps; i++)
1461 {
1462 real_sum += (float)data[i * stride].real;
1463 imag_sum += (float)data[i * stride].imag;
1464 }
1465
1466 *out = real_sum + (imag_sum * I);
1467 }
1468
1469 static void mean_i16_complex(const acc_int16_complex_t *data, uint16_t num_steps, float complex *out, uint16_t stride)
1470 {
1471 sum_i16_complex(data, num_steps, out, stride);
1472
1473 float real_mean = crealf(*out) / (float)num_steps;
1474 float imag_mean = cimagf(*out) / (float)num_steps;
1475
1476 *out = real_mean + (imag_mean * I);
1477 }
1478
1479 static void rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride)
1480 {
1481 small_rfft(data, data_length, length_shift - 1U, output, stride);
1482
1483 small_rfft_real_symmetry_conversion(output, length_shift - 1U, stride);
1484
1485 output[(((uint16_t)1U) << (length_shift - 1U)) * stride] = cimagf(output[0]);
1486 output[0] = crealf(output[0]);
1487 }
1488
1489 static void fftshift(float *data, uint16_t data_length, uint16_t stride)
1490 {
1491 uint16_t half_data_length = (data_length + 1U) / 2U;
1492
1493 for (uint16_t i = 0U; i < half_data_length; i++)
1494 {
1495 float x = data[0];
1496
1497 for (uint16_t j = 0U; j < (data_length - 1U); j++)
1498 {
1499 data[j * stride] = data[(j * stride) + stride];
1500 }
1501
1502 data[(data_length * stride) - stride] = x;
1503 }
1504 }
1505
1506 static void welch(const float complex *data,
1507 uint16_t data_length,
1508 uint16_t segment_length,
1509 float complex *data_buffer,
1510 float complex *fft_out,
1511 float *psd,
1512 const float *window,
1513 uint16_t length_shift,
1514 float fs,
1515 uint16_t stride)
1516 {
1517 uint16_t num_segments = data_length / segment_length;
1518 float scale = 0.0f;
1519
1520 for (uint16_t i = 0U; i < num_segments; i++)
1521 {
1522 scale = 0.0f;
1523 float complex mean = 0.0f;
1524
1525 for (uint16_t j = 0U; j < segment_length; j++)
1526 {
1527 mean += data[(i * segment_length * stride) + (j * stride)];
1528 }
1529
1530 float complex adj_mean_real = crealf(mean) / (float)segment_length;
1531 float complex adj_mean_imag = cimagf(mean) / (float)segment_length;
1532 mean = adj_mean_real + (adj_mean_imag * I);
1533
1534 for (uint16_t j = 0U; j < segment_length; j++)
1535 {
1536 data_buffer[j] = data[(i * segment_length * stride) + (j * stride)] - mean;
1537
1538 float complex adj_buf_real = crealf(data_buffer[j]) * window[j];
1539 float complex adj_buf_imag = cimagf(data_buffer[j]) * window[j];
1540 data_buffer[j] = adj_buf_real + (adj_buf_imag * I);
1541
1542 scale += window[j] * window[j];
1543 }
1544
1545 acc_algorithm_fft(data_buffer, segment_length, length_shift, fft_out);
1546
1547 for (uint16_t j = 0U; j < segment_length; j++)
1548 {
1549 psd[j * stride] += cabsf(fft_out[j]) * cabsf(fft_out[j]);
1550 }
1551 }
1552
1553 if (scale != 0.0f)
1554 {
1555 scale = 1.0f / (scale * fs * (float)num_segments);
1556 }
1557
1558 for (uint16_t i = 0U; i < segment_length; i++)
1559 {
1560 psd[i * stride] *= scale;
1561 }
1562 }
1563
1564 static void filter_inplace_apply(uint16_t sample_idx, const float *b, const float *a, float state[5], float *data)
1565 {
1566 float x = data[sample_idx];
1567 float y;
1568
1569 y = state[0] + (b[0] * x);
1570
1571 state[0] = state[1] + (b[1] * x) - (a[0] * y);
1572 state[1] = state[2] + (b[2] * x) - (a[1] * y);
1573 state[2] = state[3] + (b[3] * x) - (a[2] * y);
1574 state[3] = (b[4] * x) - (a[3] * y);
1575
1576 data[sample_idx] = y;
1577 }
1578
1579 static float complex get_data_padded_f32_to_f32_complex(const float *data, uint16_t data_length, uint16_t index, uint16_t stride)
1580 {
1581 float real = 0.0f;
1582 float imag = 0.0f;
1583 uint16_t i = index * 2U;
1584
1585 if (i < data_length)
1586 {
1587 real = data[i * stride];
1588 }
1589
1590 i++;
1591
1592 if (i < data_length)
1593 {
1594 imag = data[i * stride];
1595 }
1596
1597 return real + (imag * I);
1598 }
1599
1600 static float complex get_data_padded_f32_complex(const float complex *data, uint16_t data_length, uint16_t index, uint16_t stride)
1601 {
1602 float complex res = 0.0f + 0.0f * I;
1603
1604 if (index < data_length)
1605 {
1606 res = data[index * stride];
1607 }
1608
1609 return res;
1610 }
1611
1612 static void small_rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride)
1613 {
1614 uint16_t full_data_length = ((uint16_t)1U) << length_shift;
1615
1616 if (length_shift == 0U)
1617 {
1618 // Trivial 1-element FFT
1619 output[0] = get_data_padded_f32_to_f32_complex(data, data_length, 0U, stride);
1620 }
1621 else if (length_shift == 1U)
1622 {
1623 // 2-element FFT
1624 output[0] =
1625 get_data_padded_f32_to_f32_complex(data, data_length, 0U, stride) + get_data_padded_f32_to_f32_complex(data, data_length, 1U, stride);
1626 output[1U * stride] =
1627 get_data_padded_f32_to_f32_complex(data, data_length, 0U, stride) - get_data_padded_f32_to_f32_complex(data, data_length, 1U, stride);
1628 }
1629 else
1630 {
1631 // Perform element reordering
1632 uint16_t reverse_i = 0U;
1633 for (uint16_t i = 0U; i < full_data_length; i++)
1634 {
1635 if (i < reverse_i)
1636 {
1637 float complex tmp = get_data_padded_f32_to_f32_complex(data, data_length, i, stride);
1638 output[i * stride] = get_data_padded_f32_to_f32_complex(data, data_length, reverse_i, stride);
1639 output[reverse_i * stride] = tmp;
1640 }
1641 else if (i == reverse_i)
1642 {
1643 output[i * stride] = get_data_padded_f32_to_f32_complex(data, data_length, i, stride);
1644 }
1645 else
1646 {
1647 // Do nothing
1648 }
1649
1650 uint16_t bit = full_data_length >> 1U;
1651 while ((bit & reverse_i) != 0U)
1652 {
1653 reverse_i &= ~bit;
1654 bit >>= 1U;
1655 }
1656 reverse_i |= bit;
1657 }
1658
1659 small_fft_transformation(length_shift, output, stride);
1660 }
1661 }
1662
1663 static void small_fft(const float complex *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride)
1664 {
1665 uint16_t full_data_length = ((uint16_t)1U) << length_shift;
1666
1667 if (length_shift == 0U)
1668 {
1669 // Trivial 1-element FFT
1670 output[0] = get_data_padded_f32_complex(data, data_length, 0U, stride);
1671 }
1672 else if (length_shift == 1U)
1673 {
1674 // 2-element FFT
1675 output[0] = get_data_padded_f32_complex(data, data_length, 0U, stride) + get_data_padded_f32_complex(data, data_length, 1U, stride);
1676 output[1U * stride] = get_data_padded_f32_complex(data, data_length, 0U, stride) - get_data_padded_f32_complex(data, data_length, 1U, stride);
1677 }
1678 else
1679 {
1680 // Perform element reordering
1681 uint16_t reverse_i = 0U;
1682 for (uint16_t i = 0U; i < full_data_length; i++)
1683 {
1684 if (i < reverse_i)
1685 {
1686 float complex tmp = get_data_padded_f32_complex(data, data_length, i, stride);
1687 output[i * stride] = get_data_padded_f32_complex(data, data_length, reverse_i, stride);
1688 output[reverse_i * stride] = tmp;
1689 }
1690 else if (i == reverse_i)
1691 {
1692 output[i * stride] = get_data_padded_f32_complex(data, data_length, i, stride);
1693 }
1694 else
1695 {
1696 // Do nothing
1697 }
1698
1699 uint16_t bit = full_data_length >> 1U;
1700 while ((bit & reverse_i) != 0U)
1701 {
1702 reverse_i &= ~bit;
1703 bit >>= 1U;
1704 }
1705 reverse_i |= bit;
1706 }
1707
1708 small_fft_transformation(length_shift, output, stride);
1709 }
1710 }
1711
1712 static void small_fft_transformation(uint16_t length_shift, float complex *output, uint16_t stride)
1713 {
1714 uint16_t full_data_length = ((uint16_t)1U) << length_shift;
1715
1716 // Perform 4-element base transformations
1717 for (uint16_t i = 0U; i < full_data_length; i += 4U)
1718 {
1719 float complex s0 = output[i * stride] + output[(i + 1U) * stride];
1720 float complex d0 = output[i * stride] - output[(i + 1U) * stride];
1721 float complex s1 = output[(i + 2U) * stride] + output[(i + 3U) * stride];
1722 float complex d1 = output[(i + 2U) * stride] - output[(i + 3U) * stride];
1723
1724 d1 = cimagf(d1) - (I * crealf(d1)); // d1 = -I*d1;
1725
1726 output[(i + 0U) * stride] = s0 + s1;
1727 output[(i + 2U) * stride] = s0 - s1;
1728 output[(i + 1U) * stride] = d0 + d1;
1729 output[(i + 3U) * stride] = d0 - d1;
1730 }
1731
1732 // Main part of the FFT computation
1733 uint16_t block_length = 4U;
1734 float complex phase_incr = -I;
1735
1736 while (block_length < full_data_length)
1737 {
1738 // Update the phase_incr unit vector to have half phase angle
1739 phase_incr = (phase_incr + 1.0f) / cabsf(phase_incr + 1.0f);
1740
1741 float complex phase = 1.0f;
1742 for (uint16_t m = 0U; m < block_length; m++)
1743 {
1744 for (uint16_t i = m; i < full_data_length; i += block_length << 1U)
1745 {
1746 float complex delta = output[(i + block_length) * stride] * phase;
1747
1748 output[(i + block_length) * stride] = output[i * stride] - delta;
1749
1750 output[i * stride] += delta;
1751 }
1752
1753 // This phase increment is the leading error source for large transforms
1754 phase = phase * phase_incr;
1755 }
1756
1757 block_length <<= 1U;
1758 }
1759 }
1760
1761 static void small_rfft_real_symmetry_conversion(float complex *output, uint16_t length_shift, uint16_t stride)
1762 {
1763 uint16_t full_data_length = ((uint16_t)1U) << length_shift;
1764
1765 output[0] = (1.0f + (1.0f * I)) * conjf(output[0]);
1766
1767 if (length_shift > 0U)
1768 {
1769 float complex phase_incr = I;
1770 float complex z1_factor = 0.5f * phase_incr;
1771
1772 for (uint16_t i = 1U; i < length_shift; i++)
1773 {
1774 phase_incr = (phase_incr + 1.0f) / cabsf(phase_incr + 1.0f);
1775 }
1776
1777 uint16_t mid = full_data_length / 2U;
1778 for (uint16_t i = 1U; i < mid; i++)
1779 {
1780 float complex t;
1781 float complex z0 = output[i * stride];
1782 float complex z1 = output[(full_data_length - i) * stride];
1783
1784 t = z0 + conjf(z1);
1785 z1 = conjf(z0) - z1;
1786 z0 = t;
1787
1788 z0 *= 0.5f;
1789 z1_factor = z1_factor * phase_incr;
1790 z1 = z1 * z1_factor;
1791
1792 t = z0 + conjf(z1);
1793 z1 = conjf(z0) - z1;
1794 z0 = t;
1795
1796 output[i * stride] = z0;
1797 output[(full_data_length - i) * stride] = z1;
1798 }
1799
1800 output[mid * stride] = conjf(output[mid * stride]);
1801 }
1802 }
1803
1804 static void double_buffering_median_filter(acc_int16_complex_t *frame,
1805 const uint16_t num_points,
1806 const uint16_t sweep,
1807 const uint16_t point,
1808 const uint16_t median_start_sweep)
1809 {
1810 /* Get the complex median value over an array of length 4 */
1811 int32_t point_r[4U];
1812 int32_t point_i[4U];
1813 int32_t point_abs[4U];
1814
1815 /* Calculate abs value */
1816 for (uint16_t idx = 0U; idx < 4U; idx++)
1817 {
1818 point_r[idx] = frame[((median_start_sweep + idx) * num_points) + point].real;
1819 point_i[idx] = frame[((median_start_sweep + idx) * num_points) + point].imag;
1820 point_abs[idx] = (point_r[idx] * point_r[idx]) + (point_i[idx] * point_i[idx]);
1821 }
1822
1823 uint16_t high_index = 0U;
1824 uint16_t low_index = 0U;
1825 int32_t high_val = INT32_MIN;
1826 int32_t low_val = INT32_MAX;
1827
1828 /* Find highest/lowest abs index */
1829 for (uint16_t idx = 0; idx < 4U; idx++)
1830 {
1831 if (point_abs[idx] > high_val)
1832 {
1833 high_val = point_abs[idx];
1834 high_index = idx;
1835 }
1836
1837 if (point_abs[idx] < low_val)
1838 {
1839 low_val = point_abs[idx];
1840 low_index = idx;
1841 }
1842 }
1843
1844 /* Clear highest and lowest */
1845 point_r[high_index] = 0;
1846 point_i[high_index] = 0;
1847 point_r[low_index] = 0;
1848 point_i[low_index] = 0;
1849
1850 int32_t median_real = 0;
1851 int32_t median_imag = 0;
1852
1853 /* Sum complex points */
1854 for (uint16_t idx = 0U; idx < 4U; idx++)
1855 {
1856 median_real += point_r[idx];
1857 median_imag += point_i[idx];
1858 }
1859
1860 /* Update frame with median filtered value */
1861 median_real = median_real / 2;
1862 median_imag = median_imag / 2;
1863
1864 frame[(sweep * num_points) + point].real = (int16_t)median_real;
1865 frame[(sweep * num_points) + point].imag = (int16_t)median_imag;
1866 }
1867
1868 static void double_buffering_interpolate(acc_int16_complex_t *frame,
1869 const uint16_t sweeps_per_frame,
1870 const uint16_t num_points,
1871 const uint16_t sweep,
1872 const uint16_t point)
1873 {
1874 /* 2/3 of the sweep value before */
1875 int32_t interpolate_real_i32 = frame[((sweep - 1U) * num_points) + point].real;
1876 int32_t interpolate_imag_i32 = frame[((sweep - 1U) * num_points) + point].imag;
1877
1878 interpolate_real_i32 = interpolate_real_i32 * 2;
1879 interpolate_imag_i32 = interpolate_imag_i32 * 2;
1880
1881 /* 1/3 of the sweep value two positions ahead */
1882 uint16_t sweep_idx = sweep + 2U;
1883
1884 if (sweep_idx > (sweeps_per_frame - 1U))
1885 {
1886 sweep_idx = sweeps_per_frame - 1U;
1887 }
1888
1889 interpolate_real_i32 += frame[((sweep_idx)*num_points) + point].real;
1890 interpolate_imag_i32 += frame[((sweep_idx)*num_points) + point].imag;
1891
1892 interpolate_real_i32 = interpolate_real_i32 / 3;
1893 interpolate_imag_i32 = interpolate_imag_i32 / 3;
1894
1895 /* Update frame with interpolated value */
1896 frame[(sweep * num_points) + point].real = (int16_t)interpolate_real_i32;
1897 frame[(sweep * num_points) + point].imag = (int16_t)interpolate_imag_i32;
1898 }
1899
1900 static void merge_peak_cluster(uint16_t start_idx,
1901 uint16_t num_peaks,
1902 const float *velocities,
1903 const float *energies,
1904 const uint16_t *peak_idxs,
1905 float *merged_velocities,
1906 float *merged_energies,
1907 uint16_t cluster_count)
1908 {
1909 float min = INFINITY;
1910 float max = -INFINITY;
1911
1912 for (uint16_t i = 0U; i < num_peaks; i++)
1913 {
1914 merged_velocities[cluster_count] += velocities[peak_idxs[start_idx + i]];
1915 merged_energies[cluster_count] += energies[peak_idxs[start_idx + i]];
1916
1917 min = fminf(velocities[peak_idxs[start_idx + i]], min);
1918
1919 max = fmaxf(velocities[peak_idxs[start_idx + i]], max);
1920 }
1921
1922 merged_velocities[cluster_count] /= (float)num_peaks;
1923 merged_energies[cluster_count] /= (float)num_peaks;
1924 }
1925
1926 static float max_measurable_dist(acc_config_prf_t prf)
1927 {
1928 float mmd;
1929
1930 switch (prf)
1931 {
1932 case ACC_CONFIG_PRF_19_5_MHZ:
1933 mmd = 3.1f;
1934 break;
1935 case ACC_CONFIG_PRF_15_6_MHZ:
1936 mmd = 5.1f;
1937 break;
1938 case ACC_CONFIG_PRF_13_0_MHZ:
1939 mmd = 7.0f;
1940 break;
1941 case ACC_CONFIG_PRF_8_7_MHZ:
1942 mmd = 12.7f;
1943 break;
1944 case ACC_CONFIG_PRF_6_5_MHZ:
1945 mmd = 18.5f;
1946 break;
1947 case ACC_CONFIG_PRF_5_2_MHZ:
1948 mmd = 24.2f;
1949 break;
1950 default:
1951 mmd = 0.0f;
1952 break;
1953 }
1954
1955 return mmd;
1956 }
1957
1958 static acc_config_profile_t get_profile(uint16_t value)
1959 {
1960 acc_config_profile_t profile = ACC_CONFIG_PROFILE_3;
1961
1962 switch (value)
1963 {
1964 case 1U:
1965 profile = ACC_CONFIG_PROFILE_1;
1966 break;
1967
1968 case 2U:
1969 profile = ACC_CONFIG_PROFILE_2;
1970 break;
1971
1972 case 4U:
1973 profile = ACC_CONFIG_PROFILE_4;
1974 break;
1975
1976 case 5U:
1977 profile = ACC_CONFIG_PROFILE_5;
1978 break;
1979
1980 default:
1981 break;
1982 }
1983
1984 return profile;
1985 }
1986
1987 static void sort_i16(int16_t *array, uint16_t array_length)
1988 {
1989 for (uint16_t i = 0; i < (array_length - 1U); i++)
1990 {
1991 for (uint16_t j = 0; j < (array_length - i - 1U); j++)
1992 {
1993 if (array[j] > array[j + 1U])
1994 {
1995 swap_i16(array, j, j + 1U);
1996 }
1997 }
1998 }
1999 }
2000
2001 static void swap_i16(int16_t *array, uint16_t idx_a, uint16_t idx_b)
2002 {
2003 int16_t tmp = array[idx_a];
2004
2005 array[idx_a] = array[idx_b];
2006 array[idx_b] = tmp;
2007 }
2008
2009 static void sort_f32(float *array, uint16_t array_length)
2010 {
2011 for (uint16_t i = 0; i < (array_length - 1U); i++)
2012 {
2013 for (uint16_t j = 0; j < (array_length - i - 1U); j++)
2014 {
2015 if (array[j] > array[j + 1U])
2016 {
2017 swap_f32(array, j, j + 1U);
2018 }
2019 }
2020 }
2021 }
2022
2023 static void swap_f32(float *array, uint16_t idx_a, uint16_t idx_b)
2024 {
2025 float tmp = array[idx_a];
2026
2027 array[idx_a] = array[idx_b];
2028 array[idx_b] = tmp;
2029 }
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:664
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:1564
acc_algorithm_fft
void acc_algorithm_fft(const float complex *data, uint16_t data_length, uint16_t length_shift, float complex *output)
1D Fast Fourier Transform for complex input
Definition: acc_algorithm.c:622
acc_algorithm_variance_f32
float acc_algorithm_variance_f32(const float *data, uint16_t length)
Calculate variance.
Definition: acc_algorithm.c:1399
DOUBLE_BUFFERING_MEAN_ABS_DEV_OUTLIER_TH
#define DOUBLE_BUFFERING_MEAN_ABS_DEV_OUTLIER_TH
Definition: acc_algorithm.c:18
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:305
acc_algorithm_conj_f32
void acc_algorithm_conj_f32(float complex *data, uint16_t data_length)
Inline calculate conjugate of all elements in an array.
Definition: acc_algorithm.c:576
acc_algorithm_cdiv
float complex acc_algorithm_cdiv(float complex num, float complex denom)
Divide complex number num / denum.
Definition: acc_algorithm.c:708
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:733
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:1144
ACC_CONFIG_PROFILE_4
@ ACC_CONFIG_PROFILE_4
Definition: acc_definitions_a121.h:55
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:1579
ACC_CONFIG_PRF_8_7_MHZ
@ ACC_CONFIG_PRF_8_7_MHZ
Definition: acc_definitions_a121.h:120
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:72
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:456
acc_algorithm_median_i16
int16_t acc_algorithm_median_i16(int16_t *data, uint16_t length)
Calculate median of input data.
Definition: acc_algorithm.c:1324
ACC_CONFIG_PRF_6_5_MHZ
@ ACC_CONFIG_PRF_6_5_MHZ
Definition: acc_definitions_a121.h:122
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:849
acc_alg_basic_utils.h
sort_f32
static void sort_f32(float *array, uint16_t array_length)
Definition: acc_algorithm.c:2009
acc_int16_complex_t
Data type for interger-based representation of complex numbers.
Definition: acc_definitions_common.h:40
acc_algorithm_roll_and_push_matrix_f32_complex
void acc_algorithm_roll_and_push_matrix_f32_complex(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:203
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:745
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:1761
welch
static void 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)
Definition: acc_algorithm.c:1506
sum_i16_complex
static void sum_i16_complex(const acc_int16_complex_t *data, uint16_t num_steps, float complex *out, uint16_t stride)
Calculates the sum of elements of data located at (i * stride) for i in [0, num_steps)
Definition: acc_algorithm.c:1455
ACC_CONFIG_PROFILE_5
@ ACC_CONFIG_PROFILE_5
Definition: acc_definitions_a121.h:57
acc_algorithm_exp_smoothing_coefficient
float acc_algorithm_exp_smoothing_coefficient(float fs, float tc)
Calculate exponential smoothing coefficient.
Definition: acc_algorithm.c:694
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:1804
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:597
sort_i16
static void sort_i16(int16_t *array, uint16_t array_length)
Definition: acc_algorithm.c:1987
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:446
acc_algorithm_count_points_above_threshold
void acc_algorithm_count_points_above_threshold(const float *matrix, uint16_t rows, uint16_t cols, const float threshold, uint16_t *count, uint16_t offset, uint16_t threshold_check_length, uint16_t axis)
Count points in matrix above threshold row-wise or col-wise.
Definition: acc_algorithm.c:1281
ACC_CONFIG_PROFILE_2
@ ACC_CONFIG_PROFILE_2
Definition: acc_definitions_a121.h:53
acc_algorithm_sum_sweep
void acc_algorithm_sum_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 *sum_sweep)
Calculate the sum of all sweeps in a frame, from start_point to end_point.
Definition: acc_algorithm.c:536
acc_algorithm_stddev_f32
float acc_algorithm_stddev_f32(const float *data, uint16_t length)
Calculate standard deviation.
Definition: acc_algorithm.c:1426
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:1044
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:1118
acc_algorithm_fftshift
void acc_algorithm_fftshift(float *data, uint16_t data_length)
Shift the zero-frequency component to the center.
Definition: acc_algorithm.c:857
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:523
acc_algorithm_mean_i16_complex
void acc_algorithm_mean_i16_complex(const acc_int16_complex_t *data, uint16_t data_length, float complex *out)
Calculate mean value of an array.
Definition: acc_algorithm.c:549
mean_i16_complex
static void mean_i16_complex(const acc_int16_complex_t *data, uint16_t num_steps, float complex *out, uint16_t stride)
Calculates the mean of elements of data located at (i * stride) for i in [0, num_steps)
Definition: acc_algorithm.c:1469
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:652
acc_algorithm_clip_f32
float acc_algorithm_clip_f32(float value, float min, float max)
Clip passed value into the interval [min, max].
Definition: acc_algorithm.c:1431
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:627
ACC_CONFIG_PRF_5_2_MHZ
@ ACC_CONFIG_PRF_5_2_MHZ
Definition: acc_definitions_a121.h:124
acc_config_prf_t
acc_config_prf_t
Pulse Repetition Frequency.
Definition: acc_definitions_a121.h:111
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:159
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:1125
acc_algorithm_weighted_mean
float acc_algorithm_weighted_mean(const float *data, const float *weights, uint16_t length)
Calculate weighted mean.
Definition: acc_algorithm.c:1375
acc_algorithm_apply_filter_f32_complex
void acc_algorithm_apply_filter_f32_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:491
acc_algorithm_calculate_cfar
float acc_algorithm_calculate_cfar(const float *data, uint16_t data_length, uint16_t window_length, uint16_t half_guard_length, float sensitivity, uint16_t idx)
Calculate CFAR threshold.
Definition: acc_algorithm.c:892
ACC_CONFIG_PRF_15_6_MHZ
@ ACC_CONFIG_PRF_15_6_MHZ
Definition: acc_definitions_a121.h:116
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:721
small_fft_transformation
static void small_fft_transformation(uint16_t length_shift, float complex *output, uint16_t stride)
Definition: acc_algorithm.c:1712
acc_algorithm_roll_and_push_mult_matrix_i16_complex
void acc_algorithm_roll_and_push_mult_matrix_i16_complex(acc_int16_complex_t *data, uint16_t data_rows, uint16_t cols, const acc_int16_complex_t *matrix, uint16_t matrix_rows, bool pos_shift)
Roll row elements and push multiple columns.
Definition: acc_algorithm.c:237
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:1600
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)
Estimate power spectral density using Welch’s method.
Definition: acc_algorithm.c:879
result
cargo_result_t result
Definition: i2c_example_cargo.c:43
acc_int16_complex_t::real
int16_t real
Definition: acc_definitions_common.h:42
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:1900
M_PI
#define M_PI
Definition: acc_alg_basic_utils.h:14
rfft
static void rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride)
Definition: acc_algorithm.c:1479
swap_i16
static void swap_i16(int16_t *array, uint16_t idx_a, uint16_t idx_b)
Definition: acc_algorithm.c:2001
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:330
fftshift
static void fftshift(float *data, uint16_t data_length, uint16_t stride)
Definition: acc_algorithm.c:1489
acc_algorithm_rfft
void acc_algorithm_rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output)
1D Fast Fourier Transform for real input
Definition: acc_algorithm.c:592
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:379
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:1178
acc_algorithm_interpolate_peaks
float acc_algorithm_interpolate_peaks(const float *y, const float *x)
Interpolate peak.
Definition: acc_algorithm.c:322
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:283
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:1663
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:675
get_profile
static acc_config_profile_t get_profile(uint16_t value)
Definition: acc_algorithm.c:1958
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:1612
acc_algorithm.h
acc_algorithm_apply_filter_f32
void acc_algorithm_apply_filter_f32(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:464
acc_int16_complex_t::imag
int16_t imag
Definition: acc_definitions_common.h:43
acc_config_profile_t
acc_config_profile_t
Profile.
Definition: acc_definitions_a121.h:49
acc_definitions_common.h
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:337
ACC_CONFIG_PRF_13_0_MHZ
@ ACC_CONFIG_PRF_13_0_MHZ
Definition: acc_definitions_a121.h:118
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:1868
acc_algorithm_mean_matrix_i16_complex
void acc_algorithm_mean_matrix_i16_complex(const acc_int16_complex_t *matrix, uint16_t rows, uint16_t cols, float complex *out, uint16_t axis)
Calculate mean array of a matrix.
Definition: acc_algorithm.c:554
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:1009
max_measurable_dist
static float max_measurable_dist(acc_config_prf_t prf)
Get max measurable distance for some PRF.
Definition: acc_algorithm.c:1926
swap_f32
static void swap_f32(float *array, uint16_t idx_a, uint16_t idx_b)
Definition: acc_algorithm.c:2023
acc_algorithm_calculate_mirrored_one_sided_cfar
float acc_algorithm_calculate_mirrored_one_sided_cfar(const float *data, uint16_t data_length, uint16_t middle_idx, uint16_t window_length, uint16_t half_guard_length, float sensitivity, uint16_t idx)
Calculate mirrored one sided CFAR threshold.
Definition: acc_algorithm.c:928
ACC_CONFIG_PROFILE_1
@ ACC_CONFIG_PROFILE_1
Definition: acc_definitions_a121.h:52
acc_algorithm_max_f32
float acc_algorithm_max_f32(const float *data, uint16_t length)
Find max value of input data.
Definition: acc_algorithm.c:1360
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:862
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:983
acc_algorithm_normalize_f32_complex
void acc_algorithm_normalize_f32_complex(float complex *data, uint16_t data_length)
Normalize all elements in an array individually.
Definition: acc_algorithm.c:584
ACC_CONFIG_PRF_19_5_MHZ
@ ACC_CONFIG_PRF_19_5_MHZ
Definition: acc_definitions_a121.h:114
ACC_CONFIG_PROFILE_3
@ ACC_CONFIG_PROFILE_3
Definition: acc_definitions_a121.h:54
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:774
acc_algorithm_median_f32
float acc_algorithm_median_f32(float *data, uint16_t length)
Calculate median of input data.
Definition: acc_algorithm.c:1342
acc_definitions_a121.h
acc_algorithm_roll_and_push_matrix_f32
void acc_algorithm_roll_and_push_matrix_f32(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:169