example_surface_velocity.c

Go to the documentation of this file.

// Copyright (c) Acconeer AB, 2023-2025
// All rights reserved
// This file is subject to the terms and conditions defined in the file
// 'LICENSES/license_acconeer.txt', (BSD 3-Clause License) which is part
// of this source code package.
 
#include <complex.h>
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
 
#include "acc_alg_basic_utils.h"
#include "acc_algorithm.h"
#include "acc_config.h"
#include "acc_definitions_a121.h"
#include "acc_definitions_common.h"
#include "acc_hal_definitions_a121.h"
#include "acc_hal_integration_a121.h"
#include "acc_integration.h"
#include "acc_integration_log.h"
#include "acc_processing.h"
#include "acc_rss_a121.h"
#include "acc_sensor.h"
#include "acc_version.h"
 
/**
 * [Default app config - can be adapted to reflect the setup]
 *
 * Distance to surface in meters.
 */
#define CONFIG_SURFACE_DISTANCE (1.0f)
 
/**
 * [Default app config - can be adapted to reflect the setup]
 *
 * Angle of the sensor compared to the surface.
 */
#define CONFIG_SENSOR_ANGLE (45.0f)
 
/**
 * [Default app config - can be adapted to reflect the setup]
 *
 * Threshold level sensitivity.
 */
#define CONFIG_THRESHOLD_SENSITIVITY (0.15f)
 
/**
 * [Default app config - can be adapted to reflect the setup]
 *
 * Low-pass filtered power spectral density coefficient.
 *
 * Affects the stability of the estimate.
 */
#define CONFIG_PSD_LP_COEFF (0.75f)
 
/**
 * [Default app config - can be adapted to reflect the setup]
 *
 * Low-pass filtered velocity coefficient.
 *
 * Affects the stability of the estimate.
 */
#define CONFIG_VELOCITY_LP_COEFF (0.98f)
 
/**
 * [Default app config - can be adapted to reflect the setup]
 *
 * The sweep rate for sweeps in a frame during measurement.
 */
#define CONFIG_SWEEP_RATE (3000U)
 
/**
 * [Default app config - can be adapted to reflect the setup]
 *
 * Hardware accelerated average sampling.
 */
#define CONFIG_HWAAS (16U)
 
#define SENSOR_ID         (1U)
#define SENSOR_TIMEOUT_MS (1000U)
 
#define MIN_PEAK_VS 0.1f
 
/**
 * @brief Surface velocity application config container
 */
typedef struct
{
        float         surface_distance;
        uint16_t      time_series_length;
        uint16_t      slow_zone_half_length;
        float         psd_lp_coeff;
        uint16_t      cfar_guard;
        uint16_t      cfar_win;
        float         threshold_sensitivity;
        float         velocity_lp_coeff;
        float         max_peak_interval_s;
        float         sensor_angle;
        acc_config_t *sensor_config;
} acc_surface_velocity_config_t;
 
/**
 * @brief Surface velocity application context handle
 */
typedef struct
{
        acc_sensor_t             *sensor;
        acc_processing_t         *processing;
        acc_processing_metadata_t proc_meta;
        acc_processing_result_t   proc_result;
        void                     *buffer;
        uint32_t                  buffer_size;
 
        acc_surface_velocity_config_t surface_velocity_config;
 
        float    max_peak_interval_n;
        float    sweep_rate;
        uint16_t step_length;
        uint32_t start_point;
        float    base_step_length_m;
        uint16_t num_distances;
        uint16_t sweeps_per_frame;
        uint16_t segment_length;
        uint16_t padded_segment_length;
        uint16_t padded_segment_length_shift;
        uint16_t middle_index;
 
        int32_t       *double_buffer_filter_buffer;
        float complex *frame;
        float complex *time_series;
        float complex *time_series_buffer;
        float complex *fft_out;
        float         *psds;
        float         *lp_psds;
        float         *psd;
        float         *window;
        uint32_t      *threshold_check;
        float         *bin_rad_vs;
        float         *bin_vertical_vs;
 
        uint16_t update_index;
        uint16_t wait_n;
        float    lp_velocity;
        float    vertical_v;
 
        uint16_t *peak_indexes;
        uint16_t  peak_indexes_length;
        uint16_t  num_peaks;
        float    *merged_velocities;
        float    *merged_energies;
        uint16_t  merged_peaks_length;
        uint16_t  num_merged_peaks;
} acc_surface_velocity_handle_t;
 
