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 
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 
74 {
75  if (vector == NULL)
76  {
77  return;
78  }
79 
80  free(vector->data);
81  free(vector);
82 }
83 
84 
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 
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 
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 
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 
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 
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 
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 
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 
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
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:43
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:45
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
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:46
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