i2c_distance_detector.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 <stdarg.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.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_rss_a121.h"
#include "acc_version.h"
 
#include "acc_reg_protocol.h"
#include "distance_reg_protocol.h"
#include "i2c_application_system.h"
#include "i2c_distance_detector.h"
 
#define SENSOR_ID         (1U)
#define SENSOR_TIMEOUT_MS (10000U)
 
typedef struct
{
        acc_sensor_t                     *sensor;
        acc_cal_result_t                  sensor_cal_result;
        acc_detector_distance_config_t   *config;
        acc_detector_distance_handle_t   *handle;
        acc_detector_distance_result_t    result;
        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;
} distance_detector_resources_t;
 
static distance_detector_resources_t detector_resources       = {0};
static uint32_t                      i2c_detector_command     = 0U;
static uint32_t                      i2c_detector_status      = 0U;
static uint32_t                      measure_distance_counter = 0U;
static bool                          measure_distance_error   = false;
static bool                          measure_on_wakeup        = false;
static bool                          uart_logs_enabed         = false;
 
#define UART_LOG_BUFFER_SIZE 100
 
/**
 * @brief Get the i2c_detector_command values
 *
 * The i2c_detector_command is cleared during this read
 * The read and clear are protected by a critical section
 *
 * @return The command sent from the host
 */
static uint32_t get_command(void);
 
/**
 * @brief Execute the command sent from the host
 *
 * @param[in] command The command to execute
 */
static void command_handler(uint32_t command);
 
/**
 * @brief Set bits in the i2c_detector_status
 *
 * The i2c_detector_status is protected by a critical section
 *
 * @param[in] bit_mask The bit_mask to set
 */
static void detector_status_set_bits(uint32_t bit_mask);
 
/**
 * @brief Clear bits in the i2c_detector_status
 *
 * The i2c_detector_status is protected by a critical section
 *
 * @param[in] bit_mask The bit_mask to clear
 */
static void detector_status_clr_bits(uint32_t bit_mask);
 
/**
 * @brief Test bits in the i2c_detector_status
 *
 * The i2c_detector_status is protected by a critical section
 *
 * @param[in] bit_mask The bit_mask to test
 * @return true if all the bits in bit_mask is set in i2c_detector_status
 */
static bool detector_status_test_bits(uint32_t bit_mask);
 
/**
 * @brief Create sensor
 *
 * @param[in] resources distance detector resources struct
 */
static void create_sensor(distance_detector_resources_t *resources);
 
/**
 * @brief Apply detector config
 *
 * This function will create the distance detector and
 * allocate the needed memory
 *
 * @param[in] resources distance detector resources struct
 */
static void apply_detector_config(distance_detector_resources_t *resources);
 
/**
 * @brief Calibrate sensor
 *
 * @param[in] resources distance detector resources struct
 */
static void calibrate_sensor(distance_detector_resources_t *resources);
 
/**
 * @brief Calibrate detector
 *
 * @param[in] resources distance detector resources struct
 * @param[in] update_calibration Set to false for first calibration, true for calibration update
 */
static void calibrate_detector(distance_detector_resources_t *resources, bool update_calibration);
 
/**
 * @brief Test if detector is ready
 * @return true if detector is ready to do distance measurements
 */
static bool is_detector_ready(void);
 
/**
 * @brief Get next distance measurement
 *
 * @param[in] resources distance detector resources struct
 */
static bool detector_get_next(distance_detector_resources_t *resources);
 
/**
 * @brief Print the distance detector result
 *
 * Only available when the UART logs have been enabled with ENABLE_UART_LOGS
 *
 * @param[in] result The distance detector result
 */
static void print_distance_result(const acc_detector_distance_result_t *result);
 
/**
 * @brief UART logging function (can be enabled/disabled by command)
 */
static void uart_log(const char *format, ...);
 
//
// PUBLIC FUNCTIONS
//
 
acc_detector_distance_config_t *i2c_distance_detector_get_config(void)
{
        return detector_resources.config;
}
 
