ref_app_touchless_button.c

Go to the documentation of this file.

// Copyright (c) Acconeer AB, 2023-2024
// 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_algorithm.h"
#include "acc_config.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"
 
#define SENSOR_ID (1U)
// The time to measure for the application presets should be well below 1s.
// If timeouts occurs, try increasing the number.
#define SENSOR_TIMEOUT_MS (1000U)
 
/**
 * @brief Application measurement type
 */
typedef enum
{
        /**
         * Corresponds to a range of approximate 0 cm - 5 cm
         */
        ACC_TOUCHLESS_BUTTON_CLOSE_RANGE,
        /**
         * Corresponds to a range of approximate 0 cm - 24 cm
         */
        ACC_TOUCHLESS_BUTTON_FAR_RANGE,
        /**
         * Gives two detection outputs, one for each range
         */
        ACC_TOUCHLESS_BUTTON_CLOSE_AND_FAR_RANGE,
} acc_touchless_button_measurement_type_t;
 
/**
 * @brief Touchless button application config
 */
typedef struct
{
        /**
         * Sensitivity for close range detection. High sensitivity equals low
         * detection threshold, low sensitivity equals high detection threshold
         */
        float sensitivity_close;
        /**
         * Number of frames in a row above threshold to count as a new close range detection,
         * also number of frames in a row below threshold to count as end of detection
         */
        uint16_t patience_close;
        /**
         * Sensitivity for far range detection. High sensitivity equals low
         * detection threshold, low sensitivity equals high detection threshold
         */
        float sensitivity_far;
        /**
         * Number of frames in a row above threshold to count as a new close range detection,
         * also number of frames in a row below threshold to count as end of detection
         */
        uint16_t patience_far;
        /**
         * Calibration duration in seconds
         */
        float calibration_duration_s;
        /**
         * Interval between calibrations in seconds. When reached a new calibration is made.
         * Should not be set lower than the longest estimated continuous detection event
         */
        float calibration_interval_s;
        /**
         * The measurement type
         * See @ref acc_touchless_button_measurement_type_t for more information
         */
        acc_touchless_button_measurement_type_t measurement_type;
        /**
         * The sensor configuration
         */
        acc_config_t *sensor_config;
} acc_touchless_button_config_t;
 
typedef enum
{
        ACC_TOUCHLESS_BUTTON_PRESET_NONE,
        ACC_TOUCHLESS_BUTTON_PRESET_CLOSE,
        ACC_TOUCHLESS_BUTTON_PRESET_FAR,
        ACC_TOUCHLESS_BUTTON_PRESET_CLOSE_AND_FAR,
} acc_touchless_button_preset_t;
 
#define DEFAULT_PRESET ACC_TOUCHLESS_BUTTON_PRESET_CLOSE
 
/**
 * @brief Touchless button application handle
 */
typedef struct
{
        acc_touchless_button_config_t config;
        acc_sensor_t                 *sensor;
        acc_processing_t             *processing;
        acc_processing_metadata_t     proc_metadata;
        acc_processing_result_t       proc_result;
        void                         *buffer;
        uint32_t                      buffer_size;
        int32_t                      *double_buffer_filter_buffer;
        float                        *frame_variance;
        float complex                *arg_norm;
        float                        *ampl_mean;
        float                        *ampl_std;
        float                        *phase_mean;
        float                        *phase_std;
        uint16_t                      cal_interval_frames;
        uint16_t                      cal_sweeps;
        acc_int16_complex_t          *dynamic_background;
        uint16_t                      rows_in_dynamic_background;
        acc_int16_complex_t          *dynamic_background_guard;
        bool                          update_background;
        uint16_t                      frames_since_last_cal;
        float                         close_threshold;
        float                         far_threshold;
        uint16_t                     *threshold_check_count;
        bool                          run_close;
        bool                          run_far;
        uint16_t                      close_num_points;
        uint16_t                      far_num_points;
        uint16_t                      close_signal;
        uint16_t                      far_signal;
        uint16_t                      close_non_signal;
        uint16_t                      far_non_signal;
        bool                          close_detection;
        bool                          far_detection;
} acc_touchless_button_handle_t;
 
/**
 * @brief Touchless button range result
 */
typedef enum
{
        ACC_TOUCHLESS_BUTTON_RANGE_NOT_USED,
        ACC_TOUCHLESS_BUTTON_RANGE_NO_DETECTION,
        ACC_TOUCHLESS_BUTTON_RANGE_DETECTION,
} acc_touchless_button_range_result_t;
 
