ref_app_tank_level.c

This is a reference application for measuring liquid level in tanks
This reference application executes as follows:

• Retrieve HAL integration
• Initialize application resources:
  • Create application configuration
  • Create distance detector configuration
  • Update configuration settings
  • Create distance detector handle
  • Create buffer for detector calibration data
  • Create buffer for sensor data
• Create and calibrate the sensor
• Calibrate the detector
• Measure distances with the detector (loop):
  • Prepare sensor with the detector
  • Measure and wait until a read can be done
  • Process sensor measurement and get distance detector result
  • Process distance detector result and print the result
  • Handle "calibration_needed" indication
• Cleanup:
  • Destroy detector configuration
  • Destroy detector handle
  • Destroy sensor data buffer
  • Destroy detector calibration data buffer

// 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 <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
 
#include "acc_algorithm.h"
#include "acc_definitions_a121.h"
#include "acc_detector_distance.h"
#include "acc_hal_definitions_a121.h"
#include "acc_hal_integration_a121.h"
#include "acc_integration.h"
#include "acc_integration_log.h"
#include "acc_rss_a121.h"
#include "acc_sensor.h"
#include "acc_version.h"
 
#define SENSOR_ID           1U
#define SENSOR_TIMEOUT_MS   1000U
#define DEFAULT_UPDATE_RATE 1.0f
#define CLOSE_RANGE_START   0.07f
 
// Settings for a small tank
#define SMALL_TANK_MEDIAN_FILTER_LENGTH             3U
#define SMALL_TANK_NUM_MEDIANS_TO_AVERAGE           2U
#define SMALL_TANK_RANGE_START_M                    0.03f
#define SMALL_TANK_RANGE_END_M                      0.5f
#define SMALL_TANK_MAX_STEP_LENGTH                  12U
#define SMALL_TANK_MAX_PROFILE                      ACC_CONFIG_PROFILE_3
#define SMALL_TANK_NUM_FRAMES_REC                   50U
#define SMALL_TANK_PEAK_SORTING                     ACC_DETECTOR_DISTANCE_PEAK_SORTING_CLOSEST
#define SMALL_TANK_REFLECTOR_SHAPE                  ACC_DETECTOR_DISTANCE_REFLECTOR_SHAPE_PLANAR
#define SMALL_TANK_THRESHOLD_METHOD                 ACC_DETECTOR_DISTANCE_THRESHOLD_METHOD_CFAR
#define SMALL_TANK_FIXED_AMPLITUDE_THRESHOLD        100.0f
#define SMALL_TANK_FIXED_STRENGTH_THRESHOLD         0.0f
#define SMALL_TANK_THRESHOLD_SENSITIVITY            0.0f
#define SMALL_TANK_SIGNAL_QUALITY                   3.0f
#define SMALL_TANK_CLOSE_RANGE_LEAKAGE_CANCELLATION false
 
// Settings for a medium tank
#define MEDIUM_TANK_MEDIAN_FILTER_LENGTH             3U
#define MEDIUM_TANK_NUM_MEDIANS_TO_AVERAGE           1U
#define MEDIUM_TANK_RANGE_START_M                    0.05f
#define MEDIUM_TANK_RANGE_END_M                      6.0f
#define MEDIUM_TANK_MAX_STEP_LENGTH                  0U // 0 means no limit
#define MEDIUM_TANK_MAX_PROFILE                      ACC_CONFIG_PROFILE_5
#define MEDIUM_TANK_NUM_FRAMES_REC                   50U
#define MEDIUM_TANK_PEAK_SORTING                     ACC_DETECTOR_DISTANCE_PEAK_SORTING_STRONGEST
#define MEDIUM_TANK_REFLECTOR_SHAPE                  ACC_DETECTOR_DISTANCE_REFLECTOR_SHAPE_PLANAR
#define MEDIUM_TANK_THRESHOLD_METHOD                 ACC_DETECTOR_DISTANCE_THRESHOLD_METHOD_CFAR
#define MEDIUM_TANK_FIXED_AMPLITUDE_THRESHOLD        100.0f
#define MEDIUM_TANK_FIXED_STRENGTH_THRESHOLD         3.0f
#define MEDIUM_TANK_THRESHOLD_SENSITIVITY            0.0f
#define MEDIUM_TANK_SIGNAL_QUALITY                   19.0f
#define MEDIUM_TANK_CLOSE_RANGE_LEAKAGE_CANCELLATION false
 