bool i2c_distance_detector_command(uint32_t command)
{
        bool status = false;
 
        /* Make sure we do not have a race for i2c_detector_command/i2c_detector_status */
        acc_integration_critical_section_enter();
 
        if (i2c_detector_command == 0U)
        {
                /* Set Ready PIN to LOW while processing the command */
                i2c_application_system_set_ready_pin(false);
 
                /* Set status BUSY bit */
                i2c_detector_status  |= DISTANCE_REG_DETECTOR_STATUS_FIELD_BUSY_MASK;
                i2c_detector_command  = command;
                status                = true;
        }
 
        acc_integration_critical_section_exit();
        return status;
}
 
uint32_t i2c_distance_detector_get_status(void)
{
        /* Make sure we do not have a race for i2c_detector_status */
        acc_integration_critical_section_enter();
 
        uint32_t status = i2c_detector_status;
 
        acc_integration_critical_section_exit();
 
        return status;
}
 
uint32_t i2c_distance_detector_get_result(void)
{
        uint32_t value = 0;
 
        /* Add number of distances */
        value = (detector_resources.result.num_distances << DISTANCE_REG_DISTANCE_RESULT_FIELD_NUM_DISTANCES_POS) &
                DISTANCE_REG_DISTANCE_RESULT_FIELD_NUM_DISTANCES_MASK;
 
        /* Add near start edge boolean */
        if (detector_resources.result.near_start_edge_status)
        {
                value |= DISTANCE_REG_DISTANCE_RESULT_FIELD_NEAR_START_EDGE_MASK;
        }
 
        /* Add calibration needed boolean */
        if (detector_resources.result.calibration_needed)
        {
                value |= DISTANCE_REG_DISTANCE_RESULT_FIELD_CALIBRATION_NEEDED_MASK;
        }
 
        if (measure_distance_error)
        {
                value |= DISTANCE_REG_DISTANCE_RESULT_FIELD_MEASURE_DISTANCE_ERROR_MASK;
        }
 
        /* Add temperature */
        uint32_t temp = (uint32_t)detector_resources.result.temperature;
 
        temp   = (temp << DISTANCE_REG_DISTANCE_RESULT_FIELD_TEMPERATURE_POS) & DISTANCE_REG_DISTANCE_RESULT_FIELD_TEMPERATURE_MASK;
        value |= temp;
 
        return value;
}
 
uint32_t i2c_distance_detector_get_counter(void)
{
        /* Make sure we do not have a race for measure_distance_counter */
        acc_integration_critical_section_enter();
 
        uint32_t counter = measure_distance_counter;
 
        acc_integration_critical_section_exit();
 
        return counter;
}
 
float i2c_distance_detector_get_peak_distance(uint16_t peak_id)
{
        float peak_distance = 0.0f;
 
        if (peak_id < ACC_DETECTOR_DISTANCE_RESULT_MAX_NUM_DISTANCES)
        {
                peak_distance = detector_resources.result.distances[peak_id];
        }
 
        return peak_distance;
}
 
float i2c_distance_detector_get_peak_strength(uint16_t peak_id)
{
        float peak_strength = 0.0f;
 
        if (peak_id < ACC_DETECTOR_DISTANCE_RESULT_MAX_NUM_DISTANCES)
        {
                peak_strength = detector_resources.result.strengths[peak_id];
        }
 
        return peak_strength;
}
 
void i2c_distance_detector_measure_on_wakeup(bool enable)
{
        /* Make sure we do not have a race for measure_on_wakeup */
        acc_integration_critical_section_enter();
 
        measure_on_wakeup = enable;
 
        acc_integration_critical_section_exit();
}
 
bool i2c_distance_detector_get_measure_on_wakeup(void)
{
        /* Make sure we do not have a race for measure_on_wakeup */
        acc_integration_critical_section_enter();
 
        bool value = measure_on_wakeup;
 
        acc_integration_critical_section_exit();
 
        return value;
}
 
//
// MAIN
//
 
int acconeer_main(int argc, char *argv[]);
 