/**
 * @brief Touchless button application result
 */
typedef struct
{
        acc_touchless_button_range_result_t close_result;
        acc_touchless_button_range_result_t far_result;
} acc_touchless_button_result_t;
 
static void cleanup(acc_touchless_button_handle_t *handle);
 
static void set_config(acc_touchless_button_config_t *config, acc_touchless_button_preset_t preset);
 
static bool validate_config(acc_touchless_button_config_t *config);
 
static void config_log(acc_touchless_button_config_t *config);
 
static bool init_handle(acc_touchless_button_handle_t *handle);
 
static float get_threshold(float sensitivity);
 
static void reset_background(acc_touchless_button_handle_t *handle);
 
static bool init_sensor(acc_touchless_button_handle_t *handle);
 
static bool do_sensor_calibration_and_prepare(acc_touchless_button_handle_t *handle);
 
static bool measure(acc_touchless_button_handle_t *handle);
 
static bool check_count(uint16_t *count, uint16_t check_offset, uint16_t check_length);
 
static void update_background(acc_touchless_button_handle_t *handle);
 
static bool get_detection(bool current_detection, uint16_t sig_count, uint16_t non_sig_count, uint16_t patience);
 
static bool process(acc_touchless_button_handle_t *handle, acc_touchless_button_result_t *result);
 
static void print_result(acc_touchless_button_result_t *result);
 
int acconeer_main(int argc, char *argv[]);
 
int acconeer_main(int argc, char *argv[])
{
        (void)argc;
        (void)argv;
 
        acc_touchless_button_handle_t handle;
        handle.sensor                      = NULL;
        handle.processing                  = NULL;
        handle.buffer                      = NULL;
        handle.double_buffer_filter_buffer = NULL;
        handle.frame_variance              = NULL;
        handle.arg_norm                    = NULL;
        handle.ampl_mean                   = NULL;
        handle.ampl_std                    = NULL;
        handle.phase_mean                  = NULL;
        handle.phase_std                   = NULL;
        handle.dynamic_background          = NULL;
        handle.dynamic_background_guard    = NULL;
        handle.threshold_check_count       = NULL;
 
        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;
        }
 
        handle.config.sensor_config = acc_config_create();
        if (handle.config.sensor_config == NULL)
        {
                printf("acc_config_create() failed\n");
                cleanup(&handle);
                return EXIT_FAILURE;
        }
 
        set_config(&handle.config, DEFAULT_PRESET);
 
        if (!validate_config(&handle.config))
        {
                cleanup(&handle);
                return EXIT_FAILURE;
        }
 
        config_log(&handle.config);
 
        if (!init_handle(&handle))
        {
                printf("Failed to init handle\n");
                cleanup(&handle);
                return EXIT_FAILURE;
        }
 
        if (!init_sensor(&handle))
        {
                printf("Failed to init sensor\n");
                cleanup(&handle);
                return EXIT_FAILURE;
        }
 
        while (true)
        {
                if (!measure(&handle))
                {
                        printf("Failed to measure\n");
                        cleanup(&handle);
                        return EXIT_FAILURE;
                }
 
                if (handle.proc_result.data_saturated)
                {
                        printf("Data saturated. The detector result is not reliable.\n");
                }
 
                if (handle.proc_result.frame_delayed)
                {
                        printf("Frame delayed. Could not read data fast enough.\n");
                        printf("Try lowering the frame rate or call 'acc_sensor_read' more frequently.\n");
                }
 
                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))
                        {
                                printf("do_sensor_calibration_and_prepare() failed\n");
                                acc_sensor_status(handle.sensor);
                                cleanup(&handle);
                                return EXIT_FAILURE;
                        }
 
                        reset_background(&handle);
 
                        printf("The sensor was successfully re-calibrated.\n");
                }
                else
                {
                        acc_touchless_button_result_t result = {0};
 
                        if (!process(&handle, &result))
                        {
                                printf("Failed to process sensor result\n");
                                cleanup(&handle);
                                return EXIT_FAILURE;
                        }
 
                        print_result(&result);
                }
        }
 
        cleanup(&handle);
 
        printf("Application finished OK\n");
 
        return EXIT_SUCCESS;
}
 