// Settings for a large tank
#define LARGE_TANK_MEDIAN_FILTER_LENGTH             1U
#define LARGE_TANK_NUM_MEDIANS_TO_AVERAGE           1U
#define LARGE_TANK_RANGE_START_M                    0.1f
#define LARGE_TANK_RANGE_END_M                      15.0f
#define LARGE_TANK_MAX_STEP_LENGTH                  0U // 0 means no limit
#define LARGE_TANK_MAX_PROFILE                      ACC_CONFIG_PROFILE_5
#define LARGE_TANK_NUM_FRAMES_REC                   50U
#define LARGE_TANK_PEAK_SORTING                     ACC_DETECTOR_DISTANCE_PEAK_SORTING_STRONGEST
#define LARGE_TANK_REFLECTOR_SHAPE                  ACC_DETECTOR_DISTANCE_REFLECTOR_SHAPE_PLANAR
#define LARGE_TANK_THRESHOLD_METHOD                 ACC_DETECTOR_DISTANCE_THRESHOLD_METHOD_CFAR
#define LARGE_TANK_FIXED_AMPLITUDE_THRESHOLD        100.0f
#define LARGE_TANK_FIXED_STRENGTH_THRESHOLD         5.0f
#define LARGE_TANK_THRESHOLD_SENSITIVITY            0.0f
#define LARGE_TANK_SIGNAL_QUALITY                   20.0f
#define LARGE_TANK_CLOSE_RANGE_LEAKAGE_CANCELLATION false
 
/** \example ref_app_tank_level.c
 * @brief This is a reference application for measuring liquid level in tanks
 * @n
 * This reference application executes as follows:
 *   - Retrieve HAL integration
 *   - Initialize application resources:
 *     + Create application configuration
 *     + Create distance detector configuration
 *     + Update configuration settings
 *     + Create distance detector handle
 *     + Create buffer for detector calibration data
 *     + Create buffer for sensor data
 *   - Create and calibrate the sensor
 *   - Calibrate the detector
 *   - Measure distances with the detector (loop):
 *     + Prepare sensor with the detector
 *     + Measure and wait until a read can be done
 *     + Process sensor measurement and get distance detector result
 *     + Process distance detector result and print the result
 *     + Handle "calibration_needed" indication
 *   - Cleanup:
 *     + Destroy detector configuration
 *     + Destroy detector handle
 *     + Destroy sensor data buffer
 *     + Destroy detector calibration data buffer
 */
 
typedef enum
{
        TANK_LEVEL_PRESET_CONFIG_NONE = 0,
        TANK_LEVEL_PRESET_CONFIG_SMALL_TANK,
        TANK_LEVEL_PRESET_CONFIG_MEDIUM_TANK,
        TANK_LEVEL_PRESET_CONFIG_LARGE_TANK,
} tank_level_preset_config_t;
 
#define DEFAULT_PRESET_CONFIG TANK_LEVEL_PRESET_CONFIG_SMALL_TANK
 
typedef struct
{
        float                           tank_range_start_m;
        float                           tank_range_end_m;
        uint16_t                        median_filter_length;
        uint16_t                        num_medians_to_average;
        acc_detector_distance_config_t *distance_config;
} acc_ref_app_tank_level_config_t;
 