int acconeer_main(int argc, char *argv[])
{
        (void)argc;
        (void)argv;
 
        bool setup_status = true;
 
        printf("I2C Distance Detector\n");
        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))
        {
                detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_RSS_REGISTER_OK_MASK);
        }
        else
        {
                printf("ERROR: acc_rss_hal_register() failed\n\n");
                detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_DETECTOR_ERROR_MASK | DISTANCE_REG_DETECTOR_STATUS_FIELD_RSS_REGISTER_ERROR_MASK);
                setup_status = false;
        }
 
        if (setup_status)
        {
                detector_resources.config = acc_detector_distance_config_create();
                if (detector_resources.config != NULL)
                {
                        /* Config is created, write default values to registers */
                        distance_reg_protocol_write_default();
 
                        detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_CONFIG_CREATE_OK_MASK);
                }
                else
                {
                        detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_DETECTOR_ERROR_MASK |
                                                 DISTANCE_REG_DETECTOR_STATUS_FIELD_CONFIG_CREATE_ERROR_MASK);
                        printf("ERROR: acc_detector_distance_config_create() failed\n\n");
                        setup_status = false;
                }
        }
 
        /* Turn the sensor on */
        acc_hal_integration_sensor_supply_on(SENSOR_ID);
 
        if (setup_status)
        {
                /* Create sensor */
                create_sensor(&detector_resources);
        }
 
        i2c_application_system_init();
 
        /* Setup i2c register protocol */
        distance_reg_protocol_setup();
 
        while (true)
        {
 
                uint32_t command = get_command();
 
                if (command == 0)
                {
                        if (i2c_application_system_test_wakeup_pin())
                        {
                                i2c_application_system_wait_for_interrupt();
                        }
                        else
                        {
                                /* Set ready pin LOW, we are about to power down */
                                i2c_application_system_set_ready_pin(false);
 
                                uart_log("Enter low power state\n");
                                i2c_application_enter_low_power_state();
                                uart_log("Exit low power state\n");
 
                                /**
                                 * Test if measure on wake up is enable
                                 * Do a measurement if the the detector is ready (configured and calibrated)
                                 */
                                if (is_detector_ready() && i2c_distance_detector_get_measure_on_wakeup())
                                {
                                        uart_log("Measure on wakeup\n");
                                        command = DISTANCE_REG_COMMAND_ENUM_MEASURE_DISTANCE;
                                }
                        }
 
                        /* Do an extra check on command, are we in measure on wakeup mode? */
                        if (command == 0)
                        {
                                /* Set ready pin HIGH, we are ready for communication */
                                i2c_application_system_set_ready_pin(true);
 
                                /* No command to process */
                                continue;
                        }
                }
 
                /* Special command, always handle reset module command, even if error has occured */
                if (command == DISTANCE_REG_COMMAND_ENUM_RESET_MODULE)
                {
                        /* Reset system */
                        i2c_application_system_reset();
                        continue;
                }
 
                if (detector_status_test_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_DETECTOR_ERROR_MASK))
                {
                        /* Do not process commands after error state */
                        continue;
                }
 
                /* Handle command */
                command_handler(command);
 
                /* Command handler done, clear busy bit */
                detector_status_clr_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_BUSY_MASK);
 
                /* Set Ready PIN to HIGH when command processing is done */
                i2c_application_system_set_ready_pin(true);
        }
 
        return EXIT_FAILURE;
}
 
//
// PRIVATE HELPER FUNCTIONS
//
 
static uint32_t get_command(void)
{
        /* Make sure we do not have a race for i2c_detector_command */
        acc_integration_critical_section_enter();
 
        uint32_t command = i2c_detector_command;
 
        i2c_detector_command = 0U;
 
        acc_integration_critical_section_exit();
 
        return command;
}
 
