acc_processing_helpers.c
Go to the documentation of this file.
1 // Copyright (c) Acconeer AB, 2022
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
8 #include <assert.h>
9 #include <complex.h>
10 #include <math.h>
11 #include <stdbool.h>
12 #include <stdint.h>
13 #include <stdio.h>
14 #include <stdlib.h>
15 #include <string.h>
16
17 #include "acc_control_helper.h"
18 #include "acc_processing_helpers.h"
19
20 #include "acc_integration_log.h"
21
22
23 #define MAX_DATA_ENTRY_LEN_IQ 87
24 #define MAX_DATA_ENTRY_LEN_FLOAT 44
25
26
27 acc_vector_iq_t *acc_vector_iq_alloc(uint32_t data_length)
28 {
29 acc_vector_iq_t *vector_iq = malloc(sizeof(acc_vector_iq_t));
30
31 if (vector_iq == NULL)
32 {
33 return NULL;
34 }
35
36 vector_iq->data = malloc(data_length * sizeof(float complex));
37
38 if (vector_iq->data == NULL)
39 {
40 free(vector_iq);
41 return NULL;
42 }
43
44 vector_iq->data_length = data_length;
45
46 return vector_iq;
47 }
48
49
50 acc_vector_float_t *acc_vector_float_alloc(uint32_t data_length)
51 {
52 acc_vector_float_t *vector_float = malloc(sizeof(acc_vector_float_t));
53
54 if (vector_float == NULL)
55 {
56 return NULL;
57 }
58
59 vector_float->data = malloc(data_length * sizeof(float));
60
61 if (vector_float->data == NULL)
62 {
63 free(vector_float);
64 return NULL;
65 }
66
67 vector_float->data_length = data_length;
68
69 return vector_float;
70 }
71
72
73 void acc_vector_iq_free(acc_vector_iq_t *vector)
74 {
75 if (vector == NULL)
76 {
77 return;
78 }
79
80 free(vector->data);
81 free(vector);
82 }
83
84
85 void acc_vector_float_free(acc_vector_float_t *vector)
86 {
87 if (vector == NULL)
88 {
89 return;
90 }
91
92 free(vector->data);
93 free(vector);
94 }
95
96
97 float acc_processing_helper_tc_to_sf(float time_constant_s, float update_rate_hz)
98 {
99 if (time_constant_s <= 0.0f)
100 {
101 return 0.0f;
102 }
103 else
104 {
105 return expf(-1.0f / (time_constant_s * update_rate_hz));
106 }
107 }
108
109
110 float acc_processing_helper_dynamic_sf(float static_sf, uint32_t update_count)
111 {
112 return fminf(static_sf, 1.0f - 1.0f / (1.0f + update_count));
113 }
114
115
116 void acc_vector_iq_update_exponential_average(const acc_vector_iq_t *current, acc_vector_iq_t *averaged_data, float sf)
117 {
118 assert(current->data_length == averaged_data->data_length);
119
120 for (uint32_t i = 0; i < current->data_length; i++)
121 {
122 averaged_data->data[i] = sf * averaged_data->data[i] + (1.0f - sf) * current->data[i];
123 }
124 }
125
126
127 void acc_vector_float_update_exponential_average(const acc_vector_float_t *current, acc_vector_float_t *averaged_data, float sf)
128 {
129 assert(current->data_length == averaged_data->data_length);
130
131 for (uint32_t i = 0; i < current->data_length; i++)
132 {
133 averaged_data->data[i] = sf * averaged_data->data[i] + (1.0f - sf) * current->data[i];
134 }
135 }
136
137
138 void acc_get_iq_sweep_vector(const acc_control_helper_t *control_helper_state, acc_vector_iq_t *vector_out)
139 {
140 uint32_t data_length = control_helper_state->proc_meta.sweep_data_length;
141
142 assert(data_length == vector_out->data_length);
143
144 for (uint32_t i = 0; i < data_length; i++)
145 {
146 acc_int16_complex_t iq = control_helper_state->proc_result.frame[i];
147 vector_out->data[i] = iq.real + iq.imag * I;
148 }
149 }
150
151
152 void acc_get_iq_point_vector(const acc_control_helper_t *control_helper_state, uint32_t point, acc_vector_iq_t *vector_out)
153 {
154 assert(point < control_helper_state->proc_meta.sweep_data_length);
155
156 assert(vector_out->data_length == (control_helper_state->proc_meta.frame_data_length / control_helper_state->proc_meta.sweep_data_length));
157
158 for (uint32_t i = 0; i < vector_out->data_length; i++)
159 {
160 acc_int16_complex_t iq = control_helper_state->proc_result.frame[point + i * control_helper_state->proc_meta.sweep_data_length];
161 vector_out->data[i] = iq.real + iq.imag * I;
162 }
163 }
164
165
166 uint32_t acc_processing_helper_get_filter_length(uint32_t peak_width_points, uint32_t step_length)
167 {
168 return (peak_width_points / step_length) | 1;
169 }
170
171
172 void acc_vector_float_create_depth_filter_vector(acc_vector_float_t *vector_out)
173 {
174 uint32_t data_length = vector_out->data_length;
175
176 assert(data_length % 2 == 1);
177
178 uint32_t mid_offset = (data_length + 1) / 2;
179 float sum = 0.0f;
180
181 for (uint32_t i = 0; i < data_length; i++)
182 {
183 if (i < mid_offset)
184 {
185 vector_out->data[i] = (float)i + 1;
186 }
187 else
188 {
189 vector_out->data[i] = (float)(data_length - i);
190 }
191
192 sum += vector_out->data[i];
193 }
194
195 // Normalize
196 for (uint32_t i = 0; i < data_length; i++)
197 {
198 vector_out->data[i] /= sum;
199 }
200 }
201
202
203 void acc_vector_iq_apply_filter(const acc_vector_iq_t *vector_a, acc_vector_float_t *filter_vector, acc_vector_iq_t *vector_out)
204 {
205 assert(vector_a->data_length == vector_out->data_length);
206 assert(filter_vector->data_length % 2 == 1); // Filter length must be odd
207
208 uint32_t offset = filter_vector->data_length / 2;
209
210 for (uint32_t i = 0; i < vector_out->data_length; i++)
211 {
212 vector_out->data[i] = 0.0f;
213
214 for (uint32_t j = 0; j < filter_vector->data_length; j++)
215 {
216 int32_t a_index = i + j - offset;
217 if (a_index >= 0 && (uint32_t)a_index < vector_a->data_length)
218 {
219 vector_out->data[i] += vector_a->data[a_index] * filter_vector->data[j];
220 }
221 }
222 }
223 }
224
225
226 void acc_vector_iq_copy(const acc_vector_iq_t *vector_a, acc_vector_iq_t *vector_out)
227 {
228 assert(vector_a->data_length == vector_out->data_length);
229
230 for (uint32_t i = 0; i < vector_a->data_length; i++)
231 {
232 vector_out->data[i] = vector_a->data[i];
233 }
234 }
235
236
237 void acc_vector_iq_add(const acc_vector_iq_t *vector_a, const acc_vector_iq_t *vector_b, acc_vector_iq_t *vector_out)
238 {
239 assert(vector_a->data_length == vector_b->data_length);
240 assert(vector_a->data_length == vector_out->data_length);
241
242 for (uint32_t i = 0; i < vector_a->data_length; i++)
243 {
244 vector_out->data[i] = vector_a->data[i] + vector_b->data[i];
245 }
246 }
247
248
249 void acc_vector_iq_subtract(const acc_vector_iq_t *vector_a, const acc_vector_iq_t *vector_b, acc_vector_iq_t *vector_out)
250 {
251 assert(vector_a->data_length == vector_b->data_length);
252 assert(vector_a->data_length == vector_out->data_length);
253
254 for (uint32_t i = 0; i < vector_a->data_length; i++)
255 {
256 vector_out->data[i] = vector_a->data[i] - vector_b->data[i];
257 }
258 }
259
260
261 void acc_vector_iq_mult(const acc_vector_iq_t *vector_a, const acc_vector_iq_t *vector_b, acc_vector_iq_t *vector_out)
262 {
263 assert(vector_a->data_length == vector_b->data_length);
264 assert(vector_a->data_length == vector_out->data_length);
265
266 for (uint32_t i = 0; i < vector_a->data_length; i++)
267 {
268 vector_out->data[i] = vector_a->data[i] * vector_b->data[i];
269 }
270 }
271
272
273 void acc_vector_iq_mult_conj(const acc_vector_iq_t *vector_a, const acc_vector_iq_t *vector_b, acc_vector_iq_t *vector_out)
274 {
275 assert(vector_a->data_length == vector_b->data_length);
276 assert(vector_a->data_length == vector_out->data_length);
277
278 for (uint32_t i = 0; i < vector_a->data_length; i++)
279 {
280 vector_out->data[i] = vector_a->data[i] * conjf(vector_b->data[i]);
281 }
282 }
283
284
285 void acc_vector_iq_rotate_phase_inline(acc_vector_iq_t *vector_a, float radians)
286 {
287 float complex rotated_unit_vector = cexpf(radians*I);
288
289 for (uint32_t i = 0; i < vector_a->data_length; i++)
290 {
291 vector_a->data[i] *= rotated_unit_vector;
292 }
293 }
294
295
296 void acc_vector_iq_conj_inline(acc_vector_iq_t *vector_a)
297 {
298 for (uint32_t i = 0; i < vector_a->data_length; i++)
299 {
300 vector_a->data[i] = conjf(vector_a->data[i]);
301 }
302 }
303
304
305 void acc_vector_iq_amplitude(const acc_vector_iq_t *vector_a, acc_vector_float_t *vector_out)
306 {
307 assert(vector_a->data_length == vector_out->data_length);
308
309 for (uint32_t i = 0; i < vector_a->data_length; i++)
310 {
311 vector_out->data[i] = cabsf(vector_a->data[i]);
312 }
313 }
314
315
316 float acc_vector_iq_coherent_mean_amplitude(const acc_vector_iq_t *vector_a)
317 {
318 assert(vector_a->data_length > 0);
319
320 float complex coherent_mean = 0;
321
322 for (uint32_t i = 0; i < vector_a->data_length; i++)
323 {
324 coherent_mean += vector_a->data[i];
325 }
326
327 return cabsf(coherent_mean / vector_a->data_length);
328 }
329
330
331 float acc_vector_iq_noncoherent_mean_amplitude(const acc_vector_iq_t *vector_a)
332 {
333 assert(vector_a->data_length > 0);
334
335 float noncoherent_mean_amplitude = 0;
336
337 for (uint32_t i = 0; i < vector_a->data_length; i++)
338 {
339 noncoherent_mean_amplitude += cabsf(vector_a->data[i]);
340 }
341
342 return noncoherent_mean_amplitude / vector_a->data_length;
343 }
344
345
346 void acc_vector_iq_phase(const acc_vector_iq_t *vector_a, acc_vector_float_t *vector_out)
347 {
348 assert(vector_a->data_length == vector_out->data_length);
349
350 for (uint32_t i = 0; i < vector_a->data_length; i++)
351 {
352 vector_out->data[i] = cargf(vector_a->data[i]);
353 }
354 }
355
356
357 uint32_t acc_vector_float_argmax(acc_vector_float_t *vector_a)
358 {
359 float max = -INFINITY;
360 int max_index = -1;
361
362 for (uint32_t i = 0; i < vector_a->data_length; i++)
363 {
364 if (vector_a->data[i] > max)
365 {
366 max = vector_a->data[i];
367 max_index = i;
368 }
369 }
370
371 return max_index;
372 }
373
374
375 uint32_t acc_vector_float_argmax_skip_edges(acc_vector_float_t *vector_a, uint32_t elemets_to_skip)
376 {
377 float max = -INFINITY;
378 int max_index = -1;
379
380 assert(elemets_to_skip < vector_a->data_length - elemets_to_skip);
381
382 for (uint32_t i = elemets_to_skip; i < (vector_a->data_length - elemets_to_skip); i++)
383 {
384 if (vector_a->data[i] > max)
385 {
386 max = vector_a->data[i];
387 max_index = i;
388 }
389 }
390
391 return max_index;
392 }
393
394
395 float acc_processing_helper_interpolate_peak_position(float y1, float y2, float y3)
396 {
397 return (y1 - y3) / (2.0f * y1 - 4.0f * y2 + 2.0f * y3);
398 }
399
400
401 void acc_vector_float_print(const char *label, acc_vector_float_t *vector_a)
402 {
403 printf("%s:\n", label);
404
405 char buffer[MAX_DATA_ENTRY_LEN_FLOAT];
406
407 for (uint16_t i = 0; i < vector_a->data_length; i++)
408 {
409 if ((i > 0) && ((i % 8) == 0))
410 {
411 printf("\n");
412 }
413
414 snprintf(buffer, sizeof(buffer), "%" PRIfloat " ", ACC_LOG_FLOAT_TO_INTEGER(vector_a->data[i]));
415
416 printf("%14s ", buffer);
417 }
418
419 printf("\n");
420 }
421
422
423 void acc_vector_iq_print(const char *label, acc_vector_iq_t *vector_a)
424 {
425 printf("%s: ", label);
426
427 char buffer[MAX_DATA_ENTRY_LEN_IQ];
428
429 for (uint16_t i = 0; i < vector_a->data_length; i++)
430 {
431 if ((i > 0) && ((i % 8) == 0))
432 {
433 printf("\n");
434 }
435
436 snprintf(buffer, sizeof(buffer), "%" PRIfloat "+%" PRIfloat "i",
437 ACC_LOG_FLOAT_TO_INTEGER(crealf(vector_a->data[i])),
438 ACC_LOG_FLOAT_TO_INTEGER(cimagf(vector_a->data[i])));
439
440 printf("%14s ", buffer);
441 }
442
443 printf("\n");
444 }
acc_processing_helper_dynamic_sf
float acc_processing_helper_dynamic_sf(float static_sf, uint32_t update_count)
Calculate a dynamic smoothing factor.
Definition: acc_processing_helpers.c:110
acc_vector_iq_apply_filter
void acc_vector_iq_apply_filter(const acc_vector_iq_t *vector_a, acc_vector_float_t *filter_vector, acc_vector_iq_t *vector_out)
Apply a FIR filter to an IQ vector.
Definition: acc_processing_helpers.c:203
acc_vector_iq_amplitude
void acc_vector_iq_amplitude(const acc_vector_iq_t *vector_a, acc_vector_float_t *vector_out)
Amplitude of an IQ vector.
Definition: acc_processing_helpers.c:305
acc_vector_iq_mult
void acc_vector_iq_mult(const acc_vector_iq_t *vector_a, const acc_vector_iq_t *vector_b, acc_vector_iq_t *vector_out)
Multiply two IQ vectors.
Definition: acc_processing_helpers.c:261
buffer
void * buffer
Definition: i2c_example_cargo.c:40
MAX_DATA_ENTRY_LEN_IQ
#define MAX_DATA_ENTRY_LEN_IQ
Definition: acc_processing_helpers.c:23
acc_int16_complex_t
Data type for interger-based representation of complex numbers.
Definition: acc_definitions_common.h:40
acc_vector_iq_rotate_phase_inline
void acc_vector_iq_rotate_phase_inline(acc_vector_iq_t *vector_a, float radians)
Rotate the phase of elements in an IQ vector.
Definition: acc_processing_helpers.c:285
acc_vector_iq_update_exponential_average
void acc_vector_iq_update_exponential_average(const acc_vector_iq_t *current, acc_vector_iq_t *averaged_data, float sf)
Update the exponential average of an IQ vector.
Definition: acc_processing_helpers.c:116
acc_vector_float_create_depth_filter_vector
void acc_vector_float_create_depth_filter_vector(acc_vector_float_t *vector_out)
Create a distance filter vector.
Definition: acc_processing_helpers.c:172
acc_vector_float_update_exponential_average
void acc_vector_float_update_exponential_average(const acc_vector_float_t *current, acc_vector_float_t *averaged_data, float sf)
Update the exponential average of a float vector.
Definition: acc_processing_helpers.c:127
acc_vector_iq_t::data
float complex * data
Definition: acc_processing_helpers.h:30
acc_vector_float_t::data
float * data
Definition: acc_processing_helpers.h:36
acc_get_iq_point_vector
void acc_get_iq_point_vector(const acc_control_helper_t *control_helper_state, uint32_t point, acc_vector_iq_t *vector_out)
Extract an IQ vector with sweep data for a specific point from a newly captured IQ frame with multipl...
Definition: acc_processing_helpers.c:152
acc_get_iq_sweep_vector
void acc_get_iq_sweep_vector(const acc_control_helper_t *control_helper_state, acc_vector_iq_t *vector_out)
Converts a newly captured IQ frame with one sweep to an IQ vector.
Definition: acc_processing_helpers.c:138
acc_vector_iq_mult_conj
void acc_vector_iq_mult_conj(const acc_vector_iq_t *vector_a, const acc_vector_iq_t *vector_b, acc_vector_iq_t *vector_out)
Multiply first IQ vector with conjugate of second IQ vector.
Definition: acc_processing_helpers.c:273
acc_vector_iq_alloc
acc_vector_iq_t * acc_vector_iq_alloc(uint32_t data_length)
Allocate storage for an IQ vector.
Definition: acc_processing_helpers.c:27
acc_vector_iq_t
Definition: acc_processing_helpers.h:27
acc_vector_float_print
void acc_vector_float_print(const char *label, acc_vector_float_t *vector_a)
Print float vector.
Definition: acc_processing_helpers.c:401
acc_vector_float_argmax
uint32_t acc_vector_float_argmax(acc_vector_float_t *vector_a)
Index of element with maximum value in a float vector.
Definition: acc_processing_helpers.c:357
acc_processing_helper_tc_to_sf
float acc_processing_helper_tc_to_sf(float time_constant_s, float update_rate_hz)
Convert time constant to smoothing factor.
Definition: acc_processing_helpers.c:97
acc_control_helper_t::proc_meta
acc_processing_metadata_t proc_meta
Definition: acc_control_helper.h:32
acc_vector_iq_noncoherent_mean_amplitude
float acc_vector_iq_noncoherent_mean_amplitude(const acc_vector_iq_t *vector_a)
Non-coherent mean amplitude of IQ vector.
Definition: acc_processing_helpers.c:331
acc_vector_iq_add
void acc_vector_iq_add(const acc_vector_iq_t *vector_a, const acc_vector_iq_t *vector_b, acc_vector_iq_t *vector_out)
Add two IQ vectors.
Definition: acc_processing_helpers.c:237
acc_vector_iq_coherent_mean_amplitude
float acc_vector_iq_coherent_mean_amplitude(const acc_vector_iq_t *vector_a)
Coherent mean amplitude of IQ vector.
Definition: acc_processing_helpers.c:316
acc_processing_metadata_t::sweep_data_length
uint16_t sweep_data_length
Definition: acc_processing.h:41
acc_vector_iq_subtract
void acc_vector_iq_subtract(const acc_vector_iq_t *vector_a, const acc_vector_iq_t *vector_b, acc_vector_iq_t *vector_out)
Subtract two IQ vectors.
Definition: acc_processing_helpers.c:249
acc_vector_iq_free
void acc_vector_iq_free(acc_vector_iq_t *vector)
Free storage of data elements in an IQ vector.
Definition: acc_processing_helpers.c:73
acc_control_helper.h
acc_vector_float_alloc
acc_vector_float_t * acc_vector_float_alloc(uint32_t data_length)
Allocate storage for a float vector.
Definition: acc_processing_helpers.c:50
acc_processing_metadata_t::frame_data_length
uint16_t frame_data_length
Definition: acc_processing.h:39
acc_int16_complex_t::real
int16_t real
Definition: acc_definitions_common.h:42
printf
#define printf
Definition: printf.h:60
acc_processing_helper_interpolate_peak_position
float acc_processing_helper_interpolate_peak_position(float y1, float y2, float y3)
Interpolate peak position.
Definition: acc_processing_helpers.c:395
snprintf
#define snprintf
Definition: printf.h:84
acc_vector_iq_phase
void acc_vector_iq_phase(const acc_vector_iq_t *vector_a, acc_vector_float_t *vector_out)
Phase of an IQ vector.
Definition: acc_processing_helpers.c:346
acc_processing_helpers.h
acc_processing_result_t::frame
acc_int16_complex_t * frame
Definition: acc_processing.h:91
acc_vector_float_t
Definition: acc_processing_helpers.h:33
acc_integration_log.h
ACC_LOG_FLOAT_TO_INTEGER
#define ACC_LOG_FLOAT_TO_INTEGER(a)
Definition: acc_integration_log.h:26
acc_vector_float_t::data_length
uint32_t data_length
Definition: acc_processing_helpers.h:35
acc_control_helper_t
Definition: acc_control_helper.h:23
acc_vector_iq_t::data_length
uint32_t data_length
Definition: acc_processing_helpers.h:29
acc_int16_complex_t::imag
int16_t imag
Definition: acc_definitions_common.h:43
acc_vector_iq_print
void acc_vector_iq_print(const char *label, acc_vector_iq_t *vector_a)
Print IQ vector.
Definition: acc_processing_helpers.c:423
acc_vector_float_free
void acc_vector_float_free(acc_vector_float_t *vector)
Free storage of data elements in a float vector.
Definition: acc_processing_helpers.c:85
acc_vector_iq_copy
void acc_vector_iq_copy(const acc_vector_iq_t *vector_a, acc_vector_iq_t *vector_out)
Copy an IQ vector.
Definition: acc_processing_helpers.c:226
acc_processing_helper_get_filter_length
uint32_t acc_processing_helper_get_filter_length(uint32_t peak_width_points, uint32_t step_length)
Calculate filter vector length.
Definition: acc_processing_helpers.c:166
PRIfloat
#define PRIfloat
Specifier for printing float type using integers.
Definition: acc_integration_log.h:31
acc_vector_float_argmax_skip_edges
uint32_t acc_vector_float_argmax_skip_edges(acc_vector_float_t *vector_a, uint32_t elemets_to_skip)
Index of element with maximum value in a float vector disregarding edge elements.
Definition: acc_processing_helpers.c:375
MAX_DATA_ENTRY_LEN_FLOAT
#define MAX_DATA_ENTRY_LEN_FLOAT
Definition: acc_processing_helpers.c:24
acc_control_helper_t::proc_result
acc_processing_result_t proc_result
Definition: acc_control_helper.h:33
acc_vector_iq_conj_inline
void acc_vector_iq_conj_inline(acc_vector_iq_t *vector_a)
Update an IQ vector with its conjugate.
Definition: acc_processing_helpers.c:296