typedef struct
{
        acc_ref_app_tank_level_config_t  *app_config;
        acc_sensor_t                     *sensor;
        float                            *level_history;
        uint16_t                          level_history_length;
        float                            *median_vector;
        uint16_t                          median_vector_length;
        uint16_t                          median_counter;
        uint16_t                          mean_counter;
        uint16_t                          median_edge_status_count;
        uint16_t                          mean_edge_status_count;
        acc_detector_distance_handle_t   *detector_handle;
        void                             *buffer;
        uint32_t                          buffer_size;
        uint8_t                          *detector_cal_result_static;
        uint32_t                          detector_cal_result_static_size;
        acc_detector_cal_result_dynamic_t detector_cal_result_dynamic;
} app_context_t;
 
typedef enum
{
        PEAK_STATUS_IN_RANGE,
        PEAK_STATUS_NO_DETECTION,
        PEAK_STATUS_OVERFLOW,
        PEAK_STATUS_OUT_OF_RANGE
} peak_status_t;
 
typedef struct
{
        bool          peak_detected;
        peak_status_t peak_status;
        float         level;
        bool          result_ready;
} app_result_t;
 
static void cleanup(app_context_t *context);
 
static float get_detector_start_m(acc_ref_app_tank_level_config_t *app_config);
 
static float get_detector_end_m(acc_ref_app_tank_level_config_t *app_config);
 
static void set_config(acc_ref_app_tank_level_config_t *app_config, tank_level_preset_config_t preset);
 
static bool initialize_application_resources(app_context_t *context);
 
static bool sensor_calibration(acc_sensor_t *sensor, acc_cal_result_t *cal_result, void *buffer, uint32_t buffer_size);
 
static bool full_detector_calibration(app_context_t *context, const acc_cal_result_t *sensor_cal_result);
 
static bool update_detector_calibration(app_context_t *context, const acc_cal_result_t *sensor_cal_result);
 
static bool detector_get_next(app_context_t *context, const acc_cal_result_t *sensor_cal_result, acc_detector_distance_result_t *detector_result);
 
static float median(float *array, uint16_t array_length);
 
static float nanmean(float *array, uint16_t array_length);
 
static void process_detector_result(const acc_detector_distance_result_t *distance_result, app_result_t *app_result, app_context_t *context);
 
static void print_result(const app_result_t *app_result, const acc_ref_app_tank_level_config_t *app_config);
 
int acconeer_main(int argc, char *argv[]);
 