static void command_handler(uint32_t command)
{
        bool do_apply_config = false;
        bool do_calibrate    = false;
        bool do_recalibrate  = false;
 
        switch (command)
        {
                case DISTANCE_REG_COMMAND_ENUM_APPLY_CONFIGURATION:
                        if (!detector_status_test_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_CONFIG_APPLY_OK_MASK))
                        {
                                do_apply_config = true;
                        }
 
                        break;
                case DISTANCE_REG_COMMAND_ENUM_CALIBRATE:
                        if (detector_status_test_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_CONFIG_APPLY_OK_MASK))
                        {
                                do_calibrate = true;
                        }
 
                        break;
                case DISTANCE_REG_COMMAND_ENUM_RECALIBRATE:
                        if (detector_status_test_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_CONFIG_APPLY_OK_MASK))
                        {
                                do_recalibrate = true;
                        }
 
                        break;
                case DISTANCE_REG_COMMAND_ENUM_APPLY_CONFIG_AND_CALIBRATE:
                        if (!detector_status_test_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_CONFIG_APPLY_OK_MASK))
                        {
                                do_apply_config = true;
                                do_calibrate    = true;
                        }
 
                        break;
                case DISTANCE_REG_COMMAND_ENUM_MEASURE_DISTANCE:
                        memset(&detector_resources.result, 0, sizeof(detector_resources.result));
 
                        /* Increase counter - Make sure we do not have a race for measure_distance_counter */
                        acc_integration_critical_section_enter();
                        measure_distance_counter++;
                        acc_integration_critical_section_exit();
 
                        if (detector_get_next(&detector_resources))
                        {
                                print_distance_result(&detector_resources.result);
                                measure_distance_error = false;
                        }
                        else
                        {
                                printf("ERROR: Could not get next result\n");
                                measure_distance_error = true;
                        }
 
                        break;
                case DISTANCE_REG_COMMAND_ENUM_ENABLE_UART_LOGS:
                        uart_logs_enabed = true;
                        uart_log("UART logs enabled\n");
                        break;
                case DISTANCE_REG_COMMAND_ENUM_DISABLE_UART_LOGS:
                        uart_log("UART logs disabled\n");
                        uart_logs_enabed = false;
                        break;
                case DISTANCE_REG_COMMAND_ENUM_LOG_CONFIGURATION:
                        // Print the configuration
                        acc_detector_distance_config_log(detector_resources.handle, detector_resources.config);
                        break;
                default:
                        printf("ERROR: Unknown command: %" PRIu32 "", command);
                        break;
        }
 
        /* Apply config is activated by APPLY_CONFIGURATION or APPLY_CONFIG_AND_CALIBRATE */
        if (do_apply_config)
        {
                apply_detector_config(&detector_resources);
        }
 
        if (do_calibrate || do_recalibrate)
        {
                calibrate_sensor(&detector_resources);
                calibrate_detector(&detector_resources, do_recalibrate);
        }
}
 
static void detector_status_set_bits(uint32_t bit_mask)
{
        /* Make sure we do not have a race for i2c_detector_status */
        acc_integration_critical_section_enter();
 
        i2c_detector_status           |= bit_mask;
        uint32_t temp_detector_status  = i2c_detector_status;
 
        acc_integration_critical_section_exit();
 
        uart_log("Detector Status = 0x%" PRIx32 "\n", temp_detector_status);
}
 
static void detector_status_clr_bits(uint32_t bit_mask)
{
        /* Make sure we do not have a race for i2c_detector_status */
        acc_integration_critical_section_enter();
 
        i2c_detector_status           &= ~bit_mask;
        uint32_t temp_detector_status  = i2c_detector_status;
 
        acc_integration_critical_section_exit();
 
        uart_log("Detector Status = 0x%" PRIx32 "\n", temp_detector_status);
}
 
static bool detector_status_test_bits(uint32_t bit_mask)
{
        /* Make sure we do not have a race for i2c_detector_status */
        acc_integration_critical_section_enter();
 
        bool status = (i2c_detector_status & bit_mask) == bit_mask;
 
        acc_integration_critical_section_exit();
 
        return status;
}
 
static void create_sensor(distance_detector_resources_t *resources)
{
        acc_hal_integration_sensor_enable(SENSOR_ID);
 
        resources->sensor = acc_sensor_create(SENSOR_ID);
 
        acc_hal_integration_sensor_disable(SENSOR_ID);
 
        if (resources->sensor != NULL)
        {
                detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_SENSOR_CREATE_OK_MASK);
        }
        else
        {
                detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_DETECTOR_ERROR_MASK |
                                         DISTANCE_REG_DETECTOR_STATUS_FIELD_SENSOR_CREATE_ERROR_MASK);
                printf("ERROR: acc_sensor_create() failed\n");
        }
}
 