/**
 * @brief Surface velocity application results container
 */
typedef struct
{
        float estimated_v;
        float distance_m;
} acc_surface_velocity_result_t;
 
static void cleanup(acc_surface_velocity_handle_t *handle, acc_surface_velocity_config_t *config);
 
static void set_config(acc_surface_velocity_config_t *config);
 
static void set_prf(acc_surface_velocity_config_t *config);
 
static void set_profile(acc_surface_velocity_config_t *config);
 
static bool validate_config(acc_surface_velocity_config_t *config);
 
static bool do_sensor_calibration_and_prepare(acc_surface_velocity_handle_t *handle, acc_surface_velocity_config_t *config);
 
static bool init_handle(acc_surface_velocity_handle_t *handle, acc_surface_velocity_config_t *config);
 
static bool init_sensor_resources(acc_surface_velocity_handle_t *handle, acc_surface_velocity_config_t *config);
 
static bool measure(acc_surface_velocity_handle_t *handle);
 
static bool process(acc_surface_velocity_handle_t *handle, acc_surface_velocity_result_t *result);
 
static void print_result(acc_surface_velocity_result_t *result);
 
static void update_threshold(acc_surface_velocity_handle_t *handle);
 
static bool get_velocity_estimate(acc_surface_velocity_handle_t *handle);
 
static float get_angle_correction(float surface_distance, float distance);
 
static uint16_t calc_power_spectral_density(acc_surface_velocity_handle_t *handle);
 
static float get_perceived_wavelength(void);
 
static float calc_dynamic_smoothing_factor(float static_sf, uint32_t update_count);
 
int acconeer_main(int argc, char *argv[]);
 
int acconeer_main(int argc, char *argv[])
{
        (void)argc;
        (void)argv;
        acc_surface_velocity_config_t config = {0};
        acc_surface_velocity_handle_t handle = {0};
        acc_surface_velocity_result_t result = {0};
 
        printf("Acconeer software version %s\n", acc_version_get());
 
        const acc_hal_a121_t *hal = acc_hal_rss_integration_get_implementation();
 
        if (!acc_rss_hal_register(hal))
        {
                return EXIT_FAILURE;
        }
 
        config.sensor_config = acc_config_create();
        if (config.sensor_config == NULL)
        {
                printf("acc_config_create() failed\n");
                cleanup(&handle, &config);
                return EXIT_FAILURE;
        }
 
        set_config(&config);
 
        set_profile(&config);
 
        set_prf(&config);
 
        if (!validate_config(&config))
        {
                printf("Failed to validate config\n");
                cleanup(&handle, &config);
                return EXIT_FAILURE;
        }
 
        if (!init_handle(&handle, &config))
        {
                printf("Failed to initialize handle\n");
                cleanup(&handle, &config);
                return EXIT_FAILURE;
        }
 
        if (!init_sensor_resources(&handle, &config))
        {
                printf("Failed to initialize sensor resources\n");
                cleanup(&handle, &config);
                return EXIT_FAILURE;
        }
 
        while (true)
        {
                if (!measure(&handle))
                {
                        cleanup(&handle, &config);
                        return EXIT_FAILURE;
                }
 
                if (handle.proc_result.calibration_needed)
                {
                        printf("The current calibration is not valid for the current temperature.\n");
                        printf("The sensor needs to be re-calibrated.\n");
 
                        if (!do_sensor_calibration_and_prepare(&handle, &config))
                        {
                                printf("do_sensor_calibration_and_prepare() failed\n");
                                acc_sensor_status(handle.sensor);
                                cleanup(&handle, &config);
                                return EXIT_FAILURE;
                        }
 
                        printf("The sensor was successfully re-calibrated.\n");
                }
                else
                {
                        if (!process(&handle, &result))
                        {
                                cleanup(&handle, &config);
                                return EXIT_FAILURE;
                        }
 
                        print_result(&result);
                }
        }
 
        cleanup(&handle, &config);
 
        printf("Application finished OK\n");
 
        return EXIT_SUCCESS;
}
 