static void cleanup(acc_touchless_button_handle_t *handle)
{
        acc_hal_integration_sensor_disable(SENSOR_ID);
        acc_hal_integration_sensor_supply_off(SENSOR_ID);
 
        if (handle->config.sensor_config != NULL)
        {
                acc_config_destroy(handle->config.sensor_config);
        }
 
        if (handle->sensor != NULL)
        {
                acc_sensor_destroy(handle->sensor);
        }
 
        if (handle->processing != NULL)
        {
                acc_processing_destroy(handle->processing);
        }
 
        if (handle->buffer != NULL)
        {
                acc_integration_mem_free(handle->buffer);
        }
 
        if (handle->double_buffer_filter_buffer != NULL)
        {
                acc_integration_mem_free(handle->double_buffer_filter_buffer);
        }
 
        if (handle->frame_variance != NULL)
        {
                acc_integration_mem_free(handle->frame_variance);
        }
 
        if (handle->arg_norm != NULL)
        {
                acc_integration_mem_free(handle->arg_norm);
        }
 
        if (handle->ampl_mean != NULL)
        {
                acc_integration_mem_free(handle->ampl_mean);
        }
 
        if (handle->ampl_std != NULL)
        {
                acc_integration_mem_free(handle->ampl_std);
        }
 
        if (handle->phase_mean != NULL)
        {
                acc_integration_mem_free(handle->phase_mean);
        }
 
        if (handle->phase_std != NULL)
        {
                acc_integration_mem_free(handle->phase_std);
        }
 
        if (handle->dynamic_background != NULL)
        {
                acc_integration_mem_free(handle->dynamic_background);
        }
 
        if (handle->dynamic_background_guard != NULL)
        {
                acc_integration_mem_free(handle->dynamic_background_guard);
        }
 
        if (handle->threshold_check_count != NULL)
        {
                acc_integration_mem_free(handle->threshold_check_count);
        }
}
 