int acconeer_main(int argc, char *argv[])
{
        (void)argc;
        (void)argv;
        app_context_t                   context    = {0};
        acc_ref_app_tank_level_config_t app_config = {0};
        context.app_config                         = &app_config;
        context.sensor                             = 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;
        }
 
        context.app_config->distance_config = acc_detector_distance_config_create();
        if (context.app_config->distance_config == NULL)
        {
                printf("acc_detector_distance_config_create() failed\n");
                cleanup(&context);
                return EXIT_FAILURE;
        }
 
        set_config(&app_config, DEFAULT_PRESET_CONFIG);
 
        uint32_t sleep_time_ms = (uint32_t)(1000.0f / DEFAULT_UPDATE_RATE);
 
        acc_integration_set_periodic_wakeup(sleep_time_ms);
 
        if (!initialize_application_resources(&context))
        {
                printf("Initializing detector context failed\n");
                cleanup(&context);
                return EXIT_FAILURE;
        }
 
        /* Turn the sensor on */
        acc_hal_integration_sensor_supply_on(SENSOR_ID);
        acc_hal_integration_sensor_enable(SENSOR_ID);
 
        context.sensor = acc_sensor_create(SENSOR_ID);
        if (context.sensor == NULL)
        {
                printf("acc_sensor_create() failed\n");
                cleanup(&context);
                return EXIT_FAILURE;
        }
 
        acc_cal_result_t cal_result;
 
        if (!sensor_calibration(context.sensor, &cal_result, context.buffer, context.buffer_size))
        {
                printf("Sensor calibration failed\n");
                cleanup(&context);
                return EXIT_FAILURE;
        }
 
        if (!full_detector_calibration(&context, &cal_result))
        {
                printf("Detector calibration failed\n");
                cleanup(&context);
                return EXIT_FAILURE;
        }
 
        while (true)
        {
                acc_detector_distance_result_t detector_result = {0};
                app_result_t                   app_result      = {0};
 
                if (!detector_get_next(&context, &cal_result, &detector_result))
                {
                        printf("Could not get next result\n");
                        cleanup(&context);
                        return EXIT_FAILURE;
                }
 
                process_detector_result(&detector_result, &app_result, &context);
 
                /* If "calibration needed" is indicated, the sensor needs to be recalibrated and the detector calibration updated */
                if (detector_result.calibration_needed)
                {
                        printf("Sensor recalibration and detector calibration update needed ... \n");
 
                        if (!sensor_calibration(context.sensor, &cal_result, context.buffer, context.buffer_size))
                        {
                                printf("Sensor calibration failed\n");
                                cleanup(&context);
                                return EXIT_FAILURE;
                        }
 
                        /* Once the sensor is recalibrated, the detector calibration should be updated and measuring can continue. */
                        if (!update_detector_calibration(&context, &cal_result))
                        {
                                printf("Detector calibration update failed\n");
                                cleanup(&context);
                                return EXIT_FAILURE;
                        }
 
                        printf("Sensor recalibration and detector calibration update done!\n");
                }
 
                if (app_result.result_ready)
                {
                        acc_hal_integration_sensor_disable(SENSOR_ID);
 
                        print_result(&app_result, context.app_config);
 
                        acc_integration_sleep_until_periodic_wakeup();
 
                        acc_hal_integration_sensor_enable(SENSOR_ID);
                }
        }
 
        cleanup(&context);
 
        printf("Application finished OK\n");
 
        return EXIT_SUCCESS;
}
 
static void cleanup(app_context_t *context)
{
        acc_hal_integration_sensor_disable(SENSOR_ID);
        acc_hal_integration_sensor_supply_off(SENSOR_ID);
 
        acc_detector_distance_config_destroy(context->app_config->distance_config);
        acc_detector_distance_destroy(context->detector_handle);
 
        acc_integration_mem_free(context->buffer);
        acc_integration_mem_free(context->detector_cal_result_static);
        acc_integration_mem_free(context->level_history);
        acc_integration_mem_free(context->median_vector);
 
        if (context->sensor != NULL)
        {
                acc_sensor_destroy(context->sensor);
        }
}
 
static float get_detector_start_m(acc_ref_app_tank_level_config_t *app_config)
{
        // Decrease start point by 15 mm to make sure that start of the tank is fully covered
        return app_config->tank_range_start_m - 0.015f;
}
 
static float get_detector_end_m(acc_ref_app_tank_level_config_t *app_config)
{
        // Increase end point by 5% to make sure that the bottom of the tank is fully covered
        return fminf(app_config->tank_range_end_m * 1.05f, 23.0f);
}
 
