17 #define DOUBLE_BUFFERING_MEAN_ABS_DEV_OUTLIER_TH 5
33 static void small_rfft(
const float *data, uint16_t data_length, uint16_t length_shift,
float complex *output, uint16_t stride);
36 static void small_fft(
const float complex *data, uint16_t data_length, uint16_t length_shift,
float complex *output, uint16_t stride);
58 const uint16_t sweeps_per_frame,
59 const uint16_t num_points,
61 const uint16_t point);
76 const uint16_t num_points,
79 const uint16_t median_start_sweep);
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);
124 for (uint16_t i = 1U; i < data_length; i++)
126 data[i - 1U] = data[i];
129 data[data_length - 1U] = element;
137 for (uint16_t r = rows - 1U; r > 0U; r--)
139 for (uint16_t c = 0U; c < cols; c++)
141 data[(r * cols) + c] = data[((r - 1U) * cols) + c];
145 for (uint16_t c = 0U; c < cols; c++)
152 for (uint16_t r = 1U; r < rows; r++)
154 for (uint16_t c = 0U; c < cols; c++)
156 data[((r - 1U) * cols) + c] = data[(r * cols) + c];
160 for (uint16_t c = 0U; c < cols; c++)
162 data[((rows - 1U) * cols) + c] = column[c];
173 for (uint16_t r = rows - 1U; r > 0U; r--)
175 for (uint16_t c = 0U; c < cols; c++)
177 data[(r * cols) + c] = data[((r - 1U) * cols) + c];
181 for (uint16_t c = 0U; c < cols; c++)
188 for (uint16_t r = 1U; r < rows; r++)
190 for (uint16_t c = 0U; c < cols; c++)
192 data[((r - 1U) * cols) + c] = data[(r * cols) + c];
196 for (uint16_t c = 0U; c < cols; c++)
198 data[((rows - 1U) * cols) + c] = column[c];
206 for (uint16_t i = 1U; i < data_length; i++)
208 float diff = data[i] - data[i - 1U];
210 while ((diff > (
float)
M_PI) || (diff < -(
float)
M_PI))
212 if (diff > (
float)
M_PI)
214 data[i] -= 2.0f * (float)
M_PI;
218 data[i] += 2.0f * (float)
M_PI;
221 diff = data[i] - data[i - 1U];
230 float max = data[idx];
232 for (uint16_t i = 1U; i < data_length; i++)
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]));
251 return -b / (2.0f * a);
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]));
259 return x_start + (((float)peak_idx + peak_offset) * x_delta);
265 float factor = (2.0f * freq) / fs;
270 p[0] = -cexpf(((-1.0f * (
float)
M_PI) / 4.0f) * I);
271 p[1] = -cexpf(((1.0f * (
float)
M_PI) / 4.0f) * I);
274 factor = 4.0f * tanf(((
float)
M_PI * factor) / 2.0f);
277 p[0] = (
float complex)factor * p[0];
278 p[1] = (
float complex)factor * p[1];
281 float k = factor * factor;
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]);
295 float z_z[2] = {-1.0f, -1.0f};
298 a[0] = -(p[0] + p[1]);
301 b[1] = -k * (z_z[0] + z_z[1]);
302 b[2] = k * (z_z[0] * z_z[1]);
308 float min_f = (2.0f * min_freq) / fs;
309 float max_f = (2.0f * max_freq) / fs;
314 p[0] = -cexpf(((-1.0f * (
float)
M_PI) / 4.0f) * I);
315 p[1] = -cexpf(((1.0f * (
float)
M_PI) / 4.0f) * I);
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);
323 float bw = max_f - min_f;
324 float complex w0 = (
float complex)sqrtf(min_f * max_f);
327 float complex scale = (
float complex)(bw / 2.0f);
333 float complex p_bp[4];
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);
342 float k_bp = k * bw * bw;
345 float complex real_four = (
float complex)4.0f;
346 float complex p_z[4];
354 float z_z[4] = {1.0f, 1.0f, -1.0f, -1.0f};
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]);
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];
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]);
377 float filter_states[5] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
379 for (uint16_t i = 0U; i < data_length; i++)
388 for (uint16_t i = 0U; i < rows; i++)
396 const float *data, uint16_t data_rows, uint16_t data_cols,
float *output, uint16_t output_length)
398 for (uint16_t i = 0U; i < output_length; i++)
402 for (uint16_t r = 0U; r < filt_rows; r++)
404 output[i] -= a[r] * filt_data[i + (r * filt_cols)];
407 for (uint16_t r = 0U; r < data_rows; r++)
409 output[i] += b[r] * data[i + (r * data_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)
419 for (uint16_t i = 0U; i < output_length; i++)
424 for (uint16_t r = 0U; r < filt_rows; r++)
426 real -= a[r] * crealf(filt_data[i + (r * filt_cols)]);
427 imag -= a[r] * cimagf(filt_data[i + (r * filt_cols)]);
430 for (uint16_t r = 0U; r < data_rows; r++)
432 real += b[r] * crealf(data[i + (r * data_cols)]);
433 imag += b[r] * cimagf(data[i + (r * data_cols)]);
436 output[i] = real + (imag * I);
442 uint16_t end_point,
float complex *sweep)
444 for (uint16_t n = start_point; n < end_point; n++)
448 for (uint16_t s = 0U; s < sweeps_per_frame; s++)
451 real += (float)sample.
real;
452 imag += (
float)sample.
imag;
455 real /= (float)sweeps_per_frame;
456 imag /= (float)sweeps_per_frame;
458 sweep[n - start_point] = real + (imag * I);
463 void acc_algorithm_rfft(
const float *data, uint16_t data_length, uint16_t length_shift,
float complex *output, uint16_t stride)
465 small_rfft(data, data_length, length_shift - 1U, output, stride);
469 output[(((uint16_t)1U) << (length_shift - 1U)) * stride] = cimagf(output[0]);
470 output[0] = crealf(output[0]);
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)
476 uint16_t full_cols = ((uint16_t)1U) << length_shift;
480 uint16_t output_cols = (full_cols / 2U) + 1U;
481 for (uint16_t i = 0U; i < rows; i++)
483 acc_algorithm_rfft(&data[i * cols], cols, length_shift, &output[i * output_cols], 1U);
488 for (uint16_t i = 0U; i < cols; i++)
500 void acc_algorithm_fft(
const float complex *data, uint16_t data_length, uint16_t length_shift,
float complex *output, uint16_t stride)
502 small_fft(data, data_length, length_shift, output, stride);
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)
508 uint16_t full_cols = ((uint16_t)1U) << length_shift;
512 uint16_t output_cols = full_cols;
513 for (uint16_t i = 0U; i < rows; i++)
515 acc_algorithm_fft(&data[i * cols], cols, length_shift, &output[i * output_cols], 1U);
520 for (uint16_t i = 0U; i < cols; i++)
536 if ((n > 0U) && (d > 0.0f))
538 df = 1.0f / ((float)n * d);
547 uint16_t n_freqs = (n / 2U) + 1U;
550 for (uint16_t i = 0U; i < n_freqs; i++)
552 freqs[i] = (float)i * df;
560 uint16_t mid = n / 2U;
562 for (uint16_t i = 0U; i < mid; i++)
564 freqs[i] = (float)i * df;
567 int32_t n_int32 = (int32_t)n;
569 for (int32_t i = (int32_t)mid; i < n_int32; i++)
571 int32_t diff = i - n_int32;
572 freqs[i] = (float)diff * df;
581 if ((fs != 0.0f) && (tc != 0.0f))
583 float dt = 1.0f / fs;
585 res = expf(-dt / tc);
594 double a = (double)crealf(denom);
595 double b = (double)cimagf(denom);
596 double c = (double)crealf(num);
597 double d = (double)cimagf(num);
599 float real = (float)(((c * a) + (b * d)) / ((a * a) + (b * b)));
600 float imag = (float)(((a * d) - (c * b)) / ((a * a) + (b * b)));
602 return real + (imag * I);
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);
612 for (uint16_t i = 0U; i < n; i++)
614 window[i] = a - (b * cosf((
float)i * factor));
621 const float a = 0.5f;
623 const float factor = (2.0f * (float)
M_PI) / (float)n;
625 for (uint16_t i = 0U; i < n; i++)
627 window[i] = a - (a * cosf((
float)i * factor));
663 const uint16_t sweeps_per_frame,
664 const uint16_t num_points,
665 int32_t *work_buffer)
667 int32_t first_diff_r[2U];
668 int32_t first_diff_i[2U];
670 if (sweeps_per_frame >= 32U)
672 for (uint16_t point = 0U; point < num_points; point++)
674 int32_t abs_mad_sum = 0;
676 for (uint16_t sweep = 0U; sweep < (sweeps_per_frame - 2U); sweep++)
679 for (uint16_t idx = 0U; idx < 2U; idx++)
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;
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];
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;
699 work_buffer[sweep] = abs_r + abs_i;
702 abs_mad_sum += work_buffer[sweep];
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;
711 for (uint16_t sweep = 1U; sweep < (sweeps_per_frame - 1U); sweep++)
713 if (work_buffer[sweep - 1U] <= threshold)
723 else if (sweep == (sweeps_per_frame - 2U))
740 for (uint16_t i = 0U; i < cols; i++)
749 uint16_t half_data_length = (data_length + 1U) / 2U;
751 for (uint16_t i = 0U; i < half_data_length; i++)
755 for (uint16_t j = 0U; j < (data_length - 1U); j++)
757 data[j * stride] = data[(j * stride) + stride];
760 data[(data_length * stride) - stride] = x;
768 uint16_t segment_length,
769 float complex *data_buffer,
770 float complex *fft_out,
773 uint16_t length_shift,
776 for (uint16_t i = 0U; i < cols; i++)
778 acc_algorithm_welch(&(data[i]), rows, segment_length, data_buffer, fft_out, &(psds[i]), window, length_shift, fs,
785 uint16_t data_length,
786 uint16_t segment_length,
787 float complex *data_buffer,
788 float complex *fft_out,
791 uint16_t length_shift,
795 uint16_t num_segments = data_length / segment_length;
798 for (uint16_t i = 0U; i < num_segments; i++)
801 float complex mean = 0.0f;
803 for (uint16_t j = 0U; j < segment_length; j++)
805 mean += data[(i * segment_length * stride) + (j * stride)];
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);
812 for (uint16_t j = 0U; j < segment_length; j++)
814 data_buffer[j] = data[(i * segment_length * stride) + (j * stride)] - mean;
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);
820 scale += window[j] * window[j];
825 for (uint16_t j = 0U; j < segment_length; j++)
827 psd[j * stride] += cabsf(fft_out[j]) *
834 scale = 1.0f / (scale * fs * (float)num_segments);
837 for (uint16_t i = 0U; i < segment_length; i++)
839 psd[i * stride] *= scale;
845 uint16_t data_length,
847 uint16_t cfar_win_length,
848 uint16_t cfar_guard_length,
849 float cfar_sensitivity,
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);
855 float min = INFINITY;
857 for (uint16_t i = 0U; i < data_length; i++)
859 min = fminf(data[i], min);
866 for (uint16_t j = 0U; j < cfar_win_length; j++)
872 if ((idx > margin) && (idx < middle_idx))
874 for (uint16_t j = 0U; j < cfar_win_length; j++)
876 sum += data[j + (idx - margin)];
880 if ((idx >= middle_idx) && (idx < (data_length - margin - 1U)))
882 for (uint16_t j = 0U; j < cfar_win_length; j++)
884 sum += data[data_length - half_sweep_len_without_margin - j + idx - middle_idx];
888 if (idx >= (data_length - margin - 1U))
890 for (uint16_t j = 0U; j < cfar_win_length; j++)
892 sum += data[data_length - j - 1U];
896 return ((sum / (
float)cfar_win_length) + min) / cfar_sensitivity;
902 float max = -INFINITY;
906 for (uint16_t i = 0U; i < rows; i++)
908 if ((i < (middle_idx + half_slow_zone)) && (i >= (middle_idx - half_slow_zone)))
913 for (uint16_t j = 0U; j < cols; j++)
915 if (data[(i * cols) + j] > max)
917 max = data[(i * cols) + j];
928 const float *energies,
929 const uint16_t *peak_idxs,
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;
939 for (uint16_t i = 0U; i < num_peaks; i++)
941 uint16_t idx = (peak_idxs != NULL) ? peak_idxs[i] : i;
943 if (energies[idx] > biggest_energy_slow)
945 if (fabsf(velocities[idx]) < limit)
947 slow_vs = velocities[idx];
948 biggest_energy_slow = energies[idx];
952 if (energies[i] > biggest_energy_valid)
954 if (fabsf(velocities[idx]) >= limit)
956 valid_vs = velocities[idx];
957 biggest_energy_valid = energies[idx];
963 return has_valid ? valid_vs : slow_vs;
968 const float *velocities,
969 const float *energies,
970 const uint16_t *peak_idxs,
972 float *merged_velocities,
973 float *merged_energies,
974 uint16_t merged_peaks_length,
975 uint16_t *num_merged_peaks)
978 uint16_t cluster_count = 0U;
979 uint16_t cluster_start_idx = 0U;
983 for (uint16_t i = 0U; i < (num_peaks - 1U); i++)
985 uint16_t current_idx = peak_idxs[i];
986 uint16_t next_idx = peak_idxs[i + 1U];
988 uint16_t num_peaks_in_cluster = i - cluster_start_idx + 1U;
990 bool current_peak_is_in_cluster = (velocities[next_idx] - velocities[current_idx]) < max_peak_separation;
992 if (current_peak_is_in_cluster)
997 status = cluster_count < merged_peaks_length;
1001 merge_peak_cluster(cluster_start_idx, num_peaks_in_cluster, velocities, energies, peak_idxs,
1002 merged_velocities, merged_energies, cluster_count);
1011 cluster_start_idx = i + 1U;
1015 bool last_cluster_not_merged = cluster_start_idx < num_peaks;
1017 if (status && last_cluster_not_merged)
1019 status = cluster_count < merged_peaks_length;
1024 energies, peak_idxs,
1025 merged_velocities, merged_energies, cluster_count);
1036 *num_merged_peaks = cluster_count;
1045 uint16_t steps = (idx * step_length) + start_point;
1047 return (
float)steps * base_step_length_m;
1057 float MIN_DIST_M[5] = {-1.0f, 0.07f * 2.0f, 0.14f * 2.0f, 0.19f * 2.0f, 0.32f * 2.0f};
1061 if ((MIN_DIST_M[i - 1U] == -1.0f) || (MIN_DIST_M[i - 1U] <= ((float)start_point * base_step_length)))
1073 float breakpoint_p = (float)breakpoint;
1074 float breakpoint_m = breakpoint_p * base_step_length;
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)
1110 bool success =
true;
1111 uint16_t found_peaks = 0U;
1114 while (i < data_length)
1132 if (abs_sweep[i - 1U] >= abs_sweep[i])
1148 uint16_t d_upper = i + 1U;
1149 bool upper_done =
false;
1153 if (d_upper >= (data_length - 1U))
1161 else if (abs_sweep[d_upper] > abs_sweep[i])
1167 else if (abs_sweep[d_upper] < abs_sweep[i])
1179 if (found_peaks < peak_idxs_length)
1181 peak_idxs[found_peaks] = i;
1201 *num_peaks = found_peaks;
1213 float x = data[sample_idx];
1216 y = state[0] + (b[0] * x);
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);
1223 data[sample_idx] = y;
1231 uint16_t i = index * 2U;
1233 if (i < data_length)
1235 real = data[i * stride];
1240 if (i < data_length)
1242 imag = data[i * stride];
1245 return real + (imag * I);
1251 float complex res = 0.0f + 0.0f * I;
1253 if (index < data_length)
1255 res = data[index * stride];
1262 static void small_rfft(
const float *data, uint16_t data_length, uint16_t length_shift,
float complex *output, uint16_t stride)
1264 uint16_t full_data_length = ((uint16_t)1U) << length_shift;
1266 if (length_shift == 0U)
1271 else if (length_shift == 1U)
1284 uint16_t reverse_i = 0U;
1285 for (uint16_t i = 0U; i < full_data_length; i++)
1291 output[reverse_i*stride] = tmp;
1293 else if (i == reverse_i)
1302 uint16_t bit = full_data_length >> 1U;
1303 while ((bit & reverse_i) != 0U)
1316 static void small_fft(
const float complex *data, uint16_t data_length, uint16_t length_shift,
float complex *output, uint16_t stride)
1318 uint16_t full_data_length = ((uint16_t)1U) << length_shift;
1320 if (length_shift == 0U)
1325 else if (length_shift == 1U)
1336 uint16_t reverse_i = 0U;
1337 for (uint16_t i = 0U; i < full_data_length; i++)
1343 output[reverse_i*stride] = tmp;
1345 else if (i == reverse_i)
1354 uint16_t bit = full_data_length >> 1U;
1355 while ((bit & reverse_i) != 0U)
1370 uint16_t full_data_length = ((uint16_t)1U) << length_shift;
1373 for (uint16_t i = 0U; i < full_data_length; i += 4U)
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];
1380 d1 = cimagf(d1) - (I * crealf(d1));
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;
1389 uint16_t block_length = 4U;
1390 float complex phase_incr = -I;
1392 while (block_length < full_data_length)
1395 phase_incr = (phase_incr + 1.0f) / cabsf(phase_incr + 1.0f);
1397 float complex phase = 1.0f;
1398 for (uint16_t m = 0U; m < block_length; m++)
1400 for (uint16_t i = m; i < full_data_length; i += block_length << 1U)
1402 float complex delta = output[(i + block_length) * stride] * phase;
1404 output[(i + block_length) * stride] = output[i*stride] - delta;
1406 output[i*stride] += delta;
1410 phase = phase * phase_incr;
1413 block_length <<= 1U;
1420 uint16_t full_data_length = ((uint16_t)1U) << length_shift;
1422 output[0] = (1.0f + (1.0f * I)) * conjf(output[0]);
1424 if (length_shift > 0U)
1426 float complex phase_incr = I;
1427 float complex z1_factor = 0.5f * phase_incr;
1429 for (uint16_t i = 1U; i < length_shift; i++)
1431 phase_incr = (phase_incr + 1.0f) / cabsf(phase_incr + 1.0f);
1434 uint16_t mid = full_data_length / 2U;
1435 for (uint16_t i = 1U; i < mid; i++)
1438 float complex z0 = output[i * stride];
1439 float complex z1 = output[(full_data_length - i) * stride];
1442 z1 = conjf(z0) - z1;
1446 z1_factor = z1_factor * phase_incr;
1447 z1 = z1 * z1_factor;
1450 z1 = conjf(z0) - z1;
1453 output[i * stride] = z0;
1454 output[(full_data_length - i) * stride] = z1;
1457 output[mid * stride] = conjf(output[mid * stride]);
1463 const uint16_t num_points,
1464 const uint16_t sweep,
1465 const uint16_t point,
1466 const uint16_t median_start_sweep)
1469 int32_t point_r[4U];
1470 int32_t point_i[4U];
1471 int32_t point_abs[4U];
1474 for (uint16_t idx = 0U; idx < 4U; idx++)
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]);
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;
1487 for (uint16_t idx = 0; idx < 4U; idx++)
1489 if (point_abs[idx] > high_val)
1491 high_val = point_abs[idx];
1495 if (point_abs[idx] < low_val)
1497 low_val = point_abs[idx];
1503 point_r[high_index] = 0;
1504 point_i[high_index] = 0;
1505 point_r[low_index] = 0;
1506 point_i[low_index] = 0;
1508 int32_t median_real = 0;
1509 int32_t median_imag = 0;
1512 for (uint16_t idx = 0U; idx < 4U; idx++)
1514 median_real += point_r[idx];
1515 median_imag += point_i[idx];
1519 median_real = median_real / 2;
1520 median_imag = median_imag / 2;
1522 frame[(sweep*num_points) + point].real = (int16_t)median_real;
1523 frame[(sweep*num_points) + point].imag = (int16_t)median_imag;
1528 const uint16_t sweeps_per_frame,
1529 const uint16_t num_points,
1530 const uint16_t sweep,
1531 const uint16_t point)
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;
1537 interpolate_real_i32 = interpolate_real_i32 * 2;
1538 interpolate_imag_i32 = interpolate_imag_i32 * 2;
1541 uint16_t sweep_idx = sweep + 2U;
1543 if (sweep_idx > (sweeps_per_frame - 1U))
1545 sweep_idx = sweeps_per_frame - 1U;
1548 interpolate_real_i32 += frame[((sweep_idx)*num_points) + point].
real;
1549 interpolate_imag_i32 += frame[((sweep_idx)*num_points) + point].
imag;
1551 interpolate_real_i32 = interpolate_real_i32 / 3;
1552 interpolate_imag_i32 = interpolate_imag_i32 / 3;
1555 frame[(sweep*num_points) + point].real = (int16_t)interpolate_real_i32;
1556 frame[(sweep*num_points) + point].imag = (int16_t)interpolate_imag_i32;
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)
1569 float min = INFINITY;
1570 float max = -INFINITY;
1572 for (uint16_t i = 0U; i < num_peaks; i++)
1574 merged_velocities[cluster_count] +=
1575 velocities[peak_idxs[start_idx + i]];
1576 merged_energies[cluster_count] +=
1577 energies[peak_idxs[start_idx + i]];
1580 velocities[peak_idxs[start_idx + i]], min);
1582 max = fmaxf(velocities[peak_idxs[start_idx + i]], max);
1585 merged_velocities[cluster_count] /= (float)num_peaks;
1586 merged_energies[cluster_count] /= (float)num_peaks;