static void apply_detector_config(distance_detector_resources_t *resources)
{
        bool status = true;
 
        resources->handle = acc_detector_distance_create(resources->config);
        if (resources->handle != NULL)
        {
                detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_DETECTOR_CREATE_OK_MASK);
        }
        else
        {
                detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_DETECTOR_ERROR_MASK |
                                         DISTANCE_REG_DETECTOR_STATUS_FIELD_DETECTOR_CREATE_ERROR_MASK);
                printf("ERROR: acc_detector_distance_create() failed\n");
                status = false;
        }
 
        if (status)
        {
                if (acc_detector_distance_get_sizes(resources->handle, &(resources->buffer_size), &(resources->detector_cal_result_static_size)))
                {
                        detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_DETECTOR_BUFFER_OK_MASK);
                }
                else
                {
                        detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_DETECTOR_ERROR_MASK |
                                                 DISTANCE_REG_DETECTOR_STATUS_FIELD_DETECTOR_BUFFER_ERROR_MASK);
                        printf("ERROR: acc_detector_distance_get_sizes() failed\n");
                        status = false;
                }
        }
 
        if (status)
        {
                resources->buffer = acc_integration_mem_alloc(resources->buffer_size);
                if (resources->buffer != NULL)
                {
                        detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_SENSOR_BUFFER_OK_MASK);
                }
                else
                {
                        detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_DETECTOR_ERROR_MASK |
                                                 DISTANCE_REG_DETECTOR_STATUS_FIELD_SENSOR_BUFFER_ERROR_MASK);
                        printf("ERROR: sensor buffer allocation failed\n");
                        status = false;
                }
        }
 
        if (status)
        {
                resources->detector_cal_result_static = acc_integration_mem_alloc(resources->detector_cal_result_static_size);
                if (resources->detector_cal_result_static != NULL)
                {
                        detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_CALIBRATION_BUFFER_OK_MASK);
                }
                else
                {
                        detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_DETECTOR_ERROR_MASK |
                                                 DISTANCE_REG_DETECTOR_STATUS_FIELD_CALIBRATION_BUFFER_ERROR_MASK);
                        printf("ERROR: calibration buffer allocation failed\n");
                        status = false;
                }
        }
 
        if (status)
        {
                detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_CONFIG_APPLY_OK_MASK);
        }
        else
        {
                printf("ERROR: apply detector config failed\n");
                detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_DETECTOR_ERROR_MASK | DISTANCE_REG_DETECTOR_STATUS_FIELD_CONFIG_APPLY_ERROR_MASK);
        }
}
 
static void calibrate_sensor(distance_detector_resources_t *resources)
{
        acc_hal_integration_sensor_enable(SENSOR_ID);
 
        detector_status_clr_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_SENSOR_CALIBRATE_OK_MASK |
                                 DISTANCE_REG_DETECTOR_STATUS_FIELD_SENSOR_CALIBRATE_ERROR_MASK);
 
        bool status;
        bool cal_complete = false;
 
        do
        {
                status = acc_sensor_calibrate(resources->sensor, &cal_complete, &resources->sensor_cal_result, resources->buffer, resources->buffer_size);
 
                if (cal_complete)
                {
                        break;
                }
 
                if (status)
                {
                        status = acc_hal_integration_wait_for_sensor_interrupt(SENSOR_ID, SENSOR_TIMEOUT_MS);
                }
        } while (status);
 
        if (status)
        {
                detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_SENSOR_CALIBRATE_OK_MASK);
        }
        else
        {
                detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_SENSOR_CALIBRATE_ERROR_MASK);
                printf("ERROR: acc_sensor_calibrate() failed\n");
        }
 
        /* Reset sensor after calibration by disabling it */
        acc_hal_integration_sensor_disable(SENSOR_ID);
}
 
static void calibrate_detector(distance_detector_resources_t *resources, bool update_calibration)
{
        bool done = false;
        bool status;
 
        acc_hal_integration_sensor_enable(SENSOR_ID);
 
        detector_status_clr_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_DETECTOR_CALIBRATE_OK_MASK |
                                 DISTANCE_REG_DETECTOR_STATUS_FIELD_DETECTOR_CALIBRATE_ERROR_MASK);
 
        do
        {
                if (update_calibration)
                {
                        status = acc_detector_distance_update_calibration(resources->sensor,
                                                                          resources->handle,
                                                                          &resources->sensor_cal_result,
                                                                          resources->buffer,
                                                                          resources->buffer_size,
                                                                          &resources->detector_cal_result_dynamic,
                                                                          &done);
                }
                else
                {
                        status = acc_detector_distance_calibrate(resources->sensor,
                                                                 resources->handle,
                                                                 &resources->sensor_cal_result,
                                                                 resources->buffer,
                                                                 resources->buffer_size,
                                                                 resources->detector_cal_result_static,
                                                                 resources->detector_cal_result_static_size,
                                                                 &resources->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);
 
        if (status)
        {
                detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_DETECTOR_CALIBRATE_OK_MASK);
        }
        else
        {
                if (update_calibration)
                {
                        printf("ERROR: acc_detector_distance_update_calibration() failed\n");
                }
                else
                {
                        printf("ERROR: acc_detector_distance_calibrate() failed\n");
                }
 
                detector_status_set_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_DETECTOR_CALIBRATE_ERROR_MASK);
        }
 
        acc_hal_integration_sensor_disable(SENSOR_ID);
}
 