static void update_threshold(acc_surface_velocity_handle_t *handle)
{
        for (uint16_t i = 0U; i < handle->segment_length; i++)
        {
                float threshold = acc_algorithm_calculate_mirrored_one_sided_cfar(handle->psd,
                                                                                  handle->segment_length,
                                                                                  handle->middle_index,
                                                                                  handle->surface_velocity_config.cfar_win,
                                                                                  handle->surface_velocity_config.cfar_guard,
                                                                                  handle->surface_velocity_config.threshold_sensitivity,
                                                                                  i);
 
                if (handle->psd[i] > threshold)
                {
                        acc_alg_basic_utils_set_bit_bitarray_uint32(handle->threshold_check, i);
                }
                else
                {
                        acc_alg_basic_utils_clear_bit_bitarray_uint32(handle->threshold_check, i);
                }
        }
}
 
static bool get_velocity_estimate(acc_surface_velocity_handle_t *handle)
{
        memset(handle->merged_velocities, 0, handle->merged_peaks_length * sizeof(*handle->merged_velocities));
        memset(handle->merged_energies, 0, handle->merged_peaks_length * sizeof(*handle->merged_energies));
 
        bool status = acc_algorithm_merge_peaks(MIN_PEAK_VS,
                                                handle->bin_vertical_vs,
                                                handle->psd,
                                                handle->peak_indexes,
                                                handle->num_peaks,
                                                handle->merged_velocities,
                                                handle->merged_energies,
                                                handle->merged_peaks_length,
                                                &(handle->num_merged_peaks));
 
        if (status)
        {
                handle->vertical_v =
                    acc_algorithm_get_peak_velocity(handle->merged_velocities,
                                                    handle->merged_energies,
                                                    NULL,
                                                    handle->num_merged_peaks,
                                                    handle->bin_vertical_vs[handle->middle_index + handle->surface_velocity_config.slow_zone_half_length]);
        }
 
        return status;
}
 
float get_angle_correction(float surface_distance, float distance)
{
        float insonation_angle = asinf(surface_distance / distance);
        float angle_correction = 1.0f / cosf(insonation_angle);
 
        return angle_correction;
}
 
static void cleanup(acc_surface_velocity_handle_t *handle, acc_surface_velocity_config_t *config)
{
        acc_hal_integration_sensor_disable(SENSOR_ID);
        acc_hal_integration_sensor_supply_off(SENSOR_ID);
 
        if (handle->processing != NULL)
        {
                acc_processing_destroy(handle->processing);
        }
 
        if (config->sensor_config != NULL)
        {
                acc_config_destroy(config->sensor_config);
        }
 
        if (handle->buffer != NULL)
        {
                acc_integration_mem_free(handle->buffer);
        }
 
        if (handle->sensor != NULL)
        {
                acc_sensor_destroy(handle->sensor);
        }
 
        if (handle->double_buffer_filter_buffer != NULL)
        {
                acc_integration_mem_free(handle->double_buffer_filter_buffer);
        }
 
        if (handle->frame != NULL)
        {
                acc_integration_mem_free(handle->frame);
        }
 
        if (handle->time_series != NULL)
        {
                acc_integration_mem_free(handle->time_series);
        }
 
        if (handle->time_series_buffer != NULL)
        {
                acc_integration_mem_free(handle->time_series_buffer);
        }
 
        if (handle->bin_rad_vs != NULL)
        {
                acc_integration_mem_free(handle->bin_rad_vs);
        }
 
        if (handle->bin_vertical_vs != NULL)
        {
                acc_integration_mem_free(handle->bin_vertical_vs);
        }
 
        if (handle->lp_psds != NULL)
        {
                acc_integration_mem_free(handle->lp_psds);
        }
 
        if (handle->fft_out != NULL)
        {
                acc_integration_mem_free(handle->fft_out);
        }
 
        if (handle->psds != NULL)
        {
                acc_integration_mem_free(handle->psds);
        }
 
        if (handle->psd != NULL)
        {
                acc_integration_mem_free(handle->psd);
        }
 
        if (handle->window != NULL)
        {
                acc_integration_mem_free(handle->window);
        }
 
        if (handle->threshold_check != NULL)
        {
                acc_integration_mem_free(handle->threshold_check);
        }
 
        if (handle->merged_velocities != NULL)
        {
                acc_integration_mem_free(handle->merged_velocities);
        }
 
        if (handle->merged_energies != NULL)
        {
                acc_integration_mem_free(handle->merged_energies);
        }
 
        if (handle->peak_indexes != NULL)
        {
                acc_integration_mem_free(handle->peak_indexes);
        }
}
 