static void set_config(acc_touchless_button_config_t *config, acc_touchless_button_preset_t preset)
{
        switch (preset)
        {
                case ACC_TOUCHLESS_BUTTON_PRESET_NONE:
                        break;
                case ACC_TOUCHLESS_BUTTON_PRESET_CLOSE:
                        acc_config_sweeps_per_frame_set(config->sensor_config, 16U);
                        acc_config_sweep_rate_set(config->sensor_config, 320U);
                        acc_config_continuous_sweep_mode_set(config->sensor_config, true);
                        acc_config_double_buffering_set(config->sensor_config, true);
                        acc_config_inter_sweep_idle_state_set(config->sensor_config, ACC_CONFIG_IDLE_STATE_READY);
                        acc_config_inter_frame_idle_state_set(config->sensor_config, ACC_CONFIG_IDLE_STATE_READY);
                        acc_config_num_points_set(config->sensor_config, 3U);
                        acc_config_profile_set(config->sensor_config, ACC_CONFIG_PROFILE_1);
                        acc_config_receiver_gain_set(config->sensor_config, 0U);
                        acc_config_hwaas_set(config->sensor_config, 40U);
                        acc_config_start_point_set(config->sensor_config, 0U);
                        acc_config_step_length_set(config->sensor_config, 6U);
 
                        config->sensitivity_close      = 1.9f;
                        config->patience_close         = 2U;
                        config->calibration_duration_s = 0.6f;
                        config->calibration_interval_s = 20.0f;
                        config->measurement_type       = ACC_TOUCHLESS_BUTTON_CLOSE_RANGE;
                        break;
                case ACC_TOUCHLESS_BUTTON_PRESET_FAR:
                        acc_config_sweeps_per_frame_set(config->sensor_config, 16U);
                        acc_config_sweep_rate_set(config->sensor_config, 320U);
                        acc_config_continuous_sweep_mode_set(config->sensor_config, true);
                        acc_config_double_buffering_set(config->sensor_config, true);
                        acc_config_inter_sweep_idle_state_set(config->sensor_config, ACC_CONFIG_IDLE_STATE_READY);
                        acc_config_inter_frame_idle_state_set(config->sensor_config, ACC_CONFIG_IDLE_STATE_READY);
                        acc_config_num_points_set(config->sensor_config, 3U);
                        acc_config_profile_set(config->sensor_config, ACC_CONFIG_PROFILE_3);
                        acc_config_receiver_gain_set(config->sensor_config, 5U);
                        acc_config_hwaas_set(config->sensor_config, 60U);
                        acc_config_start_point_set(config->sensor_config, 0U);
                        acc_config_step_length_set(config->sensor_config, 24U);
 
                        config->sensitivity_far        = 2.0f;
                        config->patience_far           = 2U;
                        config->calibration_duration_s = 0.6f;
                        config->calibration_interval_s = 20.0f;
                        config->measurement_type       = ACC_TOUCHLESS_BUTTON_FAR_RANGE;
                        break;
                case ACC_TOUCHLESS_BUTTON_PRESET_CLOSE_AND_FAR:
                        acc_config_num_subsweeps_set(config->sensor_config, 2U);
                        acc_config_sweeps_per_frame_set(config->sensor_config, 16U);
                        acc_config_sweep_rate_set(config->sensor_config, 320U);
                        acc_config_continuous_sweep_mode_set(config->sensor_config, true);
                        acc_config_double_buffering_set(config->sensor_config, true);
                        acc_config_inter_sweep_idle_state_set(config->sensor_config, ACC_CONFIG_IDLE_STATE_READY);
                        acc_config_inter_frame_idle_state_set(config->sensor_config, ACC_CONFIG_IDLE_STATE_READY);
 
                        acc_config_subsweep_num_points_set(config->sensor_config, 3U, 0U);
                        acc_config_subsweep_profile_set(config->sensor_config, ACC_CONFIG_PROFILE_1, 0U);
                        acc_config_subsweep_receiver_gain_set(config->sensor_config, 0U, 0U);
                        acc_config_subsweep_hwaas_set(config->sensor_config, 40U, 0U);
                        acc_config_subsweep_start_point_set(config->sensor_config, 0U, 0U);
                        acc_config_subsweep_step_length_set(config->sensor_config, 6U, 0U);
 
                        acc_config_subsweep_num_points_set(config->sensor_config, 3U, 1U);
                        acc_config_subsweep_profile_set(config->sensor_config, ACC_CONFIG_PROFILE_3, 1U);
                        acc_config_subsweep_receiver_gain_set(config->sensor_config, 5U, 1U);
                        acc_config_subsweep_hwaas_set(config->sensor_config, 60U, 1U);
                        acc_config_subsweep_start_point_set(config->sensor_config, 0U, 1U);
                        acc_config_subsweep_step_length_set(config->sensor_config, 24U, 1U);
 
                        config->sensitivity_close      = 1.9f;
                        config->patience_close         = 2U;
                        config->sensitivity_far        = 2.0f;
                        config->patience_far           = 2U;
                        config->calibration_duration_s = 0.6f;
                        config->calibration_interval_s = 20.0f;
                        config->measurement_type       = ACC_TOUCHLESS_BUTTON_CLOSE_AND_FAR_RANGE;
                        break;
        }
}
 
static bool validate_config(acc_touchless_button_config_t *config)
{
        bool status = true;
 
        if (acc_config_num_subsweeps_get(config->sensor_config) != 2U && config->measurement_type == ACC_TOUCHLESS_BUTTON_CLOSE_AND_FAR_RANGE)
        {
                printf("Number of subsweeps must be 2 for 'close and far'\n");
                status = false;
        }
 
        if (acc_config_num_subsweeps_get(config->sensor_config) != 1U &&
            (config->measurement_type == ACC_TOUCHLESS_BUTTON_CLOSE_RANGE || config->measurement_type == ACC_TOUCHLESS_BUTTON_FAR_RANGE))
        {
                printf("Number of subsweeps must be 1 for 'close' and 'far'\n");
                status = false;
        }
 
        float    sweep_rate = acc_config_sweep_rate_get(config->sensor_config);
        uint16_t spf        = acc_config_sweeps_per_frame_get(config->sensor_config);
 
        if (sweep_rate == 0.0f)
        {
                printf("Sweep rate must be set\n");
                status = false;
        }
        else
        {
                if (spf > (config->calibration_duration_s * sweep_rate))
                {
                        float calibration_limit = (float)spf / sweep_rate;
 
                        printf("Calibration duration must be at least %" PRIfloat " s\n", ACC_LOG_FLOAT_TO_INTEGER(calibration_limit));
                        status = false;
                }
        }
 
        return status;
}
 