static void set_config(acc_ref_app_tank_level_config_t *app_config, tank_level_preset_config_t preset)
{
        switch (preset)
        {
                case TANK_LEVEL_PRESET_CONFIG_NONE:
                        break;
                case TANK_LEVEL_PRESET_CONFIG_SMALL_TANK:
                        app_config->tank_range_start_m     = SMALL_TANK_RANGE_START_M;
                        app_config->tank_range_end_m       = SMALL_TANK_RANGE_END_M;
                        app_config->median_filter_length   = SMALL_TANK_MEDIAN_FILTER_LENGTH;
                        app_config->num_medians_to_average = SMALL_TANK_NUM_MEDIANS_TO_AVERAGE;
 
                        acc_detector_distance_config_start_set(app_config->distance_config, get_detector_start_m(app_config));
                        acc_detector_distance_config_end_set(app_config->distance_config, get_detector_end_m(app_config));
                        acc_detector_distance_config_max_step_length_set(app_config->distance_config, SMALL_TANK_MAX_STEP_LENGTH);
                        acc_detector_distance_config_max_profile_set(app_config->distance_config, SMALL_TANK_MAX_PROFILE);
                        acc_detector_distance_config_num_frames_recorded_threshold_set(app_config->distance_config, SMALL_TANK_NUM_FRAMES_REC);
                        acc_detector_distance_config_peak_sorting_set(app_config->distance_config, SMALL_TANK_PEAK_SORTING);
                        acc_detector_distance_config_reflector_shape_set(app_config->distance_config, SMALL_TANK_REFLECTOR_SHAPE);
                        acc_detector_distance_config_threshold_method_set(app_config->distance_config, SMALL_TANK_THRESHOLD_METHOD);
                        acc_detector_distance_config_fixed_amplitude_threshold_value_set(app_config->distance_config, SMALL_TANK_FIXED_AMPLITUDE_THRESHOLD);
                        acc_detector_distance_config_fixed_strength_threshold_value_set(app_config->distance_config, SMALL_TANK_FIXED_STRENGTH_THRESHOLD);
                        acc_detector_distance_config_threshold_sensitivity_set(app_config->distance_config, SMALL_TANK_THRESHOLD_SENSITIVITY);
                        acc_detector_distance_config_signal_quality_set(app_config->distance_config, SMALL_TANK_SIGNAL_QUALITY);
                        acc_detector_distance_config_close_range_leakage_cancellation_set(app_config->distance_config,
                                                                                          SMALL_TANK_CLOSE_RANGE_LEAKAGE_CANCELLATION);
                        break;
                case TANK_LEVEL_PRESET_CONFIG_MEDIUM_TANK:
                        app_config->tank_range_start_m     = MEDIUM_TANK_RANGE_START_M;
                        app_config->tank_range_end_m       = MEDIUM_TANK_RANGE_END_M;
                        app_config->median_filter_length   = MEDIUM_TANK_MEDIAN_FILTER_LENGTH;
                        app_config->num_medians_to_average = MEDIUM_TANK_NUM_MEDIANS_TO_AVERAGE;
 
                        acc_detector_distance_config_start_set(app_config->distance_config, get_detector_start_m(app_config));
                        acc_detector_distance_config_end_set(app_config->distance_config, get_detector_end_m(app_config));
                        acc_detector_distance_config_max_step_length_set(app_config->distance_config, MEDIUM_TANK_MAX_STEP_LENGTH);
                        acc_detector_distance_config_max_profile_set(app_config->distance_config, MEDIUM_TANK_MAX_PROFILE);
                        acc_detector_distance_config_num_frames_recorded_threshold_set(app_config->distance_config, MEDIUM_TANK_NUM_FRAMES_REC);
                        acc_detector_distance_config_peak_sorting_set(app_config->distance_config, MEDIUM_TANK_PEAK_SORTING);
                        acc_detector_distance_config_reflector_shape_set(app_config->distance_config, MEDIUM_TANK_REFLECTOR_SHAPE);
                        acc_detector_distance_config_threshold_method_set(app_config->distance_config, MEDIUM_TANK_THRESHOLD_METHOD);
                        acc_detector_distance_config_fixed_amplitude_threshold_value_set(app_config->distance_config, MEDIUM_TANK_FIXED_AMPLITUDE_THRESHOLD);
                        acc_detector_distance_config_fixed_strength_threshold_value_set(app_config->distance_config, MEDIUM_TANK_FIXED_STRENGTH_THRESHOLD);
                        acc_detector_distance_config_threshold_sensitivity_set(app_config->distance_config, MEDIUM_TANK_THRESHOLD_SENSITIVITY);
                        acc_detector_distance_config_signal_quality_set(app_config->distance_config, MEDIUM_TANK_SIGNAL_QUALITY);
                        acc_detector_distance_config_close_range_leakage_cancellation_set(app_config->distance_config,
                                                                                          MEDIUM_TANK_CLOSE_RANGE_LEAKAGE_CANCELLATION);
                        break;
                case TANK_LEVEL_PRESET_CONFIG_LARGE_TANK:
                        app_config->tank_range_start_m     = LARGE_TANK_RANGE_START_M;
                        app_config->tank_range_end_m       = LARGE_TANK_RANGE_END_M;
                        app_config->median_filter_length   = LARGE_TANK_MEDIAN_FILTER_LENGTH;
                        app_config->num_medians_to_average = LARGE_TANK_NUM_MEDIANS_TO_AVERAGE;
 
                        acc_detector_distance_config_start_set(app_config->distance_config, get_detector_start_m(app_config));
                        acc_detector_distance_config_end_set(app_config->distance_config, get_detector_end_m(app_config));
                        acc_detector_distance_config_max_step_length_set(app_config->distance_config, LARGE_TANK_MAX_STEP_LENGTH);
                        acc_detector_distance_config_max_profile_set(app_config->distance_config, LARGE_TANK_MAX_PROFILE);
                        acc_detector_distance_config_num_frames_recorded_threshold_set(app_config->distance_config, LARGE_TANK_NUM_FRAMES_REC);
                        acc_detector_distance_config_peak_sorting_set(app_config->distance_config, LARGE_TANK_PEAK_SORTING);
                        acc_detector_distance_config_reflector_shape_set(app_config->distance_config, LARGE_TANK_REFLECTOR_SHAPE);
                        acc_detector_distance_config_threshold_method_set(app_config->distance_config, LARGE_TANK_THRESHOLD_METHOD);
                        acc_detector_distance_config_fixed_amplitude_threshold_value_set(app_config->distance_config, LARGE_TANK_FIXED_AMPLITUDE_THRESHOLD);
                        acc_detector_distance_config_fixed_strength_threshold_value_set(app_config->distance_config, LARGE_TANK_FIXED_STRENGTH_THRESHOLD);
                        acc_detector_distance_config_threshold_sensitivity_set(app_config->distance_config, LARGE_TANK_THRESHOLD_SENSITIVITY);
                        acc_detector_distance_config_signal_quality_set(app_config->distance_config, LARGE_TANK_SIGNAL_QUALITY);
                        acc_detector_distance_config_close_range_leakage_cancellation_set(app_config->distance_config,
                                                                                          LARGE_TANK_CLOSE_RANGE_LEAKAGE_CANCELLATION);
                        break;
        }
}
 