static void set_profile(acc_surface_velocity_config_t *config)
{
        float    optimal_distance = config->surface_distance / cosf(config->sensor_angle * (((float)M_PI) / 180.0f));
        uint16_t optimal_point    = (int)ceilf(optimal_distance / ACC_APPROX_BASE_STEP_LENGTH_M);
        int32_t  start_point      = rint(optimal_point - ((int)floor((acc_config_num_points_get(config->sensor_config) - 1)) / 2) *
                                                   acc_config_step_length_get(config->sensor_config));
 
        acc_config_start_point_set(config->sensor_config, start_point);
 
        acc_config_profile_t profile = acc_algorithm_select_profile(start_point, ACC_APPROX_BASE_STEP_LENGTH_M);
 
        acc_config_profile_set(config->sensor_config, profile);
}
 
static void set_prf(acc_surface_velocity_config_t *config)
{
        uint16_t end_point = acc_config_start_point_get(config->sensor_config) +
                             (acc_config_num_points_get(config->sensor_config) - 1U) * acc_config_step_length_get(config->sensor_config);
 
        acc_config_prf_set(config->sensor_config,
                           acc_algorithm_select_prf(end_point, acc_config_profile_get(config->sensor_config), ACC_APPROX_BASE_STEP_LENGTH_M));
}
 
static void set_config(acc_surface_velocity_config_t *config)
{
        config->surface_distance      = CONFIG_SURFACE_DISTANCE;
        config->sensor_angle          = CONFIG_SENSOR_ANGLE;
        config->psd_lp_coeff          = CONFIG_PSD_LP_COEFF;
        config->threshold_sensitivity = CONFIG_THRESHOLD_SENSITIVITY;
        config->velocity_lp_coeff     = CONFIG_VELOCITY_LP_COEFF;
 
        acc_config_hwaas_set(config->sensor_config, CONFIG_HWAAS);
        acc_config_sweep_rate_set(config->sensor_config, CONFIG_SWEEP_RATE);
 
        config->time_series_length    = 512U;
        config->slow_zone_half_length = 3U;
        config->cfar_guard            = 6U;
        config->cfar_win              = 6U;
        config->max_peak_interval_s   = 4.0f;
 
        acc_config_num_points_set(config->sensor_config, 4U);
        acc_config_step_length_set(config->sensor_config, 12U);
        acc_config_sweeps_per_frame_set(config->sensor_config, 128U);
        acc_config_frame_rate_set(config->sensor_config, 0.0f);
        acc_config_double_buffering_set(config->sensor_config, true);
        acc_config_continuous_sweep_mode_set(config->sensor_config, true);
        acc_config_inter_frame_idle_state_set(config->sensor_config, ACC_CONFIG_IDLE_STATE_READY);
        acc_config_inter_sweep_idle_state_set(config->sensor_config, ACC_CONFIG_IDLE_STATE_READY);
}
 
static bool validate_config(acc_surface_velocity_config_t *config)
{
        uint16_t num_subsweeps = acc_config_num_subsweeps_get(config->sensor_config);
        float    sweep_rate    = acc_config_sweep_rate_get(config->sensor_config);
        uint16_t start_point   = acc_config_start_point_get(config->sensor_config);
 
        bool status = true;
 
        if (num_subsweeps != 1U)
        {
                printf("Multiple subsweeps are not supported\n");
                status = false;
        }
 
        if (sweep_rate == 0.0f)
        {
                printf("Sweep rate must be set\n");
                status = false;
        }
 
        if (start_point * ACC_APPROX_BASE_STEP_LENGTH_M <= config->surface_distance)
        {
                printf("Start point must be > surface distance");
                status = false;
        }
 
        if (config->sensor_angle <= 0 || config->sensor_angle >= 90)
        {
                printf("Sensor angle must be between 0 and 90 degrees");
                status = false;
        }
 
        return status;
}
 