static bool is_detector_ready(void)
{
        return detector_status_test_bits(DISTANCE_REG_DETECTOR_STATUS_FIELD_CONFIG_APPLY_OK_MASK |
                                         DISTANCE_REG_DETECTOR_STATUS_FIELD_SENSOR_CALIBRATE_OK_MASK |
                                         DISTANCE_REG_DETECTOR_STATUS_FIELD_DETECTOR_CALIBRATE_OK_MASK);
}
 
static bool detector_get_next(distance_detector_resources_t *resources)
{
        bool result_available = false;
        bool status           = true;
 
        acc_hal_integration_sensor_enable(SENSOR_ID);
 
        do
        {
                if (!acc_detector_distance_prepare(resources->handle,
                                                   resources->config,
                                                   resources->sensor,
                                                   &resources->sensor_cal_result,
                                                   resources->buffer,
                                                   resources->buffer_size))
                {
                        printf("ERROR: acc_detector_distance_prepare() failed\n");
                        status = false;
                        break;
                }
 
                if (!acc_sensor_measure(resources->sensor))
                {
                        printf("ERROR: acc_sensor_measure() failed\n");
                        status = false;
                        break;
                }
 
                if (!acc_hal_integration_wait_for_sensor_interrupt(SENSOR_ID, SENSOR_TIMEOUT_MS))
                {
                        printf("ERROR: Sensor interrupt timeout\n");
                        status = false;
                        break;
                }
 
                if (!acc_sensor_read(resources->sensor, resources->buffer, resources->buffer_size))
                {
                        printf("ERROR: acc_sensor_read() failed\n");
                        status = false;
                        break;
                }
 
                if (!acc_detector_distance_process(resources->handle,
                                                   resources->buffer,
                                                   resources->detector_cal_result_static,
                                                   &resources->detector_cal_result_dynamic,
                                                   &result_available,
                                                   &resources->result))
                {
                        printf("ERROR: acc_detector_distance_process() failed\n");
                        status = false;
                        break;
                }
        } while (!result_available);
 
        /* Disable sensor in between measurements to save power */
        acc_hal_integration_sensor_disable(SENSOR_ID);
 
        return status;
}
 
static void print_distance_result(const acc_detector_distance_result_t *result)
{
        uart_log("%d detected distances", result->num_distances);
        if (result->num_distances > 0)
        {
                uart_log(": ");
                for (uint8_t i = 0; i < result->num_distances; i++)
                {
                        uint32_t distance_mm = (uint32_t)(1000 * result->distances[i]);
                        uart_log("%" PRIu32 "mm, ", distance_mm);
                }
        }
 
        uart_log("\n");
}
 
static void uart_log(const char *format, ...)
{
        char log_buffer[UART_LOG_BUFFER_SIZE];
 
        va_list ap;
 
        va_start(ap, format);
 
        if (uart_logs_enabed)
        {
                int ret = vsnprintf(log_buffer, UART_LOG_BUFFER_SIZE, format, ap);
 
                if (ret >= UART_LOG_BUFFER_SIZE)
                {
                        log_buffer[UART_LOG_BUFFER_SIZE - 4] = '.';
                        log_buffer[UART_LOG_BUFFER_SIZE - 3] = '.';
                        log_buffer[UART_LOG_BUFFER_SIZE - 2] = '.';
                        log_buffer[UART_LOG_BUFFER_SIZE - 1] = 0;
                }
 
                printf("%s", log_buffer);
        }
 
        va_end(ap);
}