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  */
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  */
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 
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 
633 {
634  float fwhm;
635 
636  switch (profile)
637  {
639  fwhm = 0.04f;
640  break;
642  fwhm = 0.07f;
643  break;
645  fwhm = 0.14f;
646  break;
648  fwhm = 0.19f;
649  break;
651  fwhm = 0.32f;
652  break;
653  default:
654  fwhm = 0.0f;
655  break;
656  }
657 
658  return fwhm;
659 }
660 
661 
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 {
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  {
1081  }
1082  else if (breakpoint_m < max_measurable_dist(ACC_CONFIG_PRF_15_6_MHZ))
1083  {
1085  }
1086  else if (breakpoint_m < max_measurable_dist(ACC_CONFIG_PRF_13_0_MHZ))
1087  {
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 
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 
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 
1591 {
1592  float mmd;
1593 
1594  switch (prf)
1595  {
1597  mmd = 3.1f;
1598  break;
1600  mmd = 5.1f;
1601  break;
1603  mmd = 7.0f;
1604  break;
1606  mmd = 12.7f;
1607  break;
1609  mmd = 18.5f;
1610  break;
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 {
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