static bool do_sensor_calibration_and_prepare(acc_surface_velocity_handle_t *handle, acc_surface_velocity_config_t *config)
{
        bool             status       = false;
        bool             cal_complete = false;
        acc_cal_result_t cal_result;
        const uint16_t   calibration_retries = 1U;
 
        // Random disturbances may cause the calibration to fail. At failure, retry at least once.
        for (uint16_t i = 0; !status && (i <= calibration_retries); i++)
        {
                // Reset sensor before calibration by disabling/enabling it
                acc_hal_integration_sensor_disable(SENSOR_ID);
                acc_hal_integration_sensor_enable(SENSOR_ID);
 
                do
                {
                        status = acc_sensor_calibrate(handle->sensor, &cal_complete, &cal_result, handle->buffer, handle->buffer_size);
 
                        if (status && !cal_complete)
                        {
                                status = acc_hal_integration_wait_for_sensor_interrupt(SENSOR_ID, SENSOR_TIMEOUT_MS);
                        }
                } while (status && !cal_complete);
        }
 
        if (status)
        {
                // Reset sensor after calibration by disabling/enabling it
                acc_hal_integration_sensor_disable(SENSOR_ID);
                acc_hal_integration_sensor_enable(SENSOR_ID);
 
                status = acc_sensor_prepare(handle->sensor, config->sensor_config, &cal_result, handle->buffer, handle->buffer_size);
        }
 
        return status;
}
 
static bool init_sensor_resources(acc_surface_velocity_handle_t *handle, acc_surface_velocity_config_t *config)
{
        handle->processing = acc_processing_create(config->sensor_config, &handle->proc_meta);
        if (handle->processing == NULL)
        {
                printf("acc_processing_create() failed\n");
                return false;
        }
 
        if (!acc_rss_get_buffer_size(config->sensor_config, &handle->buffer_size))
        {
                printf("acc_rss_get_buffer_size() failed\n");
                return false;
        }
 
        handle->buffer = acc_integration_mem_alloc(handle->buffer_size);
        if (handle->buffer == NULL)
        {
                printf("buffer allocation failed\n");
                return false;
        }
 
        acc_hal_integration_sensor_supply_on(SENSOR_ID);
        acc_hal_integration_sensor_enable(SENSOR_ID);
 
        handle->sensor = acc_sensor_create(SENSOR_ID);
        if (handle->sensor == NULL)
        {
                printf("acc_sensor_create() failed\n");
                return false;
        }
 
        if (!do_sensor_calibration_and_prepare(handle, config))
        {
                printf("do_sensor_calibration_and_prepare() failed\n");
                acc_sensor_status(handle->sensor);
                cleanup(handle, config);
                return false;
        }
 
        return true;
}
 
