example_detector_distance_calibration_caching.c

Go to the documentation of this file.

// Copyright (c) Acconeer AB, 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 <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.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"
 
/** \example example_detector_distance_calibration_caching.c
 * @brief This is an example on how the sensor and detector calibration can be cached
 * @n
 * This example executes as follows:
 *   - Retrieve and register HAL struct
 *   - Create and initialize distance detector resources
 *   - Create and calibrate sensor
 *   - Calibrate detector
 *   - Loop:
 *     - Prepare, measure, read, process data using the detector
 *     - Check 'calibration_needed' indication
 *     - If needed, either use cached calibration or perform new calibration and store
 *   - Destroy resources
 */
 
#define SENSOR_ID (1U)
// 2 seconds should be enough even for long ranges and high signal quality
#define SENSOR_TIMEOUT_MS (2000U)
 
/**
 * A sensor calibration and the dynamic part of a detector calibration are valid at the
 * temperature they were done +- 15 degrees.
 *
 * If the temperature isn't controlled during caching, which is the case in this example,
 * the maximum amount of caches needs to be calculated from a temperature difference of 16.
 *
 * For example
 * - A calibration is done at 25 degrees, which means it is valid between 10 and 40 degrees
 * - The temperature changes to 41 degrees
 * - A new calibration needs to be done at 41 degrees since it is above the valid range for the previous calibration
 * - The new calibration is then valid between 26 and 56 degrees
 *
 * However, if the temperature is controlled, for example in a factory, the maximum amount
 * of caches can be calculated from a temperature difference of 30.
 *
 * For example
 * - A calibration is done at 25 degrees, which means it is valid between 10 and 40 degrees
 * - The temperature is manually changed to 55 degrees
 * - A new calibration can be done which is valid between 40 and 70 degrees
 *
 * The maximum temperature variation that the application will operate in is assumed to be
 * -40 to 85 degrees in this example.
 */
#define MAX_CAL_TEMP_DIFF  (16)
#define MAX_TEMP_VARIATION (125)
#define MAX_CACHE_COUNT    ((MAX_TEMP_VARIATION / MAX_CAL_TEMP_DIFF) + 1)
 
// Variables for temperature independent caching
static uint8_t *detector_cal_result_static;
static uint32_t detector_cal_result_static_size = 0;
 
// Variables for temperature dependent caching
static acc_cal_result_t                  sensor_cal_results[MAX_CACHE_COUNT];
static acc_detector_cal_result_dynamic_t detector_cal_results_dynamic[MAX_CACHE_COUNT];
static int16_t                           cal_temps[MAX_CACHE_COUNT];
static uint16_t                          curr_cache_count   = 0;
static uint16_t                          cache_index_in_use = 0;
 
typedef struct
{
        acc_sensor_t                   *sensor;
        acc_detector_distance_config_t *config;
        acc_detector_distance_handle_t *handle;
        void                           *buffer;
        uint32_t                        buffer_size;
} distance_detector_resources_t;
 
// General control functions
static void set_config(acc_detector_distance_config_t *detector_config);
 
static bool initialize_detector_resources(distance_detector_resources_t *resources);
 
static bool detector_get_next(distance_detector_resources_t *resources, acc_detector_distance_result_t *result);
 
static void cleanup(distance_detector_resources_t *resources);
 
// Calibration and caching functions
static bool sensor_and_detector_calibration(distance_detector_resources_t *resources, uint16_t cache_index);
 
static bool sensor_calibration(distance_detector_resources_t *resources, uint16_t cache_index);
 
static bool detector_calibration_full(distance_detector_resources_t *resources, uint16_t cache_index);
 
static bool detector_calibration_update(distance_detector_resources_t *resources, uint16_t cache_index);
 
static bool calibration_caching(distance_detector_resources_t *resources, int16_t temp);
 
// Caching helper functions
static bool find_cache_index(int16_t temp, uint16_t *cache_index);
 
static bool get_next_empty_cache_index(uint16_t *cache_index);
 
static bool add_cache(uint16_t cache_index, int16_t temp);
 
// Main function
int acconeer_main(int argc, char *argv[]);
 