static bool initialize_application_resources(app_context_t *context)
{
        context->detector_handle = acc_detector_distance_create(context->app_config->distance_config);
        if (context->detector_handle == NULL)
        {
                printf("acc_detector_distance_create() failed\n");
                return false;
        }
 
        if (!acc_detector_distance_get_sizes(context->detector_handle, &(context->buffer_size), &(context->detector_cal_result_static_size)))
        {
                printf("acc_detector_distance_get_sizes() failed\n");
                return false;
        }
 
        context->buffer = acc_integration_mem_alloc(context->buffer_size);
        if (context->buffer == NULL)
        {
                printf("sensor buffer allocation failed\n");
                return false;
        }
 
        context->detector_cal_result_static = acc_integration_mem_alloc(context->detector_cal_result_static_size);
        if (context->detector_cal_result_static == NULL)
        {
                printf("calibration buffer allocation failed\n");
                return false;
        }
 
        context->level_history_length = context->app_config->median_filter_length;
 
        context->level_history = acc_integration_mem_alloc(context->level_history_length * sizeof(*context->level_history));
        if (context->level_history == NULL)
        {
                printf("level history buffer allocation failed\n");
                return false;
        }
 
        context->median_vector_length = context->app_config->num_medians_to_average;
 
        context->median_vector = acc_integration_mem_alloc(context->median_vector_length * sizeof(*context->median_vector));
        if (context->median_vector == NULL)
        {
                printf("median vector allocation failed\n");
                return false;
        }
 
        context->median_counter           = 0U;
        context->mean_counter             = 0U;
        context->median_edge_status_count = 0U;
        context->mean_edge_status_count   = 0U;
 
        return true;
}
 