static bool init_handle(acc_surface_velocity_handle_t *handle, acc_surface_velocity_config_t *config)
{
        handle->wait_n       = 0U;
        handle->update_index = 0U;
        handle->lp_velocity  = 0.0f;
        handle->vertical_v   = 0.0f;
 
        handle->surface_velocity_config.cfar_guard            = config->cfar_guard;
        handle->surface_velocity_config.cfar_win              = config->cfar_win;
        handle->surface_velocity_config.slow_zone_half_length = config->slow_zone_half_length;
        handle->surface_velocity_config.velocity_lp_coeff     = config->velocity_lp_coeff;
        handle->surface_velocity_config.surface_distance      = config->surface_distance;
        handle->surface_velocity_config.psd_lp_coeff          = config->psd_lp_coeff;
 
        if (config->threshold_sensitivity <= 0.0f)
        {
                printf("Invalid CFAR sensitivity config\n");
                return false;
        }
 
        handle->surface_velocity_config.threshold_sensitivity = config->threshold_sensitivity;
 
        handle->num_distances      = acc_config_num_points_get(config->sensor_config);
        handle->sweep_rate         = acc_config_sweep_rate_get(config->sensor_config);
        handle->start_point        = acc_config_start_point_get(config->sensor_config);
        handle->sweeps_per_frame   = acc_config_sweeps_per_frame_get(config->sensor_config);
        handle->step_length        = acc_config_step_length_get(config->sensor_config);
        handle->base_step_length_m = acc_processing_points_to_meter(1);
 
        float estimated_frame_rate;
 
        if (acc_config_frame_rate_get(config->sensor_config) == 0.0f)
        {
                estimated_frame_rate = handle->sweep_rate / handle->sweeps_per_frame;
        }
        else
        {
                estimated_frame_rate = acc_config_frame_rate_get(config->sensor_config);
        }
 
        handle->max_peak_interval_n = config->max_peak_interval_s * estimated_frame_rate;
 
        if (!acc_config_continuous_sweep_mode_get(config->sensor_config))
        {
                handle->surface_velocity_config.time_series_length = handle->sweeps_per_frame;
        }
        else
        {
                if (handle->sweeps_per_frame > config->time_series_length)
                {
                        printf("Too many sweeps per frame to be handled\n");
                        return false;
                }
                else
                {
                        handle->surface_velocity_config.time_series_length = config->time_series_length;
                }
        }
 
        handle->segment_length = handle->surface_velocity_config.time_series_length / 4U;
 
        if (handle->segment_length % 2U != 0U)
        {
                handle->segment_length += 1U;
        }
 
        handle->padded_segment_length_shift = 0U;
        handle->padded_segment_length       = 1U << handle->padded_segment_length_shift;
 
        while (handle->padded_segment_length < handle->segment_length)
        {
                handle->padded_segment_length_shift++;
                handle->padded_segment_length = 1U << handle->padded_segment_length_shift;
        }
 
        handle->middle_index = rintf((float)handle->segment_length / 2.0f);
 
        handle->double_buffer_filter_buffer = acc_integration_mem_alloc((handle->sweeps_per_frame - 2U) * sizeof(*handle->double_buffer_filter_buffer));
        handle->time_series =
            acc_integration_mem_alloc(handle->surface_velocity_config.time_series_length * handle->num_distances * sizeof(*handle->time_series));
        handle->time_series_buffer = acc_integration_mem_alloc(handle->segment_length * sizeof(*handle->time_series_buffer));
        handle->frame              = acc_integration_mem_alloc(handle->num_distances * handle->sweeps_per_frame * sizeof(*handle->frame));
        handle->bin_rad_vs         = acc_integration_mem_alloc(handle->segment_length * sizeof(*handle->bin_rad_vs));
        handle->bin_vertical_vs    = acc_integration_mem_alloc(handle->segment_length * sizeof(*handle->bin_vertical_vs));
        handle->lp_psds            = acc_integration_mem_alloc(handle->segment_length * handle->num_distances * sizeof(*handle->lp_psds));
        handle->fft_out            = acc_integration_mem_alloc(handle->padded_segment_length * sizeof(*handle->fft_out));
        handle->psds               = acc_integration_mem_alloc(handle->segment_length * handle->num_distances * sizeof(*handle->psds));
        handle->psd                = acc_integration_mem_alloc(handle->segment_length * sizeof(*handle->psd));
        handle->window             = acc_integration_mem_alloc(handle->segment_length * sizeof(*handle->window));
 
        size_t threshold_check_length = acc_alg_basic_utils_calculate_length_of_bitarray_uint32(handle->segment_length);
 
        handle->threshold_check = acc_integration_mem_alloc(threshold_check_length * sizeof(*handle->threshold_check));
 
        handle->merged_peaks_length = handle->segment_length / 2U;
        handle->merged_velocities   = acc_integration_mem_alloc(handle->merged_peaks_length * sizeof(*handle->merged_velocities));
        handle->merged_energies     = acc_integration_mem_alloc(handle->merged_peaks_length * sizeof(*handle->merged_energies));
        handle->num_merged_peaks    = 0U;
 
        handle->peak_indexes_length = handle->segment_length / 2U;
        handle->peak_indexes        = acc_integration_mem_alloc(handle->peak_indexes_length * sizeof(*handle->peak_indexes));
        handle->num_peaks           = 0U;
 
        bool alloc_success = handle->double_buffer_filter_buffer && handle->time_series != NULL && handle->time_series_buffer != NULL &&
                             handle->frame != NULL && handle->bin_rad_vs != NULL && handle->bin_vertical_vs != NULL && handle->lp_psds != NULL &&
                             handle->fft_out != NULL && handle->psds != NULL && handle->window != NULL && handle->threshold_check != NULL &&
                             handle->merged_velocities != NULL && handle->merged_energies != NULL && handle->peak_indexes != NULL;
 
        if (!alloc_success)
        {
                printf("Failed to allocate resources for the surface velocity handle\n");
                return false;
        }
 
        memset(handle->time_series, 0, handle->surface_velocity_config.time_series_length * handle->num_distances * sizeof(*handle->time_series));
        memset(handle->lp_psds, 0, handle->segment_length * handle->num_distances * sizeof(*handle->lp_psds));
        memset(handle->psds, 0, handle->segment_length * handle->num_distances * sizeof(*handle->psds));
        memset(handle->merged_velocities, 0, handle->merged_peaks_length * sizeof(*handle->merged_velocities));
        memset(handle->merged_energies, 0, handle->merged_peaks_length * sizeof(*handle->merged_energies));
 
        acc_algorithm_hann(handle->segment_length, handle->window);
 
        acc_algorithm_fftfreq(handle->segment_length, 1.0f / handle->sweep_rate, handle->bin_rad_vs);
        acc_algorithm_fftshift(handle->bin_rad_vs, handle->segment_length);
 
        float perceived_wavelength = get_perceived_wavelength();
 
        for (uint16_t i = 0U; i < handle->segment_length; i++)
        {
                handle->bin_rad_vs[i] *= perceived_wavelength;
        }
 
        return true;
}
 