int acconeer_main(int argc, char *argv[])
{
        (void)argc;
        (void)argv;
        distance_detector_resources_t resources = {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;
        }
 
        resources.config = acc_detector_distance_config_create();
        if (resources.config == NULL)
        {
                printf("Config creation failed\n");
                return EXIT_FAILURE;
        }
 
        set_config(resources.config);
 
        if (!initialize_detector_resources(&resources))
        {
                printf("Initializing detector resources failed\n");
                cleanup(&resources);
                return EXIT_FAILURE;
        }
 
        acc_detector_distance_config_log(resources.handle, resources.config);
 
        acc_hal_integration_sensor_supply_on(SENSOR_ID);
        acc_hal_integration_sensor_enable(SENSOR_ID);
 
        resources.sensor = acc_sensor_create(SENSOR_ID);
        if (resources.sensor == NULL)
        {
                printf("Sensor creation failed\n");
                cleanup(&resources);
                return EXIT_FAILURE;
        }
 
        // Do a sensor calibration and a full detector calibration and store first in cache.
        if (!sensor_and_detector_calibration(&resources, 0))
        {
                printf("Sensor or detector calibration failed\n");
                cleanup(&resources);
                return EXIT_FAILURE;
        }
 
        uint32_t update_count = 5U;
 
        for (uint32_t i = 0U; i < update_count; i++)
        {
                acc_detector_distance_result_t result = {0};
 
                if (!detector_get_next(&resources, &result))
                {
                        printf("Could not get next result\n");
                        cleanup(&resources);
                        return EXIT_FAILURE;
                }
 
                if (result.calibration_needed)
                {
                        printf("New calibrations needed due to temperature change\n");
 
                        if (!calibration_caching(&resources, result.temperature))
                        {
                                printf("Calibration caching failed\n");
                                cleanup(&resources);
                                return EXIT_FAILURE;
                        }
                }
                else
                {
                        printf("Distance result retrieved\n");
                }
        }
 
        cleanup(&resources);
 
        printf("Application finished OK\n");
 
        return EXIT_SUCCESS;
}
 
// General control functions
static void set_config(acc_detector_distance_config_t *detector_config)
{
        // Balanced detector config
        acc_detector_distance_config_start_set(detector_config, 0.25f);
        acc_detector_distance_config_end_set(detector_config, 3.0f);
        acc_detector_distance_config_max_step_length_set(detector_config, 0U);
        acc_detector_distance_config_max_profile_set(detector_config, ACC_CONFIG_PROFILE_5);
        acc_detector_distance_config_reflector_shape_set(detector_config, ACC_DETECTOR_DISTANCE_REFLECTOR_SHAPE_GENERIC);
        acc_detector_distance_config_peak_sorting_set(detector_config, ACC_DETECTOR_DISTANCE_PEAK_SORTING_STRONGEST);
        acc_detector_distance_config_threshold_method_set(detector_config, ACC_DETECTOR_DISTANCE_THRESHOLD_METHOD_CFAR);
        acc_detector_distance_config_num_frames_recorded_threshold_set(detector_config, 100U);
        acc_detector_distance_config_threshold_sensitivity_set(detector_config, 0.5f);
        acc_detector_distance_config_signal_quality_set(detector_config, 15.0f);
        acc_detector_distance_config_close_range_leakage_cancellation_set(detector_config, false);
}
 
static bool initialize_detector_resources(distance_detector_resources_t *resources)
{
        resources->handle = acc_detector_distance_create(resources->config);
        if (resources->handle == NULL)
        {
                printf("acc_detector_distance_create() failed\n");
                return false;
        }
 
        if (!acc_detector_distance_get_sizes(resources->handle, &(resources->buffer_size), &detector_cal_result_static_size))
        {
                printf("acc_detector_distance_get_sizes() failed\n");
                return false;
        }
 
        resources->buffer = acc_integration_mem_alloc(resources->buffer_size);
        if (resources->buffer == NULL)
        {
                printf("sensor buffer allocation failed\n");
                return false;
        }
 
        detector_cal_result_static = acc_integration_mem_alloc(detector_cal_result_static_size);
        if (detector_cal_result_static == NULL)
        {
                printf("static cal result allocation failed\n");
                return false;
        }
 
        return true;
}
 
static bool detector_get_next(distance_detector_resources_t *resources, acc_detector_distance_result_t *result)
{
        bool result_available = false;
 
        do
        {
                if (!acc_detector_distance_prepare(resources->handle,
                                                   resources->config,
                                                   resources->sensor,
                                                   &sensor_cal_results[cache_index_in_use],
                                                   resources->buffer,
                                                   resources->buffer_size))
                {
                        printf("acc_detector_distance_prepare() failed\n");
                        return false;
                }
 
                if (!acc_sensor_measure(resources->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(resources->sensor, resources->buffer, resources->buffer_size))
                {
                        printf("acc_sensor_read() failed\n");
                        return false;
                }
 
                if (!acc_detector_distance_process(resources->handle,
                                                   resources->buffer,
                                                   detector_cal_result_static,
                                                   &detector_cal_results_dynamic[cache_index_in_use],
                                                   &result_available,
                                                   result))
                {
                        printf("acc_detector_distance_process() failed\n");
                        return false;
                }
        } while (!result_available);
 
        return true;
}
 
static void cleanup(distance_detector_resources_t *resources)
{
        acc_hal_integration_sensor_disable(SENSOR_ID);
        acc_hal_integration_sensor_supply_off(SENSOR_ID);
 
        if (resources->sensor != NULL)
        {
                acc_sensor_destroy(resources->sensor);
        }
 
        if (detector_cal_result_static != NULL)
        {
                acc_integration_mem_free(detector_cal_result_static);
        }
 
        if (resources->buffer != NULL)
        {
                acc_integration_mem_free(resources->buffer);
        }
 
        if (resources->handle != NULL)
        {
                acc_detector_distance_destroy(resources->handle);
        }
 
        if (resources->config != NULL)
        {
                acc_detector_distance_config_destroy(resources->config);
        }
}
 
// Calibration and caching functions
static bool sensor_and_detector_calibration(distance_detector_resources_t *resources, uint16_t cache_index)
{
        bool status = sensor_calibration(resources, cache_index);
 
        if (status)
        {
                status = detector_calibration_full(resources, cache_index);
        }
 
        if (status)
        {
                acc_cal_info_t cal_info;
                acc_sensor_get_cal_info(&sensor_cal_results[cache_index], &cal_info);
                add_cache(cache_index, cal_info.temperature);
                cache_index_in_use = cache_index;
        }
 
        return status;
}
 
static bool sensor_calibration(distance_detector_resources_t *resources, uint16_t cache_index)
{
        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(resources->sensor, &cal_complete, &sensor_cal_results[cache_index], resources->buffer, resources->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);
        }
 
        return status;
}
 