static void config_log(acc_touchless_button_config_t *config)
{
        float                                   sensitivity_close      = config->sensitivity_close;
        uint16_t                                patience_close         = config->patience_close;
        float                                   sensitivity_far        = config->sensitivity_far;
        uint16_t                                patience_far           = config->patience_far;
        float                                   calibration_duration_s = config->calibration_duration_s;
        float                                   calibration_interval_s = config->calibration_interval_s;
        acc_touchless_button_measurement_type_t measurement_type       = config->measurement_type;
 
        printf("Reference application Touchless Button config\n");
 
        if (measurement_type == ACC_TOUCHLESS_BUTTON_CLOSE_RANGE || measurement_type == ACC_TOUCHLESS_BUTTON_CLOSE_AND_FAR_RANGE)
        {
                printf("sensitivity close: %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(sensitivity_close));
                printf("patience close: %" PRIu16 "\n", patience_close);
        }
 
        if (measurement_type == ACC_TOUCHLESS_BUTTON_FAR_RANGE || measurement_type == ACC_TOUCHLESS_BUTTON_CLOSE_AND_FAR_RANGE)
        {
                printf("sensitivity far: %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(sensitivity_far));
                printf("patience far: %" PRIu16 "\n", patience_far);
        }
 
        printf("calibration duration: %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(calibration_duration_s));
        printf("calibration interval: %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(calibration_interval_s));
 
        char *measurement_type_str = "";
 
        switch (measurement_type)
        {
                case ACC_TOUCHLESS_BUTTON_CLOSE_RANGE:
                        measurement_type_str = "close";
                        break;
                case ACC_TOUCHLESS_BUTTON_FAR_RANGE:
                        measurement_type_str = "far";
                        break;
                case ACC_TOUCHLESS_BUTTON_CLOSE_AND_FAR_RANGE:
                        measurement_type_str = "close and far";
                        break;
        }
 
        printf("measurement type: %s\n", measurement_type_str);
 
        printf("Sensor config\n");
        acc_config_log(config->sensor_config);
}
 
static bool init_handle(acc_touchless_button_handle_t *handle)
{
        bool status = true;
 
        if (!acc_rss_get_buffer_size(handle->config.sensor_config, &handle->buffer_size))
        {
                printf("acc_rss_get_buffer_size() failed\n");
                status = false;
        }
 
        if (status)
        {
                handle->processing = acc_processing_create(handle->config.sensor_config, &handle->proc_metadata);
                handle->buffer     = acc_integration_mem_alloc(handle->buffer_size);
 
                uint16_t spf        = acc_config_sweeps_per_frame_get(handle->config.sensor_config);
                float    sweep_rate = acc_config_sweep_rate_get(handle->config.sensor_config);
                float    frame_rate = sweep_rate / (float)spf;
 
                handle->cal_interval_frames = (uint16_t)((frame_rate * handle->config.calibration_interval_s) + 0.5f);
                handle->cal_sweeps          = (uint16_t)((sweep_rate * handle->config.calibration_duration_s) + 0.5f);
                uint16_t num_points         = handle->proc_metadata.sweep_data_length;
 
                handle->double_buffer_filter_buffer = acc_integration_mem_alloc((spf - 2U) * sizeof(*handle->double_buffer_filter_buffer));
                handle->frame_variance              = acc_integration_mem_alloc(handle->proc_metadata.frame_data_length * sizeof(*handle->frame_variance));
                handle->arg_norm                    = acc_integration_mem_alloc(handle->proc_metadata.sweep_data_length * sizeof(*handle->arg_norm));
                handle->ampl_mean                   = acc_integration_mem_alloc(handle->proc_metadata.sweep_data_length * sizeof(*handle->ampl_mean));
                handle->ampl_std                    = acc_integration_mem_alloc(handle->proc_metadata.sweep_data_length * sizeof(*handle->ampl_std));
                handle->phase_mean                  = acc_integration_mem_alloc(handle->proc_metadata.sweep_data_length * sizeof(*handle->phase_mean));
                handle->phase_std                   = acc_integration_mem_alloc(handle->proc_metadata.sweep_data_length * sizeof(*handle->phase_std));
                handle->dynamic_background          = acc_integration_mem_alloc(handle->cal_sweeps * num_points * sizeof(*handle->dynamic_background));
                handle->dynamic_background_guard =
                    acc_integration_mem_alloc(handle->proc_metadata.frame_data_length * sizeof(*handle->dynamic_background_guard));
                handle->threshold_check_count = acc_integration_mem_alloc(handle->proc_metadata.sweep_data_length * sizeof(*handle->threshold_check_count));
 
                status = (handle->processing != NULL && handle->buffer != NULL && handle->double_buffer_filter_buffer != NULL &&
                          handle->frame_variance != NULL && handle->arg_norm != NULL && handle->ampl_mean != NULL && handle->ampl_std != NULL &&
                          handle->phase_mean != NULL && handle->phase_std != NULL && handle->dynamic_background != NULL &&
                          handle->dynamic_background_guard != NULL && handle->threshold_check_count != NULL);
        }
 
        if (status)
        {
                reset_background(handle);
 
                handle->run_close = handle->config.measurement_type == ACC_TOUCHLESS_BUTTON_CLOSE_RANGE ||
                                    handle->config.measurement_type == ACC_TOUCHLESS_BUTTON_CLOSE_AND_FAR_RANGE;
                handle->run_far = handle->config.measurement_type == ACC_TOUCHLESS_BUTTON_FAR_RANGE ||
                                  handle->config.measurement_type == ACC_TOUCHLESS_BUTTON_CLOSE_AND_FAR_RANGE;
                handle->close_num_points = handle->run_close ? acc_config_subsweep_num_points_get(handle->config.sensor_config, 0U) : 0U;
                handle->far_num_points   = handle->run_far ? (handle->run_close ? acc_config_subsweep_num_points_get(handle->config.sensor_config, 1U)
                                                                                : acc_config_num_points_get(handle->config.sensor_config))
                                                           : 0U;
 
                handle->close_threshold = get_threshold(handle->config.sensitivity_close);
                handle->far_threshold   = get_threshold(handle->config.sensitivity_far);
 
                handle->close_signal     = 0U;
                handle->far_signal       = 0U;
                handle->close_non_signal = handle->config.patience_close + 1U;
                handle->far_non_signal   = handle->config.patience_far + 1U;
                handle->close_detection  = false;
                handle->far_detection    = false;
        }
 
        return status;
}
 
static float get_threshold(float sensitivity)
{
        return (1.0f / sensitivity) * 10.0f;
}
 
static void reset_background(acc_touchless_button_handle_t *handle)
{
        handle->frames_since_last_cal = 0U;
 
        uint16_t spf        = acc_config_sweeps_per_frame_get(handle->config.sensor_config);
        uint16_t num_points = handle->proc_metadata.sweep_data_length;
 
        memset(handle->dynamic_background, 0, handle->cal_sweeps * num_points * sizeof(*handle->dynamic_background));
        handle->rows_in_dynamic_background = 0U;
        memset(handle->dynamic_background_guard, 0, spf * num_points * sizeof(*handle->dynamic_background_guard));
        handle->update_background = false;
}
 
static bool init_sensor(acc_touchless_button_handle_t *handle)
{
        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)
        {
                return false;
        }
 
        if (!do_sensor_calibration_and_prepare(handle))
        {
                return false;
        }
 
        return true;
}
 
static bool do_sensor_calibration_and_prepare(acc_touchless_button_handle_t *handle)
{
        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, handle->config.sensor_config, &cal_result, handle->buffer, handle->buffer_size);
        }
 
        return status;
}
 
static bool measure(acc_touchless_button_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;
}
 
static void calc_variance(acc_touchless_button_handle_t *handle)
{
        uint16_t sweep_data_length = handle->proc_metadata.sweep_data_length;
 
        acc_algorithm_mean_matrix_i16_complex(handle->dynamic_background, handle->cal_sweeps, sweep_data_length, handle->arg_norm, 0U);
 
        acc_algorithm_conj_f32(handle->arg_norm, sweep_data_length);
 
        acc_algorithm_normalize_f32_complex(handle->arg_norm, sweep_data_length);
 
        for (uint16_t c = 0U; c < sweep_data_length; c++)
        {
                float abs_mean      = 0.0f;
                float abs_sq_term   = 0.0f;
                float phase_mean    = 0.0f;
                float phase_sq_term = 0.0f;
                for (uint16_t r = 0U; r < handle->cal_sweeps; r++)
                {
                        float         ac      = handle->dynamic_background[r * sweep_data_length + c].real * crealf(handle->arg_norm[c]);
                        float         bd      = handle->dynamic_background[r * sweep_data_length + c].imag * cimagf(handle->arg_norm[c]);
                        float         ad      = handle->dynamic_background[r * sweep_data_length + c].real * cimagf(handle->arg_norm[c]);
                        float         bc      = handle->dynamic_background[r * sweep_data_length + c].imag * crealf(handle->arg_norm[c]);
                        float complex element = (ac - bd) + I * (ad + bc);
 
                        float    delta  = cabsf(element) - abs_mean;
                        uint16_t div    = (r == 0U) ? 1U : r;
                        abs_mean       += (delta / (float)div);
                        abs_sq_term    += delta * (cabsf(element) - abs_mean);
 
                        delta          = cargf(element) - phase_mean;
                        phase_mean    += (delta / (float)div);
                        phase_sq_term += delta * (cargf(element) - phase_mean);
                }
 
                handle->ampl_mean[c]  = abs_mean;
                handle->ampl_std[c]   = sqrtf(abs_sq_term / handle->cal_sweeps);
                handle->phase_mean[c] = phase_mean;
                handle->phase_std[c]  = sqrtf(phase_sq_term / handle->cal_sweeps);
        }
 
        uint16_t spf = acc_config_sweeps_per_frame_get(handle->config.sensor_config);
 
        for (uint16_t r = 0U; r < spf; r++)
        {
                for (uint16_t c = 0U; c < sweep_data_length; c++)
                {
                        float         ac      = handle->proc_result.frame[r * sweep_data_length + c].real * crealf(handle->arg_norm[c]);
                        float         bd      = handle->proc_result.frame[r * sweep_data_length + c].imag * cimagf(handle->arg_norm[c]);
                        float         ad      = handle->proc_result.frame[r * sweep_data_length + c].real * cimagf(handle->arg_norm[c]);
                        float         bc      = handle->proc_result.frame[r * sweep_data_length + c].imag * crealf(handle->arg_norm[c]);
                        float complex element = (ac - bd) + I * (ad + bc);
                        float         a       = (cabsf(element) - handle->ampl_mean[c]) / handle->ampl_std[c];
                        float         b       = (cargf(element) - handle->phase_mean[c]) / handle->phase_std[c];
 
                        handle->frame_variance[r * sweep_data_length + c] = sqrtf(powf(a, 2.0f) + powf(b, 2.0f));
                }
        }
}
 
static bool check_count(uint16_t *count, uint16_t check_offset, uint16_t check_length)
{
        bool success = false;
 
        for (uint16_t i = 0U; i < check_length; i++)
        {
                if (count[check_offset + i] > 1U)
                {
                        success = true;
                }
        }
 
        return success;
}
 
static void update_background(acc_touchless_button_handle_t *handle)
{
        uint16_t spf = acc_config_sweeps_per_frame_get(handle->config.sensor_config);
 
        acc_algorithm_roll_and_push_mult_matrix_i16_complex(handle->dynamic_background,
                                                            handle->cal_sweeps,
                                                            handle->proc_metadata.sweep_data_length,
                                                            handle->dynamic_background_guard,
                                                            acc_config_sweeps_per_frame_get(handle->config.sensor_config),
                                                            false);
 
        handle->rows_in_dynamic_background += spf;
        handle->rows_in_dynamic_background =
            handle->rows_in_dynamic_background > handle->cal_sweeps ? handle->cal_sweeps : handle->rows_in_dynamic_background;
        handle->frames_since_last_cal = 0U;
}
 
static bool get_detection(bool current_detection, uint16_t sig_count, uint16_t non_sig_count, uint16_t patience)
{
        bool new_detection  = !current_detection && sig_count >= patience;
        bool keep_detection = current_detection && non_sig_count <= patience;
 
        return new_detection || keep_detection;
}
 
static bool process(acc_touchless_button_handle_t *handle, acc_touchless_button_result_t *result)
{
        if (acc_config_double_buffering_get(handle->config.sensor_config))
        {
                acc_algorithm_double_buffering_frame_filter(handle->proc_result.frame,
                                                            acc_config_sweeps_per_frame_get(handle->config.sensor_config),
                                                            acc_config_num_points_get(handle->config.sensor_config),
                                                            handle->double_buffer_filter_buffer);
        }
 
        memset(handle->frame_variance, 0, handle->proc_metadata.frame_data_length * sizeof(*handle->frame_variance));
 
        if (handle->frames_since_last_cal > handle->cal_interval_frames)
        {
                reset_background(handle);
        }
        else if (handle->rows_in_dynamic_background == handle->cal_sweeps)
        {
                calc_variance(handle);
        }
 
        uint16_t threshold_check_offset = 0U;
 
        if (handle->run_close)
        {
                acc_algorithm_count_points_above_threshold(handle->frame_variance,
                                                           acc_config_sweeps_per_frame_get(handle->config.sensor_config),
                                                           handle->proc_metadata.sweep_data_length,
                                                           handle->close_threshold,
                                                           handle->threshold_check_count,
                                                           threshold_check_offset,
                                                           handle->close_num_points,
                                                           1U);
 
                threshold_check_offset = handle->close_num_points;
        }
 
        if (handle->run_far)
        {
                acc_algorithm_count_points_above_threshold(handle->frame_variance,
                                                           acc_config_sweeps_per_frame_get(handle->config.sensor_config),
                                                           handle->proc_metadata.sweep_data_length,
                                                           handle->far_threshold,
                                                           handle->threshold_check_count,
                                                           threshold_check_offset,
                                                           handle->far_num_points,
                                                           1U);
        }
 
        if (check_count(handle->threshold_check_count, 0U, handle->proc_metadata.sweep_data_length))
        {
                uint16_t close_offset = 0U;
                uint16_t far_offset   = handle->close_num_points;
 
                if (handle->run_close)
                {
                        if (check_count(handle->threshold_check_count, close_offset, handle->close_num_points))
                        {
                                handle->close_signal++;
                                handle->close_non_signal = 0U;
                        }
                        else
                        {
                                handle->close_signal = 0U;
                                handle->close_non_signal++;
                        }
                }
 
                if (handle->run_far)
                {
                        if (check_count(handle->threshold_check_count, far_offset, handle->far_num_points))
                        {
                                handle->far_signal++;
                                handle->far_non_signal = 0U;
                        }
                        else
                        {
                                handle->far_signal = 0U;
                                handle->far_non_signal++;
                        }
                }
 
                handle->update_background = false;
                memset(handle->dynamic_background_guard, 0, handle->proc_metadata.frame_data_length * sizeof(*handle->dynamic_background_guard));
                handle->frames_since_last_cal++;
        }
        else
        {
                handle->close_signal = 0U;
                handle->far_signal   = 0U;
                handle->close_non_signal++;
                handle->far_non_signal++;
 
                if (handle->update_background)
                {
                        update_background(handle);
                }
 
                memcpy(handle->dynamic_background_guard,
                       handle->proc_result.frame,
                       handle->proc_metadata.frame_data_length * sizeof(*handle->dynamic_background_guard));
                handle->update_background = true;
        }
 
        memset(result, 0, sizeof(*result));
 
        result->close_result = ACC_TOUCHLESS_BUTTON_RANGE_NOT_USED;
        result->far_result   = ACC_TOUCHLESS_BUTTON_RANGE_NOT_USED;
 
        if (handle->run_close)
        {
                handle->close_detection =
                    get_detection(handle->close_detection, handle->close_signal, handle->close_non_signal, handle->config.patience_close);
                result->close_result = handle->close_detection ? ACC_TOUCHLESS_BUTTON_RANGE_DETECTION : ACC_TOUCHLESS_BUTTON_RANGE_NO_DETECTION;
        }
 
        if (handle->run_far)
        {
                handle->far_detection = get_detection(handle->far_detection, handle->far_signal, handle->far_non_signal, handle->config.patience_far);
                result->far_result    = handle->far_detection ? ACC_TOUCHLESS_BUTTON_RANGE_DETECTION : ACC_TOUCHLESS_BUTTON_RANGE_NO_DETECTION;
        }
 
        return true;
}
 
static void print_result(acc_touchless_button_result_t *result)
{
        if (result->close_result != ACC_TOUCHLESS_BUTTON_RANGE_NOT_USED)
        {
                printf("Close detection: %s\n", result->close_result == ACC_TOUCHLESS_BUTTON_RANGE_DETECTION ? "True" : "False");
        }
 
        if (result->far_result != ACC_TOUCHLESS_BUTTON_RANGE_NOT_USED)
        {
                printf("Far detection: %s\n", result->far_result == ACC_TOUCHLESS_BUTTON_RANGE_DETECTION ? "True" : "False");
        }
}