static bool measure(acc_surface_velocity_handle_t *handle)
{
        if (!acc_sensor_measure(handle->sensor))
        {
                printf("acc_sensor_measure failed\n");
                acc_sensor_status(handle->sensor);
                return false;
        }
 
        if (!acc_hal_integration_wait_for_sensor_interrupt(SENSOR_ID, SENSOR_TIMEOUT_MS))
        {
                printf("Sensor interrupt timeout\n");
                acc_sensor_status(handle->sensor);
                return false;
        }
 
        if (!acc_sensor_read(handle->sensor, handle->buffer, handle->buffer_size))
        {
                printf("acc_sensor_read failed\n");
                acc_sensor_status(handle->sensor);
                return false;
        }
 
        acc_processing_execute(handle->processing, handle->buffer, &handle->proc_result);
 
        return true;
}
 
/**
 * @brief Calculate PSD (power spectral density)
 *
 * @param handle Surface velocity handle
 * @return Distance index
 */
static uint16_t calc_power_spectral_density(acc_surface_velocity_handle_t *handle)
{
        for (uint16_t i = 0U; i < handle->sweeps_per_frame; i++)
        {
                for (uint16_t j = 0U; j < handle->num_distances; j++)
                {
                        uint16_t index = i * handle->num_distances + j;
 
                        handle->frame[j] = ((float)handle->proc_result.frame[index].real) + ((float)handle->proc_result.frame[index].imag) * I;
                }
 
                acc_algorithm_roll_and_push_matrix_f32_complex(handle->time_series,
                                                               handle->surface_velocity_config.time_series_length,
                                                               handle->num_distances,
                                                               handle->frame,
                                                               false);
        }
 
        acc_algorithm_welch_matrix(handle->time_series,
                                   handle->surface_velocity_config.time_series_length,
                                   handle->num_distances,
                                   handle->segment_length,
                                   handle->time_series_buffer,
                                   handle->fft_out,
                                   handle->psds,
                                   handle->window,
                                   handle->padded_segment_length_shift,
                                   handle->sweep_rate);
 
        acc_algorithm_fftshift_matrix(handle->psds, handle->segment_length, handle->num_distances);
 
        if (handle->update_index * handle->sweeps_per_frame < handle->surface_velocity_config.time_series_length)
        {
                for (uint16_t i = 0U; i < handle->segment_length; i++)
                {
                        for (uint16_t j = 0U; j < handle->num_distances; j++)
                        {
                                handle->lp_psds[i * handle->num_distances + j] = handle->psds[i * handle->num_distances + j];
                        }
                }
        }
 
        for (uint16_t i = 0U; i < handle->segment_length; i++)
        {
                for (uint16_t j = 0U; j < handle->num_distances; j++)
                {
                        handle->lp_psds[i * handle->num_distances + j] =
                            handle->lp_psds[i * handle->num_distances + j] * handle->surface_velocity_config.psd_lp_coeff +
                            handle->psds[i * handle->num_distances + j] * (1 - handle->surface_velocity_config.psd_lp_coeff);
                }
        }
 
        uint16_t index = acc_algorithm_get_distance_idx(handle->lp_psds,
                                                        handle->num_distances,
                                                        handle->segment_length,
                                                        handle->middle_index,
                                                        handle->surface_velocity_config.slow_zone_half_length);
 
        for (uint16_t i = 0U; i < handle->segment_length; i++)
        {
                handle->psd[i] = handle->lp_psds[i * handle->num_distances + index];
        }
 
        return index;
}
 