static bool detector_calibration_full(distance_detector_resources_t *resources, uint16_t cache_index)
{
        bool status       = false;
        bool cal_complete = false;
 
        do
        {
                status = acc_detector_distance_calibrate(resources->sensor,
                                                         resources->handle,
                                                         &sensor_cal_results[cache_index],
                                                         resources->buffer,
                                                         resources->buffer_size,
                                                         detector_cal_result_static,
                                                         detector_cal_result_static_size,
                                                         &detector_cal_results_dynamic[cache_index],
                                                         &cal_complete);
 
                if (status && !cal_complete)
                {
                        status = acc_hal_integration_wait_for_sensor_interrupt(SENSOR_ID, SENSOR_TIMEOUT_MS);
                }
        } while (status && !cal_complete);
 
        return status;
}
 
static bool detector_calibration_update(distance_detector_resources_t *resources, uint16_t cache_index)
{
        bool status       = false;
        bool cal_complete = false;
 
        do
        {
                status = acc_detector_distance_update_calibration(resources->sensor,
                                                                  resources->handle,
                                                                  &sensor_cal_results[cache_index],
                                                                  resources->buffer,
                                                                  resources->buffer_size,
                                                                  &detector_cal_results_dynamic[cache_index],
                                                                  &cal_complete);
 
                if (status && !cal_complete)
                {
                        status = acc_hal_integration_wait_for_sensor_interrupt(SENSOR_ID, SENSOR_TIMEOUT_MS);
                }
        } while (status && !cal_complete);
 
        return status;
}
 
static bool calibration_caching(distance_detector_resources_t *resources, int16_t temp)
{
        bool     status      = true;
        uint16_t cache_index = 0;
        bool     use_cache   = find_cache_index(temp, &cache_index);
 
        // If no cached calibration can be used, a new calibration should be made
        // and the result stored at an empty cache_index
        if (!use_cache)
        {
                printf("New sensor calibration and detector calibration update is performed\n");
 
                if (!get_next_empty_cache_index(&cache_index))
                {
                        printf("No empty cache_index to store calibration result\n");
                        return false;
                }
 
                status = sensor_calibration(resources, cache_index);
 
                if (status)
                {
                        status = detector_calibration_update(resources, cache_index);
                }
 
                if (status)
                {
                        acc_cal_info_t cal_info;
                        acc_sensor_get_cal_info(&sensor_cal_results[cache_index], &cal_info);
                        add_cache(cache_index, cal_info.temperature);
                        cache_index_in_use = cache_index;
                }
        }
        else
        {
                cache_index_in_use = cache_index;
                printf("Using cached calibration for %u degrees Celsius\n", cal_temps[cache_index_in_use]);
        }
 
        return status;
}
 
// Caching helper functions
static bool find_cache_index(int16_t temp, uint16_t *cache_index)
{
        bool     cache_found   = false;
        uint16_t min_temp_diff = UINT16_MAX;
 
        // If caches have overlapping temperature ranges, the cache
        // with center temperature closest to 'temp' will be chosen
        for (uint16_t index = 0; index < curr_cache_count; index++)
        {
                uint16_t temp_diff = abs(cal_temps[index] - temp);
 
                if (temp_diff < MAX_CAL_TEMP_DIFF && temp_diff < min_temp_diff)
                {
                        min_temp_diff = temp_diff;
                        *cache_index  = index;
                        cache_found   = true;
                }
        }
 
        return cache_found;
}
 
static bool get_next_empty_cache_index(uint16_t *cache_index)
{
        *cache_index = curr_cache_count;
 
        return curr_cache_count < MAX_CACHE_COUNT;
}
 
static bool add_cache(uint16_t cache_index, int16_t temp)
{
        bool status = false;
 
        if ((cache_index == curr_cache_count) && (cache_index < MAX_CACHE_COUNT))
        {
                cal_temps[cache_index] = temp;
                curr_cache_count++;
                status = true;
        }
 
        return status;
}