example_vibration.c
Go to the documentation of this file.
1 // Copyright (c) Acconeer AB, 2024
2 // All rights reserved
3 // This file is subject to the terms and conditions defined in the file
4 // 'LICENSES/license_acconeer.txt', (BSD 3-Clause License) which is part
5 // of this source code package.
6
7 #include <complex.h>
8 #include <float.h>
9 #include <inttypes.h>
10 #include <math.h>
11 #include <stdbool.h>
12 #include <stddef.h>
13 #include <stdint.h>
14
15 #include "acc_alg_basic_utils.h"
16 #include "acc_algorithm.h"
17 #include "acc_config.h"
18 #include "acc_config_subsweep.h"
19 #include "acc_integration.h"
20 #include "acc_integration_log.h"
21 #include "acc_processing.h"
22 #include "example_vibration.h"
23
24 #define MODULE "example_vibration"
25
26 /** / c/f \
27 * | ------- | * 1e6 with c = 299792458 m/s,
28 * \ 2 / f = 60.5 GHz
29 * ------------------------
30 * 2 pi
31 */
32 #define RADIANS_TO_DISPLACEMENT_FACTOR (394.32604621792916f)
33
34 #define NUM_POINTS (1U)
35 #define CFAR_WINDOW_LENGTH (10U)
36 #define CFAR_HALF_GUARD_LENGTH (5U)
37 #define CFAR_MARGIN (CFAR_WINDOW_LENGTH + CFAR_HALF_GUARD_LENGTH)
38
39 #define IS_POWER_OF_TWO(x) (((x) & ((x) - 1)) == 0)
40 #define BOOL_TO_STR(b) (b ? "true" : "false")
41
42 /**
43 * Circular buffer.
44 * Usually contains a "length" to keep track of the number of valid elements,
45 * But that was not needed here.
46 */
47 typedef struct
48 {
49 float *buffer;
50
51 /** The next index to write to. Starts at 0 and grows up to 'capacity' before wrapping */
52 uint16_t write_idx;
53
54 /** The maximum number of elements the buffer can hold */
55 uint16_t capacity;
56 } circular_float_buffer_t;
57
58 struct acc_vibration_handle
59 {
60 acc_config_t *sensor_config;
61
62 bool low_frequency_enhancement_enabled;
63 bool continuous_data_acquisition;
64 float psd_to_radians_conversion_factor;
65 float displacement_conversion_factor;
66
67 bool has_init;
68
69 circular_float_buffer_t time_series;
70 float *frequencies;
71 float *lp_displacements;
72
73 int32_t *double_buffer_filter_buffer;
74
75 uint16_t subframe_length;
76 uint16_t padded_time_series_length;
77 uint16_t rfft_length_shift;
78 uint16_t rfft_output_length;
79 uint16_t rfft_read_offset;
80 uint16_t data_length;
81
82 float *zero_mean_time_series;
83 complex float *rfft_output;
84 float *threshold;
85 };
86
87 //-----------------------------
88 // Private declarations
89 //-----------------------------
90
91 /** @brief Write an angle (in rads) to the circular buffer. The angle will be "unwrapped" before write */
92 static void circular_float_buffer_write_angle(circular_float_buffer_t *cb, float new_element);
93
94 /** @brief Get an element of the circular buffer given its "age". A chronological index of 0 returns the oldest element in the buffer */
95 static float circular_float_buffer_get(const circular_float_buffer_t *cb, uint16_t chronological_idx);
96
97 static bool validate_config(const acc_vibration_config_t *config);
98
99 static bool translate_config(const acc_vibration_config_t *config, acc_config_t *sensor_config);
100
101 static void setup_rfft_bounds(acc_vibration_handle_t *handle, const acc_vibration_config_t *config);
102
103 static void setup_sample_frequencies(acc_vibration_handle_t *handle, float sweep_rate);
104
105 static void update_vibration_result(acc_vibration_handle_t *handle, acc_vibration_config_t *config, acc_vibration_result_t *result);
106
107 static float *get_zero_mean_time_series(acc_vibration_handle_t *handle);
108
109 static void calculate_threshold(acc_vibration_handle_t *handle, acc_vibration_config_t *config);
110
111 //-----------------------------
112 // Public definitions
113 //-----------------------------
114
115 void acc_vibration_preset_set(acc_vibration_config_t *config, acc_vibration_preset_t preset)
116 {
117 bool status = config != NULL;
118
119 if (!status)
120 {
121 ACC_LOG_ERROR("Invalid config");
122 }
123 else
124 {
125 switch (preset)
126 {
127 case ACC_VIBRATION_PRESET_LOW_FREQUENCY:
128 config->measured_point = 80U;
129 config->time_series_length = 1024U;
130 config->lp_coeff = 0.8f;
131 config->threshold_sensitivity = 1.5f;
132 config->amplitude_threshold = 100.0f;
133 config->reported_displacement_mode = ACC_VIBRATION_REPORT_DISPLACEMENT_AS_AMPLITUDE;
134 config->low_frequency_enhancement = true;
135 config->profile = ACC_CONFIG_PROFILE_3;
136 config->frame_rate = 0.0f;
137 config->sweep_rate = 200.0f;
138 config->sweeps_per_frame = 20U;
139 config->hwaas = 16U;
140 config->double_buffering = true;
141 config->continuous_sweep_mode = true;
142 config->inter_frame_idle_state = ACC_CONFIG_IDLE_STATE_READY;
143 config->inter_sweep_idle_state = ACC_CONFIG_IDLE_STATE_READY;
144 break;
145 case ACC_VIBRATION_PRESET_HIGH_FREQUENCY:
146 config->measured_point = 80U;
147 config->time_series_length = 1024U;
148 config->lp_coeff = 0.5f;
149 config->threshold_sensitivity = 1.5f;
150 config->amplitude_threshold = 100.0f;
151 config->reported_displacement_mode = ACC_VIBRATION_REPORT_DISPLACEMENT_AS_AMPLITUDE;
152 config->low_frequency_enhancement = false;
153 config->profile = ACC_CONFIG_PROFILE_3;
154 config->frame_rate = 0.0f;
155 config->sweep_rate = 10000.0f;
156 config->sweeps_per_frame = 2048;
157 config->hwaas = 16U;
158 config->double_buffering = false;
159 config->continuous_sweep_mode = false;
160 config->inter_frame_idle_state = ACC_CONFIG_IDLE_STATE_READY;
161 config->inter_sweep_idle_state = ACC_CONFIG_IDLE_STATE_READY;
162 break;
163 default:
164 ACC_LOG_ERROR("Unknown preset");
165 break;
166 }
167 }
168 }
169
170 void acc_vibration_config_log(const acc_vibration_config_t *config)
171 {
172 ACC_LOG_INFO("measured_point: %" PRIi32, config->measured_point);
173 ACC_LOG_INFO("time_series_length: %" PRIu16, config->time_series_length);
174 ACC_LOG_INFO("lp_coeff: %" PRIfloat, ACC_LOG_FLOAT_TO_INTEGER(config->lp_coeff));
175 ACC_LOG_INFO("threshold_sensitivity: %" PRIfloat, ACC_LOG_FLOAT_TO_INTEGER(config->threshold_sensitivity));
176 ACC_LOG_INFO("amplitude_threshold: %" PRIfloat, ACC_LOG_FLOAT_TO_INTEGER(config->amplitude_threshold));
177
178 switch (config->reported_displacement_mode)
179 {
180 case ACC_VIBRATION_REPORT_DISPLACEMENT_AS_AMPLITUDE:
181 ACC_LOG_INFO("reported_displacement_mode: AMPLITUDE");
182 break;
183 case ACC_VIBRATION_REPORT_DISPLACEMENT_AS_PEAK2PEAK:
184 ACC_LOG_INFO("reported_displacement_mode: PEAK2PEAK");
185 break;
186 default:
187 ACC_LOG_ERROR("reported_displacement_mode: UNKNOWN");
188 break;
189 }
190
191 ACC_LOG_INFO("low_frequency_enhancement: %s", BOOL_TO_STR(config->low_frequency_enhancement));
192 ACC_LOG_INFO("profile: %d", config->profile);
193 ACC_LOG_INFO("frame_rate: %" PRIfloat, ACC_LOG_FLOAT_TO_INTEGER(config->frame_rate));
194 ACC_LOG_INFO("sweep_rate: %" PRIfloat, ACC_LOG_FLOAT_TO_INTEGER(config->sweep_rate));
195 ACC_LOG_INFO("sweeps_per_frame: %" PRIu16, config->sweeps_per_frame);
196 ACC_LOG_INFO("hwaas: %" PRIu16, config->hwaas);
197 ACC_LOG_INFO("double_buffering: %s", BOOL_TO_STR(config->double_buffering));
198 ACC_LOG_INFO("continuous_sweep_mode: %s", BOOL_TO_STR(config->continuous_sweep_mode));
199
200 switch (config->inter_frame_idle_state)
201 {
202 case ACC_CONFIG_IDLE_STATE_DEEP_SLEEP:
203 ACC_LOG_INFO("inter_frame_idle_state: DEEP_SLEEP");
204 break;
205 case ACC_CONFIG_IDLE_STATE_SLEEP:
206 ACC_LOG_INFO("inter_frame_idle_state: SLEEP");
207 break;
208 case ACC_CONFIG_IDLE_STATE_READY:
209 ACC_LOG_INFO("inter_frame_idle_state: READY");
210 break;
211 default:
212 ACC_LOG_ERROR("inter_frame_idle_state: UNKNOWN");
213 break;
214 }
215
216 switch (config->inter_sweep_idle_state)
217 {
218 case ACC_CONFIG_IDLE_STATE_DEEP_SLEEP:
219 ACC_LOG_INFO("inter_sweep_idle_state: DEEP_SLEEP");
220 break;
221 case ACC_CONFIG_IDLE_STATE_SLEEP:
222 ACC_LOG_INFO("inter_sweep_idle_state: SLEEP");
223 break;
224 case ACC_CONFIG_IDLE_STATE_READY:
225 ACC_LOG_INFO("inter_sweep_idle_state: READY");
226 break;
227 default:
228 ACC_LOG_ERROR("inter_sweep_idle_state: UNKNOWN");
229 break;
230 }
231 }
232
233 acc_vibration_handle_t *acc_vibration_handle_create(const acc_vibration_config_t *config)
234 {
235 uint16_t sweeps_per_frame = 0U;
236 float sweep_rate = 0.0f;
237
238 acc_vibration_handle_t *handle = NULL;
239
240 bool status = config != NULL;
241
242 if (status)
243 {
244 status = validate_config(config);
245 }
246
247 if (status)
248 {
249 handle = acc_integration_mem_alloc(sizeof(*handle));
250 status = handle != NULL;
251 }
252
253 if (status)
254 {
255 handle->low_frequency_enhancement_enabled = config->low_frequency_enhancement;
256 sweeps_per_frame = config->sweeps_per_frame;
257 sweep_rate = config->sweep_rate;
258
259 handle->has_init = false;
260
261 setup_rfft_bounds(handle, config);
262
263 handle->subframe_length = sweeps_per_frame;
264 handle->rfft_output_length = (handle->padded_time_series_length / 2U) + 1U;
265 handle->rfft_read_offset = 1U;
266 handle->data_length = handle->rfft_output_length - handle->rfft_read_offset;
267
268 handle->continuous_data_acquisition = config->continuous_sweep_mode;
269
270 handle->psd_to_radians_conversion_factor =
271 2.0f / (handle->continuous_data_acquisition ? (float)config->time_series_length : (float)sweeps_per_frame);
272
273 handle->displacement_conversion_factor = handle->psd_to_radians_conversion_factor * RADIANS_TO_DISPLACEMENT_FACTOR;
274
275 if (config->reported_displacement_mode == ACC_VIBRATION_REPORT_DISPLACEMENT_AS_PEAK2PEAK)
276 {
277 handle->displacement_conversion_factor *= 2.0f;
278 }
279 }
280
281 if (status)
282 {
283 handle->sensor_config = acc_config_create();
284 status = handle->sensor_config != NULL;
285 }
286
287 if (status)
288 {
289 status = translate_config(config, handle->sensor_config);
290 }
291
292 if (status)
293 {
294 handle->time_series.buffer = acc_integration_mem_calloc(config->time_series_length, sizeof(*handle->time_series.buffer));
295 handle->time_series.write_idx = 0U;
296 handle->time_series.capacity = config->time_series_length;
297
298 status = handle->time_series.buffer != NULL;
299 }
300
301 if (status)
302 {
303 handle->frequencies = acc_integration_mem_calloc(handle->rfft_output_length, sizeof(*handle->frequencies));
304 status = handle->frequencies != NULL;
305 }
306
307 if (status)
308 {
309 setup_sample_frequencies(handle, sweep_rate);
310 }
311
312 if (status)
313 {
314 handle->double_buffer_filter_buffer = acc_integration_mem_calloc(sweeps_per_frame - 2U, sizeof(*handle->double_buffer_filter_buffer));
315 status = handle->double_buffer_filter_buffer != NULL;
316 }
317
318 if (status)
319 {
320 handle->zero_mean_time_series = acc_integration_mem_calloc(config->time_series_length, sizeof(*handle->zero_mean_time_series));
321 status = handle->zero_mean_time_series != NULL;
322 }
323
324 if (status)
325 {
326 handle->rfft_output = acc_integration_mem_calloc(handle->rfft_output_length, sizeof(*handle->rfft_output));
327 status = handle->rfft_output != NULL;
328 }
329
330 if (status)
331 {
332 handle->lp_displacements = acc_integration_mem_calloc(handle->data_length, sizeof(*handle->lp_displacements));
333 status = handle->lp_displacements != NULL;
334 }
335
336 if (status)
337 {
338 handle->threshold = acc_integration_mem_calloc(handle->data_length, sizeof(*handle->threshold));
339 status = handle->threshold != NULL;
340 }
341
342 if (!status)
343 {
344 acc_vibration_handle_destroy(handle);
345 }
346
347 return status ? handle : NULL;
348 }
349
350 const acc_config_t *acc_vibration_handle_sensor_config_get(acc_vibration_handle_t *handle)
351 {
352 return handle->sensor_config;
353 }
354
355 const float *acc_vibration_handle_displacement_history_get(acc_vibration_handle_t *handle, uint16_t *num_elem)
356 {
357 float *history_p = NULL;
358 bool status = handle != NULL;
359
360 if (status)
361 {
362 history_p = handle->lp_displacements;
363 *num_elem = handle->data_length;
364 }
365
366 return status ? history_p : NULL;
367 }
368
369 bool acc_vibration_handle_continuous_data_acquisition_get(acc_vibration_handle_t *handle, bool *continuous_data_acquisition)
370 {
371 bool status = handle != NULL;
372
373 if (status)
374 {
375 *continuous_data_acquisition = handle->continuous_data_acquisition;
376 }
377
378 return status;
379 }
380
381 void acc_vibration_handle_destroy(acc_vibration_handle_t *handle)
382 {
383 if (handle != NULL)
384 {
385 if (handle->sensor_config != NULL)
386 {
387 acc_config_destroy(handle->sensor_config);
388 handle->sensor_config = NULL;
389 }
390
391 if (handle->time_series.buffer != NULL)
392 {
393 acc_integration_mem_free(handle->time_series.buffer);
394 handle->time_series.buffer = NULL;
395 }
396
397 if (handle->frequencies != NULL)
398 {
399 acc_integration_mem_free(handle->frequencies);
400 handle->frequencies = NULL;
401 }
402
403 if (handle->lp_displacements != NULL)
404 {
405 acc_integration_mem_free(handle->lp_displacements);
406 handle->lp_displacements = NULL;
407 }
408
409 if (handle->double_buffer_filter_buffer != NULL)
410 {
411 acc_integration_mem_free(handle->double_buffer_filter_buffer);
412 handle->double_buffer_filter_buffer = NULL;
413 }
414
415 if (handle->zero_mean_time_series != NULL)
416 {
417 acc_integration_mem_free(handle->zero_mean_time_series);
418 handle->zero_mean_time_series = NULL;
419 }
420
421 if (handle->rfft_output != NULL)
422 {
423 acc_integration_mem_free(handle->rfft_output);
424 handle->rfft_output = NULL;
425 }
426
427 if (handle->threshold != NULL)
428 {
429 acc_integration_mem_free(handle->threshold);
430 handle->threshold = NULL;
431 }
432
433 acc_integration_mem_free(handle);
434 }
435 }
436
437 void acc_vibration_process(acc_processing_result_t *proc_result,
438 acc_vibration_handle_t *handle,
439 acc_vibration_config_t *config,
440 acc_vibration_result_t *result)
441 {
442 const uint16_t sweeps_per_frame = config->sweeps_per_frame;
443
444 result->max_sweep_amplitude = 0.0f;
445 result->max_displacement = FLT_MAX;
446 result->max_displacement_freq = FLT_MAX;
447 result->time_series_std = FLT_MAX;
448
449 /*
450 * Check if an object is close enough in front of the sensor to
451 * reach the configured threshold.
452 */
453
454 for (uint16_t i = 0; i < handle->subframe_length; i++)
455 {
456 uint16_t frame_idx;
457
458 if (handle->low_frequency_enhancement_enabled)
459 {
460 frame_idx = 2U * i;
461 }
462 else
463 {
464 frame_idx = i;
465 }
466
467 acc_int16_complex_t point = proc_result->frame[frame_idx];
468 result->max_sweep_amplitude = fmaxf(result->max_sweep_amplitude, cabsf(point.real + (point.imag * I)));
469 }
470
471 if (result->max_sweep_amplitude < config->amplitude_threshold)
472 {
473 // Return without a (complete) result.
474 handle->has_init = false;
475 return;
476 }
477
478 if (handle->low_frequency_enhancement_enabled)
479 {
480 /* Adjust measure frame with loopback frame */
481 for (uint16_t frame_idx = 0U; frame_idx < handle->subframe_length; frame_idx++)
482 {
483 uint16_t measure_src_idx = 2U * frame_idx;
484 uint16_t loopback_src_idx = (2U * frame_idx) + 1U;
485 uint16_t dst_idx = frame_idx;
486
487 acc_int16_complex_t measure = proc_result->frame[measure_src_idx];
488 acc_int16_complex_t loopback = proc_result->frame[loopback_src_idx];
489
490 complex float measure_cf = (float)measure.real + ((float)measure.imag * I);
491 complex float loopback_cf = (float)loopback.real + ((float)loopback.imag * I);
492
493 complex float lb_norm = loopback_cf / cabsf(loopback_cf);
494 complex float compensated = measure_cf * conjf(lb_norm);
495
496 proc_result->frame[dst_idx].real = (int16_t)(crealf(compensated) + 0.5f);
497 proc_result->frame[dst_idx].imag = (int16_t)(cimagf(compensated) + 0.5f);
498 }
499 }
500
501 /*
502 * Process sensor data.
503 */
504
505 // Handle frame based on whether or not continuous sweep mode is used.
506 if (handle->continuous_data_acquisition)
507 {
508 acc_algorithm_double_buffering_frame_filter(proc_result->frame, sweeps_per_frame, NUM_POINTS, handle->double_buffer_filter_buffer);
509 }
510
511 for (uint16_t i = 0; i < handle->subframe_length; i++)
512 {
513 acc_int16_complex_t point = proc_result->frame[i];
514 float new_element = cargf(point.real + (point.imag * I));
515
516 circular_float_buffer_write_angle(&handle->time_series, new_element);
517 }
518
519 /*
520 * Estimate displacement per frequency.
521 */
522
523 float *zero_mean_time_series = get_zero_mean_time_series(handle);
524
525 acc_algorithm_rfft(zero_mean_time_series, config->time_series_length, handle->rfft_length_shift, handle->rfft_output);
526
527 for (uint16_t i = 0; i < handle->data_length; i++)
528 {
529 float z_abs = cabsf(handle->rfft_output[i + handle->rfft_read_offset]);
530 float displacement = z_abs * handle->displacement_conversion_factor;
531
532 if (handle->has_init)
533 {
534 handle->lp_displacements[i] = handle->lp_displacements[i] * config->lp_coeff + displacement * (1.0f - config->lp_coeff);
535 }
536 else
537 {
538 handle->lp_displacements[i] = displacement;
539 }
540 }
541
542 handle->has_init = true;
543
544 for (uint16_t i = 0U; i < config->time_series_length; i++)
545 {
546 zero_mean_time_series[i] *= RADIANS_TO_DISPLACEMENT_FACTOR;
547 }
548
549 result->time_series_std = acc_algorithm_stddev_f32(zero_mean_time_series, config->time_series_length);
550
551 update_vibration_result(handle, config, result);
552 }
553
554 //-----------------------------
555 // Private definitions
556 //-----------------------------
557
558 static void circular_float_buffer_write_angle(circular_float_buffer_t *cb, float new_element)
559 {
560 // "unwrap" the angle to make fall in the range [-pi, pi] instead of being e.g. 42pi
561 float unwrapped = new_element;
562 acc_algorithm_unwrap(&unwrapped, 1U);
563
564 cb->buffer[cb->write_idx] = unwrapped;
565 cb->write_idx = (cb->write_idx + 1U) % cb->capacity;
566 }
567
568 static float circular_float_buffer_get(const circular_float_buffer_t *cb, uint16_t chronological_idx)
569 {
570 // Say a, b & c has just been written, then 'write_index' will point to the next index to
571 // write over (i.e. the oldest). We can use this to index the circular buffer in ascending
572 // chronological order (oldest to newest).
573 // | a | b | c | x | y | z |
574 // ^
575 uint16_t buffer_idx = (cb->write_idx + chronological_idx) % cb->capacity;
576
577 return cb->buffer[buffer_idx];
578 }
579
580 static bool validate_config(const acc_vibration_config_t *config)
581 {
582 bool success = true;
583
584 if (!IS_POWER_OF_TWO(config->time_series_length))
585 {
586 ACC_LOG_WARNING("**************************** [Warning] *********************************");
587 ACC_LOG_WARNING("** time_series_length should be power of 2 for efficient usage of FFT **");
588 ACC_LOG_WARNING("************************************************************************");
589 }
590
591 if (config->time_series_length < 2U)
592 {
593 ACC_LOG_ERROR("time_series_length too small");
594 success = false;
595 }
596
597 if (config->sweep_rate == 0.0f)
598 {
599 ACC_LOG_ERROR("sweep_rate must be set");
600 success = false;
601 }
602
603 if (config->continuous_sweep_mode && !config->double_buffering)
604 {
605 ACC_LOG_ERROR("double buffering needs to be activated to achieve sufficient sweep rate when using continuous sweep mode");
606 success = false;
607 }
608
609 return success;
610 }
611
612 static bool translate_config(const acc_vibration_config_t *config, acc_config_t *sensor_config)
613 {
614 bool status = config != NULL;
615
616 if (status)
617 {
618 acc_config_prf_t measure_prf = acc_algorithm_select_prf(config->measured_point, config->profile, ACC_APPROX_BASE_STEP_LENGTH_M);
619
620 // clang-format off
621 acc_config_sweep_rate_set(sensor_config, config->sweep_rate);
622 acc_config_frame_rate_set(sensor_config, config->frame_rate);
623 acc_config_sweeps_per_frame_set(sensor_config, config->sweeps_per_frame);
624 acc_config_continuous_sweep_mode_set(sensor_config, config->continuous_sweep_mode);
625 acc_config_double_buffering_set(sensor_config, config->double_buffering);
626 acc_config_inter_frame_idle_state_set(sensor_config, config->inter_frame_idle_state);
627 acc_config_inter_sweep_idle_state_set(sensor_config, config->inter_sweep_idle_state);
628 // clang-format on
629
630 acc_config_num_subsweeps_set(sensor_config, 1U);
631
632 // clang-format off
633 acc_config_subsweep_start_point_set(sensor_config, config->measured_point, 0U);
634 acc_config_subsweep_num_points_set(sensor_config, 1U, 0U);
635 acc_config_subsweep_step_length_set(sensor_config, 1U, 0U);
636 acc_config_subsweep_hwaas_set(sensor_config, config->hwaas, 0U);
637 acc_config_subsweep_receiver_gain_set(sensor_config, 10U, 0U);
638 acc_config_subsweep_enable_tx_set(sensor_config, true, 0U);
639 acc_config_subsweep_phase_enhancement_set(sensor_config, false, 0U);
640 acc_config_subsweep_enable_loopback_set(sensor_config, false, 0U);
641 acc_config_subsweep_profile_set(sensor_config, config->profile, 0U);
642 acc_config_subsweep_prf_set(sensor_config, measure_prf, 0U);
643 // clang-format on
644
645 if (config->low_frequency_enhancement)
646 {
647 acc_config_prf_t loopback_prf = acc_algorithm_select_prf(0, config->profile, ACC_APPROX_BASE_STEP_LENGTH_M);
648
649 acc_config_num_subsweeps_set(sensor_config, 2U);
650
651 // clang-format off
652 acc_config_subsweep_start_point_set(sensor_config, 0, 1U);
653 acc_config_subsweep_num_points_set(sensor_config, 1U, 1U);
654 acc_config_subsweep_step_length_set(sensor_config, 1U, 1U);
655 acc_config_subsweep_hwaas_set(sensor_config, 8U, 1U);
656 acc_config_subsweep_receiver_gain_set(sensor_config, 10U, 1U);
657 acc_config_subsweep_enable_tx_set(sensor_config, true, 1U);
658 acc_config_subsweep_phase_enhancement_set(sensor_config, false, 1U);
659 acc_config_subsweep_enable_loopback_set(sensor_config, true, 1U);
660 acc_config_subsweep_profile_set(sensor_config, ACC_CONFIG_PROFILE_5, 1U);
661 acc_config_subsweep_prf_set(sensor_config, loopback_prf, 1U);
662 // clang-format on
663 }
664 }
665
666 return status;
667 }
668
669 static void setup_sample_frequencies(acc_vibration_handle_t *handle, float sweep_rate)
670 {
671 float sample_spacing = 1.0f / sweep_rate;
672
673 acc_algorithm_rfftfreq(handle->padded_time_series_length, sample_spacing, handle->frequencies);
674 }
675
676 static void update_vibration_result(acc_vibration_handle_t *handle, acc_vibration_config_t *config, acc_vibration_result_t *result)
677 {
678 /*
679 * Compare displacements to threshold (exclude first point as it
680 * does not form a peak).
681 */
682
683 calculate_threshold(handle, config);
684
685 float max_displacement = 0.0f;
686 uint16_t max_displacement_idx = 0;
687
688 result->max_displacement = 0.0f;
689
690 for (uint16_t i = 1U; i < handle->data_length; i++)
691 {
692 if (handle->lp_displacements[i] > handle->threshold[i])
693 {
694 max_displacement = result->max_displacement;
695
696 result->max_displacement = fmaxf(result->max_displacement, handle->lp_displacements[i]);
697
698 if (result->max_displacement > max_displacement)
699 {
700 max_displacement_idx = i;
701 }
702 }
703 }
704
705 if (max_displacement_idx > 0)
706 {
707 result->max_displacement_freq = handle->frequencies[max_displacement_idx + handle->rfft_read_offset];
708 }
709 else
710 {
711 result->max_displacement = FLT_MAX;
712 }
713 }
714
715 static float *get_zero_mean_time_series(acc_vibration_handle_t *handle)
716 {
717 float mean = 0.0f;
718
719 for (uint16_t i = 0; i < handle->time_series.capacity; i++)
720 {
721 mean += handle->time_series.buffer[i];
722 }
723
724 mean /= (float)handle->time_series.capacity;
725
726 for (uint16_t i = 0; i < handle->time_series.capacity; i++)
727 {
728 handle->zero_mean_time_series[i] = circular_float_buffer_get(&handle->time_series, i) - mean;
729 }
730
731 return handle->zero_mean_time_series;
732 }
733
734 static void calculate_threshold(acc_vibration_handle_t *handle, acc_vibration_config_t *config)
735 {
736 for (uint16_t i = 0; i < handle->data_length; i++)
737 {
738 handle->threshold[i] = acc_algorithm_calculate_cfar(
739 handle->lp_displacements, handle->data_length, CFAR_WINDOW_LENGTH, CFAR_HALF_GUARD_LENGTH, config->threshold_sensitivity, i);
740 }
741
742 /*
743 * Extend the CFAR threshold using extrapolation.
744 *
745 * The head is extended using linear extrapolation based on the first
746 * points of the original threshold.
747 *
748 * The tail is extended using the average of the last points of the
749 * original threshold.
750 */
751
752 const float head_slope_multiplier = 2.0f;
753 const uint16_t head_slope_calculation_width = 2U;
754 const uint16_t tail_mean_calculation_width = 10U;
755 const uint16_t tail_mean_calculation_start = handle->data_length - CFAR_MARGIN - tail_mean_calculation_width;
756
757 const float head_base_offset = handle->threshold[CFAR_MARGIN];
758 const uint16_t tail_offset = handle->data_length - CFAR_MARGIN;
759
760 /*
761 * Calculate head.
762 */
763
764 float head_mean_slope = 0.0f;
765
766 for (uint16_t i = 0; i < head_slope_calculation_width; i++)
767 {
768 uint16_t pos = CFAR_MARGIN + i;
769 head_mean_slope += handle->threshold[pos + 1U] - handle->threshold[pos];
770 }
771
772 head_mean_slope /= (float)head_slope_calculation_width;
773 head_mean_slope *= head_slope_multiplier;
774
775 /*
776 * Calculate tail.
777 */
778
779 float tail_mean = 0.0f;
780
781 for (int16_t i = 0; i < tail_mean_calculation_width; i++)
782 {
783 tail_mean += handle->threshold[tail_mean_calculation_start + i];
784 }
785
786 tail_mean /= (float)tail_mean_calculation_width;
787
788 /*
789 * Extend the threshold at both ends.
790 */
791
792 for (uint16_t i = 0; i < CFAR_MARGIN; i++)
793 {
794 handle->threshold[i] = ((float)i - CFAR_MARGIN) * head_mean_slope + head_base_offset;
795 handle->threshold[tail_offset + i] = tail_mean;
796 }
797 }
798
799 static void setup_rfft_bounds(acc_vibration_handle_t *handle, const acc_vibration_config_t *config)
800 {
801 const uint16_t N = config->time_series_length;
802
803 uint16_t shift = 0;
804 uint16_t length = 1U;
805
806 if (N > 1U)
807 {
808 while (length < N)
809 {
810 length <<= 1;
811 shift++;
812 }
813 }
814
815 handle->padded_time_series_length = length;
816 handle->rfft_length_shift = shift;
817 }
circular_float_buffer_t::buffer
float * buffer
Definition: example_vibration.c:49
acc_integration_mem_calloc
void * acc_integration_mem_calloc(size_t nmemb, size_t size)
Allocate dynamic memory.
Definition: acc_integration_stm32.c:597
acc_config_inter_frame_idle_state_set
void acc_config_inter_frame_idle_state_set(acc_config_t *config, acc_config_idle_state_t idle_state)
Set inter frame idle state.
acc_algorithm_rfft
void acc_algorithm_rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output)
1D Fast Fourier Transform for real input
Definition: acc_algorithm.c:592
acc_vibration_handle::sensor_config
acc_config_t * sensor_config
Definition: example_vibration.c:60
ACC_LOG_WARNING
#define ACC_LOG_WARNING(...)
Definition: acc_integration_log.h:17
measure
static bool measure(acc_surface_velocity_handle_t *handle)
Definition: example_surface_velocity.c:756
acc_processing_result_t
Result provided by the processing module.
Definition: acc_processing.h:71
NUM_POINTS
#define NUM_POINTS
Definition: example_vibration.c:34
acc_algorithm_unwrap
void acc_algorithm_unwrap(float *data, uint16_t data_length)
Unwraps a signal by changing elements which have an absolute difference from their predecessor of mor...
Definition: acc_algorithm.c:283
ACC_LOG_INFO
#define ACC_LOG_INFO(...)
Definition: acc_integration_log.h:18
CFAR_WINDOW_LENGTH
#define CFAR_WINDOW_LENGTH
Definition: example_vibration.c:35
acc_algorithm_stddev_f32
float acc_algorithm_stddev_f32(const float *data, uint16_t length)
Calculate standard deviation.
Definition: acc_algorithm.c:1426
setup_sample_frequencies
static void setup_sample_frequencies(acc_vibration_handle_t *handle, float sweep_rate)
Definition: example_vibration.c:669
circular_float_buffer_t
Definition: example_vibration.c:47
acc_alg_basic_utils.h
acc_vibration_handle::lp_displacements
float * lp_displacements
Definition: example_vibration.c:71
acc_int16_complex_t
Data type for interger-based representation of complex numbers.
Definition: acc_definitions_common.h:40
acc_algorithm_double_buffering_frame_filter
void acc_algorithm_double_buffering_frame_filter(acc_int16_complex_t *frame, const uint16_t sweeps_per_frame, const uint16_t num_points, int32_t *work_buffer)
Double buffering frame filter.
Definition: acc_algorithm.c:774
validate_config
static bool validate_config(const acc_vibration_config_t *config)
Definition: example_vibration.c:580
acc_vibration_handle::continuous_data_acquisition
bool continuous_data_acquisition
Definition: example_vibration.c:63
acc_config_sweeps_per_frame_set
void acc_config_sweeps_per_frame_set(acc_config_t *config, uint16_t sweeps)
Set sweeps per frame.
acc_config_destroy
void acc_config_destroy(acc_config_t *config)
Destroy a configuration freeing any resources allocated.
acc_vibration_handle::rfft_read_offset
uint16_t rfft_read_offset
Definition: example_vibration.c:79
example_vibration.h
acc_config_subsweep.h
get_zero_mean_time_series
static float * get_zero_mean_time_series(acc_vibration_handle_t *handle)
Definition: example_vibration.c:715
ACC_CONFIG_IDLE_STATE_DEEP_SLEEP
@ ACC_CONFIG_IDLE_STATE_DEEP_SLEEP
Definition: acc_definitions_a121.h:73
acc_config_inter_sweep_idle_state_set
void acc_config_inter_sweep_idle_state_set(acc_config_t *config, acc_config_idle_state_t idle_state)
Set inter sweep idle state.
acc_vibration_config_t
Vibration config container.
Definition: example_vibration.h:37
acc_vibration_handle
Definition: example_vibration.c:58
ACC_CONFIG_PROFILE_5
@ ACC_CONFIG_PROFILE_5
Definition: acc_definitions_a121.h:57
acc_config_create
acc_config_t * acc_config_create(void)
Create a configuration.
acc_vibration_config_log
void acc_vibration_config_log(const acc_vibration_config_t *config)
Definition: example_vibration.c:170
acc_integration.h
ACC_LOG_ERROR
#define ACC_LOG_ERROR(...)
Definition: acc_integration_log.h:16
acc_vibration_handle_create
acc_vibration_handle_t * acc_vibration_handle_create(const acc_vibration_config_t *config)
Definition: example_vibration.c:233
acc_config_subsweep_phase_enhancement_set
void acc_config_subsweep_phase_enhancement_set(acc_config_t *config, bool enable, uint8_t index)
Set the phase enhancement enabled configuration.
config
cargo_config_t config
Definition: i2c_example_cargo.c:34
acc_config_frame_rate_set
void acc_config_frame_rate_set(acc_config_t *config, float frame_rate)
Set the frame rate.
acc_integration_mem_alloc
void * acc_integration_mem_alloc(size_t size)
Allocate dynamic memory.
Definition: acc_integration_stm32.c:592
circular_float_buffer_t::write_idx
uint16_t write_idx
Definition: example_vibration.c:52
cargo_config_t::sweeps_per_frame
uint16_t sweeps_per_frame
Definition: example_cargo.h:80
CFAR_MARGIN
#define CFAR_MARGIN
Definition: example_vibration.c:37
acc_vibration_handle::rfft_output_length
uint16_t rfft_output_length
Definition: example_vibration.c:78
acc_algorithm_rfftfreq
void acc_algorithm_rfftfreq(uint16_t n, float d, float *freqs)
Calculate the real Fast Fourier Transform sample frequencies.
Definition: acc_algorithm.c:664
acc_config_double_buffering_set
void acc_config_double_buffering_set(acc_config_t *config, bool enable)
Enable or disable double buffering.
acc_config_prf_t
acc_config_prf_t
Pulse Repetition Frequency.
Definition: acc_definitions_a121.h:111
acc_vibration_handle::psd_to_radians_conversion_factor
float psd_to_radians_conversion_factor
Definition: example_vibration.c:64
acc_vibration_handle::rfft_length_shift
uint16_t rfft_length_shift
Definition: example_vibration.c:77
handle
cargo_handle_t * handle
Definition: i2c_example_cargo.c:35
acc_vibration_handle::low_frequency_enhancement_enabled
bool low_frequency_enhancement_enabled
Definition: example_vibration.c:62
ACC_CONFIG_IDLE_STATE_READY
@ ACC_CONFIG_IDLE_STATE_READY
Definition: acc_definitions_a121.h:76
result
cargo_result_t result
Definition: i2c_example_cargo.c:43
acc_int16_complex_t::real
int16_t real
Definition: acc_definitions_common.h:42
acc_config_t
struct acc_config acc_config_t
Definition: acc_config.h:24
ACC_VIBRATION_PRESET_LOW_FREQUENCY
@ ACC_VIBRATION_PRESET_LOW_FREQUENCY
Definition: example_vibration.h:20
acc_vibration_handle::time_series
circular_float_buffer_t time_series
Definition: example_vibration.c:69
ACC_VIBRATION_REPORT_DISPLACEMENT_AS_PEAK2PEAK
@ ACC_VIBRATION_REPORT_DISPLACEMENT_AS_PEAK2PEAK
Definition: example_vibration.h:31
acc_vibration_handle::subframe_length
uint16_t subframe_length
Definition: example_vibration.c:75
ACC_VIBRATION_REPORT_DISPLACEMENT_AS_AMPLITUDE
@ ACC_VIBRATION_REPORT_DISPLACEMENT_AS_AMPLITUDE
Definition: example_vibration.h:29
acc_config_num_subsweeps_set
void acc_config_num_subsweeps_set(acc_config_t *config, uint8_t num_subsweeps)
Set the number of subsweeps to use.
acc_vibration_handle_destroy
void acc_vibration_handle_destroy(acc_vibration_handle_t *handle)
Definition: example_vibration.c:381
acc_config_subsweep_start_point_set
void acc_config_subsweep_start_point_set(acc_config_t *config, int32_t start_point, uint8_t index)
Set the starting point of the sweep.
acc_vibration_handle::threshold
float * threshold
Definition: example_vibration.c:84
acc_config_sweep_rate_set
void acc_config_sweep_rate_set(acc_config_t *config, float sweep_rate)
Set the sweep rate.
acc_config_subsweep_profile_set
void acc_config_subsweep_profile_set(acc_config_t *config, acc_config_profile_t profile, uint8_t index)
Set a profile.
acc_vibration_handle::frequencies
float * frequencies
Definition: example_vibration.c:70
acc_vibration_handle_displacement_history_get
const float * acc_vibration_handle_displacement_history_get(acc_vibration_handle_t *handle, uint16_t *num_elem)
Definition: example_vibration.c:355
ACC_APPROX_BASE_STEP_LENGTH_M
#define ACC_APPROX_BASE_STEP_LENGTH_M
Approximate minimum step length for a sensor measurement in meters.
Definition: acc_algorithm.h:19
acc_vibration_result_t
Vibration processing result.
Definition: example_vibration.h:91
acc_algorithm_select_prf
acc_config_prf_t acc_algorithm_select_prf(int16_t breakpoint, acc_config_profile_t profile, float base_step_length)
Select a suitable PRF given a breakpoint and profile.
Definition: acc_algorithm.c:1144
acc_processing_result_t::frame
acc_int16_complex_t * frame
Definition: acc_processing.h:91
translate_config
static bool translate_config(const acc_vibration_config_t *config, acc_config_t *sensor_config)
Definition: example_vibration.c:612
acc_integration_log.h
acc_vibration_process
void acc_vibration_process(acc_processing_result_t *proc_result, acc_vibration_handle_t *handle, acc_vibration_config_t *config, acc_vibration_result_t *result)
Definition: example_vibration.c:437
circular_float_buffer_write_angle
static void circular_float_buffer_write_angle(circular_float_buffer_t *cb, float new_element)
Write an angle (in rads) to the circular buffer. The angle will be "unwrapped" before write.
Definition: example_vibration.c:558
acc_vibration_handle_sensor_config_get
const acc_config_t * acc_vibration_handle_sensor_config_get(acc_vibration_handle_t *handle)
Definition: example_vibration.c:350
circular_float_buffer_get
static float circular_float_buffer_get(const circular_float_buffer_t *cb, uint16_t chronological_idx)
Get an element of the circular buffer given its "age". A chronological index of 0 returns the oldest ...
Definition: example_vibration.c:568
acc_vibration_handle::double_buffer_filter_buffer
int32_t * double_buffer_filter_buffer
Definition: example_vibration.c:73
ACC_LOG_FLOAT_TO_INTEGER
#define ACC_LOG_FLOAT_TO_INTEGER(a)
Definition: acc_integration_log.h:26
acc_vibration_handle_continuous_data_acquisition_get
bool acc_vibration_handle_continuous_data_acquisition_get(acc_vibration_handle_t *handle, bool *continuous_data_acquisition)
Definition: example_vibration.c:369
calculate_threshold
static void calculate_threshold(acc_vibration_handle_t *handle, acc_vibration_config_t *config)
Definition: example_vibration.c:734
acc_vibration_preset_set
void acc_vibration_preset_set(acc_vibration_config_t *config, acc_vibration_preset_t preset)
Definition: example_vibration.c:115
acc_algorithm_calculate_cfar
float acc_algorithm_calculate_cfar(const float *data, uint16_t data_length, uint16_t window_length, uint16_t half_guard_length, float sensitivity, uint16_t idx)
Calculate CFAR threshold.
Definition: acc_algorithm.c:892
acc_config_continuous_sweep_mode_set
void acc_config_continuous_sweep_mode_set(acc_config_t *config, bool enabled)
Set continuous sweep mode.
acc_vibration_preset_t
acc_vibration_preset_t
Vibration presets.
Definition: example_vibration.h:17
circular_float_buffer_t::capacity
uint16_t capacity
Definition: example_vibration.c:55
update_vibration_result
static void update_vibration_result(acc_vibration_handle_t *handle, acc_vibration_config_t *config, acc_vibration_result_t *result)
Definition: example_vibration.c:676
acc_config_subsweep_receiver_gain_set
void acc_config_subsweep_receiver_gain_set(acc_config_t *config, uint8_t gain, uint8_t index)
Set receiver gain setting.
acc_algorithm.h
acc_vibration_handle::rfft_output
complex float * rfft_output
Definition: example_vibration.c:83
ACC_VIBRATION_PRESET_HIGH_FREQUENCY
@ ACC_VIBRATION_PRESET_HIGH_FREQUENCY
Definition: example_vibration.h:19
acc_integration_mem_free
void acc_integration_mem_free(void *ptr)
Free dynamic memory.
Definition: acc_integration_stm32.c:602
acc_int16_complex_t::imag
int16_t imag
Definition: acc_definitions_common.h:43
acc_vibration_handle::displacement_conversion_factor
float displacement_conversion_factor
Definition: example_vibration.c:65
acc_config_subsweep_enable_loopback_set
void acc_config_subsweep_enable_loopback_set(acc_config_t *config, bool enable, uint8_t index)
Set the loopback enabled configuration.
acc_config_subsweep_hwaas_set
void acc_config_subsweep_hwaas_set(acc_config_t *config, uint16_t hwaas, uint8_t index)
Set the hardware accelerated average samples (HWAAS)
acc_config_subsweep_step_length_set
void acc_config_subsweep_step_length_set(acc_config_t *config, uint16_t step_length, uint8_t index)
Set the step length in a sweep.
acc_config.h
ACC_CONFIG_IDLE_STATE_SLEEP
@ ACC_CONFIG_IDLE_STATE_SLEEP
Definition: acc_definitions_a121.h:74
acc_vibration_handle::has_init
bool has_init
Definition: example_vibration.c:67
acc_vibration_handle::zero_mean_time_series
float * zero_mean_time_series
Definition: example_vibration.c:82
acc_config_subsweep_num_points_set
void acc_config_subsweep_num_points_set(acc_config_t *config, uint16_t num_points, uint8_t index)
Set the number of data points to measure.
CFAR_HALF_GUARD_LENGTH
#define CFAR_HALF_GUARD_LENGTH
Definition: example_vibration.c:36
acc_vibration_handle::padded_time_series_length
uint16_t padded_time_series_length
Definition: example_vibration.c:76
PRIfloat
#define PRIfloat
Specifier for printing float type using integers.
Definition: acc_integration_log.h:31
acc_vibration_handle::data_length
uint16_t data_length
Definition: example_vibration.c:80
acc_config_subsweep_enable_tx_set
void acc_config_subsweep_enable_tx_set(acc_config_t *config, bool enable, uint8_t index)
Enable or disable the transmitter.
setup_rfft_bounds
static void setup_rfft_bounds(acc_vibration_handle_t *handle, const acc_vibration_config_t *config)
Definition: example_vibration.c:799
acc_processing.h
cargo_config_t::threshold_sensitivity
float threshold_sensitivity
Definition: example_cargo.h:65
BOOL_TO_STR
#define BOOL_TO_STR(b)
Definition: example_vibration.c:40
ACC_CONFIG_PROFILE_3
@ ACC_CONFIG_PROFILE_3
Definition: acc_definitions_a121.h:54
IS_POWER_OF_TWO
#define IS_POWER_OF_TWO(x)
Definition: example_vibration.c:39
acc_config_subsweep_prf_set
void acc_config_subsweep_prf_set(acc_config_t *config, acc_config_prf_t prf, uint8_t index)
Set Pulse Repetition Frequency.
RADIANS_TO_DISPLACEMENT_FACTOR
#define RADIANS_TO_DISPLACEMENT_FACTOR
Definition: example_vibration.c:32