static bool process(acc_surface_velocity_handle_t *handle, acc_surface_velocity_result_t *result)
{
        bool status = false;
 
        acc_algorithm_double_buffering_frame_filter(handle->proc_result.frame,
                                                    handle->sweeps_per_frame,
                                                    handle->num_distances,
                                                    handle->double_buffer_filter_buffer);
 
        uint16_t distance_index   = calc_power_spectral_density(handle);
        float    distance         = acc_algorithm_get_distance_m(handle->step_length, handle->start_point, handle->base_step_length_m, distance_index);
        float    angle_correction = get_angle_correction(handle->surface_velocity_config.surface_distance, distance);
 
        for (uint16_t i = 0U; i < handle->segment_length; i++)
        {
                handle->bin_vertical_vs[i] = handle->bin_rad_vs[i] * angle_correction;
        }
 
        update_threshold(handle);
        handle->num_peaks = 0U;
        status            = acc_algorithm_find_peaks(handle->psd,
                                      handle->segment_length,
                                      handle->threshold_check,
                                      handle->peak_indexes,
                                      handle->peak_indexes_length,
                                      &(handle->num_peaks));
        if (status)
        {
                if (handle->num_peaks > 0U)
                {
                        float max_abs_bin_vertical_v = -INFINITY;
                        for (uint16_t i = 0U; i < handle->num_peaks; i++)
                        {
                                float abs_bin_vertical_v = fabsf(handle->bin_vertical_vs[handle->peak_indexes[i]]);
 
                                max_abs_bin_vertical_v = fmaxf(abs_bin_vertical_v, max_abs_bin_vertical_v);
                        }
 
                        uint16_t velocity_index = handle->middle_index + handle->surface_velocity_config.slow_zone_half_length;
 
                        if (max_abs_bin_vertical_v > handle->bin_vertical_vs[velocity_index])
                        {
                                status = get_velocity_estimate(handle);
 
                                if (!status)
                                {
                                        printf("Failed to merge peaks\n");
                                }
                        }
                        else
                        {
                                handle->vertical_v = acc_algorithm_get_peak_velocity(handle->bin_vertical_vs,
                                                                                     handle->psd,
                                                                                     handle->peak_indexes,
                                                                                     handle->num_peaks,
                                                                                     handle->bin_vertical_vs[velocity_index]);
                        }
 
                        if (status)
                        {
                                if (fabsf(handle->lp_velocity) > 0.0f && handle->vertical_v / handle->lp_velocity < 0.8f)
                                {
                                        if (handle->wait_n < handle->max_peak_interval_n)
                                        {
                                                handle->vertical_v  = handle->lp_velocity;
                                                handle->wait_n     += 1U;
                                        }
                                }
                                else
                                {
                                        handle->wait_n = 0U;
                                }
                        }
                }
                else
                {
                        if (handle->wait_n < handle->max_peak_interval_n)
                        {
                                handle->vertical_v  = handle->lp_velocity;
                                handle->wait_n     += 1U;
                        }
                        else
                        {
                                handle->vertical_v = 0.0f;
                        }
                }
 
                if (status)
                {
                        float sf = calc_dynamic_smoothing_factor(handle->surface_velocity_config.velocity_lp_coeff, handle->update_index);
 
                        if (handle->update_index * handle->sweeps_per_frame > handle->surface_velocity_config.time_series_length)
                        {
                                handle->lp_velocity = sf * handle->lp_velocity + (1.0f - sf) * handle->vertical_v;
                        }
 
                        handle->update_index += 1U;
 
                        result->estimated_v = handle->lp_velocity;
                        result->distance_m  = distance;
                }
        }
        else
        {
                printf("Failed to detect peaks\n");
        }
 
        return status;
}
 
static void print_result(acc_surface_velocity_result_t *result)
{
        printf("Velocity: %" PRIfloat " m/s, distance: %" PRIfloat " m\n",
               ACC_LOG_FLOAT_TO_INTEGER(result->estimated_v),
               ACC_LOG_FLOAT_TO_INTEGER(result->distance_m));
}
 
static float get_perceived_wavelength(void)
{
        const float speed_of_light  = 299792458.0f;
        const float radio_frequency = 60.5e9f;
        const float wavelength      = speed_of_light / radio_frequency;
 
        return wavelength / 2.0f;
}
 
/**
 * @brief Calculate a dynamic smoothing factor
 *
 * @param[in] static_sf The target smoothing factor that will be used after the first sweeps
 * @param[in] update_count The update count should be 0 for the first sweep and increased by one for each update
 * @return Dynamic smoothing factor
 */
static float calc_dynamic_smoothing_factor(float static_sf, uint32_t update_count)
{
        return fminf(static_sf, 1.0f - 1.0f / (1.0f + update_count));
}