static bool sensor_calibration(acc_sensor_t *sensor, acc_cal_result_t *sensor_cal_result, void *buffer, uint32_t buffer_size)
{
        bool           status              = false;
        bool           cal_complete        = false;
        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(sensor, &cal_complete, sensor_cal_result, buffer, 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);
        }
        else
        {
                printf("acc_sensor_calibrate() failed\n");
                acc_sensor_status(sensor);
        }
 
        return status;
}
 
static bool full_detector_calibration(app_context_t *context, const acc_cal_result_t *sensor_cal_result)
{
        bool done = false;
        bool status;
 
        do
        {
                status = acc_detector_distance_calibrate(context->sensor,
                                                         context->detector_handle,
                                                         sensor_cal_result,
                                                         context->buffer,
                                                         context->buffer_size,
                                                         context->detector_cal_result_static,
                                                         context->detector_cal_result_static_size,
                                                         &context->detector_cal_result_dynamic,
                                                         &done);
                if (done)
                {
                        break;
                }
 
                if (status)
                {
                        status = acc_hal_integration_wait_for_sensor_interrupt(SENSOR_ID, SENSOR_TIMEOUT_MS);
                }
        } while (status);
 
        return status;
}
 
static bool update_detector_calibration(app_context_t *context, const acc_cal_result_t *sensor_cal_result)
{
        bool done = false;
        bool status;
 
        do
        {
                status = acc_detector_distance_update_calibration(context->sensor,
                                                                  context->detector_handle,
                                                                  sensor_cal_result,
                                                                  context->buffer,
                                                                  context->buffer_size,
                                                                  &context->detector_cal_result_dynamic,
                                                                  &done);
                if (done)
                {
                        break;
                }
 
                if (status)
                {
                        status = acc_hal_integration_wait_for_sensor_interrupt(SENSOR_ID, SENSOR_TIMEOUT_MS);
                }
        } while (status);
 
        return status;
}
 
static bool detector_get_next(app_context_t *context, const acc_cal_result_t *sensor_cal_result, acc_detector_distance_result_t *detector_result)
{
        bool result_available = false;
 
        do
        {
                if (!acc_detector_distance_prepare(context->detector_handle,
                                                   context->app_config->distance_config,
                                                   context->sensor,
                                                   sensor_cal_result,
                                                   context->buffer,
                                                   context->buffer_size))
                {
                        printf("acc_detector_distance_prepare() failed\n");
                        return false;
                }
 
                if (!acc_sensor_measure(context->sensor))
                {
                        printf("acc_sensor_measure() failed\n");
                        return false;
                }
 
                if (!acc_hal_integration_wait_for_sensor_interrupt(SENSOR_ID, SENSOR_TIMEOUT_MS))
                {
                        printf("Sensor interrupt timeout\n");
                        return false;
                }
 
                if (!acc_sensor_read(context->sensor, context->buffer, context->buffer_size))
                {
                        printf("acc_sensor_read() failed\n");
                        return false;
                }
 
                if (!acc_detector_distance_process(context->detector_handle,
                                                   context->buffer,
                                                   context->detector_cal_result_static,
                                                   &context->detector_cal_result_dynamic,
                                                   &result_available,
                                                   detector_result))
                {
                        printf("acc_detector_distance_process() failed\n");
                        return false;
                }
        } while (!result_available);
 
        return true;
}
 
static float median(float *array, uint16_t array_length)
{
        bool  status = true;
        float result = 0.0f;
 
        for (uint16_t i = 0U; i < array_length && status; i++)
        {
                if (isnan(array[i]))
                {
                        result = (float)NAN;
                        status = false;
                }
        }
 
        if (status)
        {
                result = acc_algorithm_median_f32(array, array_length);
        }
 
        return result;
}
 
static float nanmean(float *array, uint16_t array_length)
{
        uint16_t samples = 0U;
        float    sum     = 0.0f;
 
        for (uint16_t i = 0U; i < array_length; i++)
        {
                if (!isnan(array[i]))
                {
                        samples++;
                        sum += array[i];
                }
        }
 
        return samples > 0U ? sum / (float)samples : (float)NAN;
}
 
static void process_detector_result(const acc_detector_distance_result_t *distance_result, app_result_t *app_result, app_context_t *context)
{
        app_result->peak_detected = false;
        app_result->peak_status   = PEAK_STATUS_NO_DETECTION;
        app_result->level         = 0.0f;
        app_result->result_ready  = false;
        float level               = 0.0f;
 
        if (distance_result->num_distances > 0)
        {
                app_result->peak_detected = true;
                level                     = context->app_config->tank_range_end_m - distance_result->distances[0];
        }
        else
        {
                level = (float)NAN;
        }
 
        if (distance_result->near_start_edge_status)
        {
                context->median_edge_status_count++;
        }
 
        context->level_history[context->median_counter++] = level;
 
        if (context->median_counter == context->level_history_length)
        {
                float med = median(context->level_history, context->level_history_length);
 
                context->median_vector[context->mean_counter++] = med;
 
                if (context->median_edge_status_count > context->level_history_length / 2)
                {
                        context->mean_edge_status_count++;
                }
 
                context->median_counter           = 0U;
                context->median_edge_status_count = 0U;
        }
 
        if (context->mean_counter == context->median_vector_length)
        {
                level = nanmean(context->median_vector, context->median_vector_length);
 
                if (!isnan(level))
                {
                        if (level < 0.0f)
                        {
                                app_result->peak_status = PEAK_STATUS_OUT_OF_RANGE;
                        }
                        else if ((level > (context->app_config->tank_range_end_m - context->app_config->tank_range_start_m)) &&
                                 (context->app_config->tank_range_start_m >= CLOSE_RANGE_START))
                        {
                                app_result->peak_status = PEAK_STATUS_OVERFLOW;
                        }
                        else
                        {
                                app_result->peak_status = PEAK_STATUS_IN_RANGE;
                                app_result->level       = level;
                        }
                }
                else if ((context->mean_edge_status_count > context->median_vector_length / 2) &&
                         (context->app_config->tank_range_start_m >= CLOSE_RANGE_START))
                {
                        app_result->peak_status = PEAK_STATUS_OVERFLOW;
                }
 
                if (app_result->peak_status == PEAK_STATUS_OVERFLOW || app_result->peak_status == PEAK_STATUS_OUT_OF_RANGE)
                {
                        app_result->level = (float)NAN;
                }
 
                app_result->result_ready        = true;
                context->mean_counter           = 0U;
                context->mean_edge_status_count = 0U;
        }
}
 
static void print_result(const app_result_t *app_result, const acc_ref_app_tank_level_config_t *app_config)
{
        switch (app_result->peak_status)
        {
                case PEAK_STATUS_IN_RANGE:
                        printf("Level within range\n");
                        printf("Level: %" PRIfloat " cm, %" PRIfloat " %%\n",
                               ACC_LOG_FLOAT_TO_INTEGER(app_result->level * 100.0f),
                               ACC_LOG_FLOAT_TO_INTEGER((app_result->level / (app_config->tank_range_end_m - app_config->tank_range_start_m) * 100.f)));
                        break;
                case PEAK_STATUS_NO_DETECTION:
                        printf("No detection\n");
                        break;
                case PEAK_STATUS_OVERFLOW:
                        printf("Overflow!\n");
                        break;
                case PEAK_STATUS_OUT_OF_RANGE:
                        printf("Out of range\n");
                        break;
                default:
                        break;
        }
}