example_waste_level.c

Go to the documentation of this file.

// Copyright (c) Acconeer AB, 2024-2025
// All rights reserved
// This file is subject to the terms and conditions defined in the file
// 'LICENSES/license_acconeer.txt', (BSD 3-Clause License) which is part
// of this source code package.
 
#include <complex.h>
#include <inttypes.h>
#include <math.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
 
#include "acc_algorithm.h"
#include "acc_config.h"
#include "acc_config_subsweep.h"
#include "acc_definitions_common.h"
#include "acc_integration.h"
#include "acc_integration_log.h"
#include "acc_processing.h"
#include "example_waste_level.h"
 
#define MODULE "example_waste_level"
 
struct waste_level_handle
{
        /** State. Affects output between frames */
        struct
        {
                /** Ring buffer for distance estimations. Has capacity processing_config.median_filter_len */
                float *distance_history_points;
 
                /** How many valid elements there are in distance_history_points (starting at 0) */
                uint16_t distance_history_len;
 
                /** Write index for the distance_history_points */
                uint16_t distance_history_write_idx;
        } state;
 
        /** Float scratch. Has length == max(sweeps_per_frame, median_filter_len) */
        float *scratch;
};
 
/**
 * @brief Validate an app_config (processing & sensor config).
 *
 * @param[in] app_config The app config
 * @return True if config was valid, false otherwise.
 */
static bool validate_app_config(const waste_level_app_config_t *app_config);
 
/**
 * @brief Helper function for validation
 *
 * @param[in] sensor_config The sensor config
 * @return True if all subsweeps' start_points are strictly increasing, false otherwise
 */
static bool subsweep_start_points_strictly_increases(const acc_config_t *sensor_config);
 
/**
 * @brief Helper function for validation
 *
 * @param[in] sensor_config The sensor config
 * @param[out] offending_subsweep The subsweep index of a subsweep that overlaps into
 *                                the subsweep after it
 * @return True if sensor config has overlapping (in distance) subsweeps, false otherwise
 */
static bool has_overlapping_subsweeps(const acc_config_t *sensor_config, uint8_t *offending_subsweep);
 
/**
 * @brief Helper function for validation
 *
 * @param[in] sensor_config The sensor config
 * @return True if start_point for subsweep 0 is invalid
 */
static bool is_invalid_start_point(const acc_config_t *sensor_config);
 
/**
 * @brief Helper function for validation
 *
 * @param[in] sensor_config The sensor config
 * @param[in] distance_m The distance to check
 * @return True if distance_m is outside the interval measured by sensor_config, false otherwise
 */
static bool is_outside_measuring_interval(const acc_config_t *sensor_config, float distance_m);
 
/**
 * @brief Helper function for validation
 *
 * @param[in] sensor_config The sensor config
 * @param[in] subsweep_idx The distance to check
 * @return The approximate distance to the first point measured in subsweep with index subsweep_idx
 */
static float subsweep_approx_start_m(const acc_config_t *sensor_config, uint8_t subsweep_idx);
 
/**
 * @brief Helper function for validation
 *
 * @param[in] sensor_config The sensor config
 * @param[in] subsweep_idx The distance to check
 * @return The approximate distance to the last point measured in subsweep with index subsweep_idx
 */
static float subsweep_approx_end_m(const acc_config_t *sensor_config, uint8_t subsweep_idx);
 
/**
 * @brief Calculate a point's corresponding distance in meters
 *
 * @param[in] sensor_config The sensor config
 * @param[in] metadata The processing metadata
 * @param[in] point_idx The index in a sweep of the point to get distance for
 * @return The distance of point at point_idx
 */
static float point_in_sweep_to_distance_m(const acc_config_t *sensor_config, const acc_processing_metadata_t *metadata, uint16_t point_idx);
 
/**
 * @brief Write next value into a ring buffer
 *
 * @param[in, out] buffer The circular buffer
 * @param[in] capacity The allocated number of elements of buffer
 * @param[in, out] length The buffer length (number of valid elements)
 * @param[in, out] write_idx The next index in buffer to write to
 * @param[in] elem The new element to write
 */
static void write_to_ring_buffer(float *buffer, uint16_t capacity, uint16_t *length, uint16_t *write_idx, float elem);
 
/**
 * @brief Copy non-NaN floats between buffers
 *
 * @param[in] in Source buffer of floats which can contain NaNs
 * @param[out] out The destination buffer. Will not contain NaNs
 * @param[in] length The allocated size of in & out
 * @return The number of elements written to out
 */
static uint16_t copy_non_nan_floats(const float *in, float *out, uint16_t length);
 
/**
 * @brief Populate a waste level result with its human-readable entries
 *
 * @param[in] filtered_distance The median filtered distance
 * @param[in] processing_config The waste processing config
 * @param[out] result The waste result
 */
static void set_level_numbers(float filtered_distance, const waste_level_processing_config_t *processing_config, waste_level_result_t *result);
 
waste_level_app_config_t *waste_level_app_config_create(void)
{
        waste_level_app_config_t *app_config = acc_integration_mem_alloc(sizeof(*app_config));
        bool                      status     = app_config != NULL;
 
        if (status)
        {
                app_config->sensor_config = acc_config_create();
                status                    = app_config->sensor_config != NULL;
        }
 
        if (status)
        {
                waste_level_app_config_set_preset(WASTE_LEVEL_PRESET_PLASTIC_WASTE_BIN, app_config);
        }
 
        return app_config;
}
 
void waste_level_app_config_destroy(waste_level_app_config_t *app_config)
{
        if (app_config != NULL)
        {
                if (app_config->sensor_config != NULL)
                {
                        acc_config_destroy(app_config->sensor_config);
                }
 
                acc_integration_mem_free(app_config);
        }
}
 
void waste_level_app_config_set_preset(waste_level_preset_t preset, waste_level_app_config_t *app_config)
{
        bool status = app_config != NULL;
 
        if (status)
        {
                switch (preset)
                {
                        case WASTE_LEVEL_PRESET_PLASTIC_WASTE_BIN:
                                app_config->processing_config.bin_start_m           = 0.15f;
                                app_config->processing_config.bin_end_m             = 1.00f;
                                app_config->processing_config.threshold             = 0.30f;
                                app_config->processing_config.distance_sequence_len = 4U;
                                app_config->processing_config.median_filter_len     = 5U;
 
                                acc_config_frame_rate_set(app_config->sensor_config, 5.0f);
                                acc_config_sweeps_per_frame_set(app_config->sensor_config, 32U);
                                acc_config_num_subsweeps_set(app_config->sensor_config, 2U);
 
                                acc_config_subsweep_start_point_set(app_config->sensor_config, 40U, 0U);
                                acc_config_subsweep_step_length_set(app_config->sensor_config, 8U, 0U);
                                acc_config_subsweep_num_points_set(app_config->sensor_config, 14U, 0U);
                                acc_config_subsweep_profile_set(app_config->sensor_config, ACC_CONFIG_PROFILE_1, 0U);
                                acc_config_subsweep_hwaas_set(app_config->sensor_config, 4U, 0U);
 
                                acc_config_subsweep_start_point_set(app_config->sensor_config, 150U, 1U);
                                acc_config_subsweep_step_length_set(app_config->sensor_config, 12U, 1U);
                                acc_config_subsweep_num_points_set(app_config->sensor_config, 23U, 1U);
                                acc_config_subsweep_profile_set(app_config->sensor_config, ACC_CONFIG_PROFILE_3, 1U);
                                acc_config_subsweep_hwaas_set(app_config->sensor_config, 8U, 1U);
                                break;
                        default:
                        case WASTE_LEVEL_PRESET_NONE:
                                break;
                }
        }
}
 
waste_level_handle_t *waste_level_handle_create(const waste_level_app_config_t *app_config)
{
        uint16_t              sweeps_per_frame  = 0U;
        uint16_t              median_filter_len = 0U;
        waste_level_handle_t *handle            = NULL;
 
        bool status = validate_app_config(app_config);
 
        if (status)
        {
                handle = acc_integration_mem_alloc(sizeof(*handle));
                status = handle != NULL;
        }
 
        if (status)
        {
                sweeps_per_frame  = acc_config_sweeps_per_frame_get(app_config->sensor_config);
                median_filter_len = app_config->processing_config.median_filter_len;
 
                handle->state.distance_history_len       = 0U;
                handle->state.distance_history_write_idx = 0U;
        }
 
        if (status)
        {
                uint16_t num_elem = sweeps_per_frame > median_filter_len ? sweeps_per_frame : median_filter_len;
                handle->scratch   = (float *)acc_integration_mem_alloc(num_elem * sizeof(*handle->scratch));
                status            = handle->scratch != NULL;
        }
 
        if (status)
        {
                handle->state.distance_history_points =
                    (float *)acc_integration_mem_alloc(median_filter_len * sizeof(*handle->state.distance_history_points));
                status = handle->state.distance_history_points != NULL;
        }
 
        return handle;
}
 
void waste_level_handle_destroy(waste_level_handle_t *handle)
{
        if (handle != NULL)
        {
                if (handle->state.distance_history_points != NULL)
                {
                        acc_integration_mem_free(handle->state.distance_history_points);
                }
 
                if (handle->scratch != NULL)
                {
                        acc_integration_mem_free(handle->scratch);
                }
 
                acc_integration_mem_free(handle);
        }
}
 
void waste_level_process(waste_level_handle_t            *handle,
                         const waste_level_app_config_t  *app_config,
                         const acc_processing_metadata_t *metadata,
                         const acc_int16_complex_t       *frame,
                         waste_level_result_t            *waste_level_result)
{
        if (handle != NULL && app_config != NULL && metadata != NULL && frame != NULL && waste_level_result != NULL)
        {
                uint16_t sweep_length      = metadata->sweep_data_length;
                uint16_t sweeps_per_frame  = acc_config_sweeps_per_frame_get(app_config->sensor_config);
                uint16_t distance_seq_len  = app_config->processing_config.distance_sequence_len;
                uint16_t median_filter_len = app_config->processing_config.median_filter_len;
                float    threshold_squared = app_config->processing_config.threshold * app_config->processing_config.threshold;
 
                uint16_t phase_vars_under_threshold = 0U;
 
                bool     point_of_waste_found = false;
                uint16_t point_of_waste       = 0U;
 
                for (uint16_t point_idx = 0U; point_idx < sweep_length; point_idx++)
                {
                        float complex column_sum;
 
                        acc_algorithm_sum_sweep(frame, sweep_length, sweeps_per_frame, point_idx, point_idx + 1U, &column_sum);
 
                        acc_algorithm_conj_f32(&column_sum, 1U);
 
                        for (uint16_t sweep_idx = 0U; sweep_idx < sweeps_per_frame; sweep_idx++)
                        {
                                uint16_t            frame_idx   = sweep_idx * sweep_length + point_idx;
                                acc_int16_complex_t point       = frame[frame_idx];
                                float complex       float_point = (float)point.real + (float)point.imag * I;
 
                                handle->scratch[sweep_idx] = cargf(float_point * column_sum);
                        }
 
                        float point_phase_variance = acc_algorithm_variance_f32(handle->scratch, sweeps_per_frame);
 
                        if (point_phase_variance < threshold_squared)
                        {
                                phase_vars_under_threshold++;
                        }
                        else
                        {
                                phase_vars_under_threshold = 0U;
                        }
 
                        if (phase_vars_under_threshold >= distance_seq_len)
                        {
                                point_of_waste       = point_idx - (phase_vars_under_threshold - 1U);
                                point_of_waste_found = true;
                                break;
                        }
                }
 
                float new_distance = !point_of_waste_found ? NAN : point_in_sweep_to_distance_m(app_config->sensor_config, metadata, point_of_waste);
 
                write_to_ring_buffer(handle->state.distance_history_points,
                                     median_filter_len,
                                     &handle->state.distance_history_len,
                                     &handle->state.distance_history_write_idx,
                                     new_distance);
 
                // copy non-NaN distance history points as median sorts inplace
                uint16_t num_normal = copy_non_nan_floats(handle->state.distance_history_points, handle->scratch, handle->state.distance_history_len);
                if (num_normal > 0U)
                {
                        waste_level_result->level_found = true;
 
                        float median_distance = acc_algorithm_median_f32(handle->scratch, num_normal);
                        set_level_numbers(median_distance, &app_config->processing_config, waste_level_result);
                }
                else
                {
                        waste_level_result->level_found = false;
                }
        }
}
 
void waste_level_processing_config_log(const waste_level_processing_config_t *config)
{
        printf("Waste level config:\n");
        printf("bin_start_m:           %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(config->bin_start_m));
        printf("bin_end_m:             %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(config->bin_end_m));
        printf("threshold:             %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(config->threshold));
        printf("distance_sequence_len: %" PRIu16 "\n", config->distance_sequence_len);
        printf("median_filter_len:     %" PRIu16 "\n", config->median_filter_len);
}
 
static bool validate_app_config(const waste_level_app_config_t *app_config)
{
        const waste_level_processing_config_t *processing_config;
        const acc_config_t                    *sensor_config;
 
        bool status = app_config != NULL;
 
        if (status)
        {
                sensor_config     = app_config->sensor_config;
                processing_config = &app_config->processing_config;
                status            = sensor_config != NULL && processing_config != NULL;
        }
 
        if (status && processing_config->bin_start_m < 0.08f)
        {
                fprintf(stderr, "Bin start needs to be greater or equal than 0.08m\n");
                status = false;
        }
 
        if (status && processing_config->bin_end_m < processing_config->bin_start_m)
        {
                fprintf(stderr, "Bin end needs to be greater than bin start\n");
                status = false;
        }
 
        if (status && (processing_config->threshold < 0.0f || processing_config->threshold > 4.0f))
        {
                fprintf(stderr, "Threshold shoud be in the range [0.0, 4.0], was %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(processing_config->threshold));
                status = false;
        }
 
        if (status && (processing_config->distance_sequence_len > 10U || processing_config->distance_sequence_len < 1U))
        {
                fprintf(stderr, "Distance sequence length shoud be in the range [1, 10], was %" PRIu16 "\n", processing_config->distance_sequence_len);
                status = false;
        }
 
        if (status && (processing_config->median_filter_len > 10U || processing_config->median_filter_len < 1U))
        {
                fprintf(stderr, "Median filter length shoud be in the range [1, 10], was %" PRIu16 "\n", processing_config->median_filter_len);
                status = false;
        }
 
        if (status && !subsweep_start_points_strictly_increases(sensor_config))
        {
                fprintf(stderr, "Subsweeps with higher indexes should measure farther away than those with lower indexes\n");
                status = false;
        }
 
        if (status)
        {
                uint8_t subsweep_idx;
 
                if (has_overlapping_subsweeps(sensor_config, &subsweep_idx))
                {
                        fprintf(stderr, "Disallowed range overlap between susweeps %" PRIu16 " and %" PRIu16 "\n", subsweep_idx, subsweep_idx + 1U);
                        status = false;
                }
        }
 
        if (status && is_invalid_start_point(sensor_config))
        {
                fprintf(stderr, "Start point must be greater or equal than 32\n");
                status = false;
        }
 
        if (status && is_outside_measuring_interval(sensor_config, processing_config->bin_start_m))
        {
                fprintf(stderr, "Bin start is outside measuring interval\n");
                status = false;
        }
 
        if (status && is_outside_measuring_interval(sensor_config, processing_config->bin_end_m))
        {
                fprintf(stderr, "Bin end is outside measuring interval\n");
                status = false;
        }
 
        if (status && acc_config_sweeps_per_frame_get(sensor_config) <= 3)
        {
                fprintf(stderr, "Sweeps per frame needs to be greater than 3\n");
                status = false;
        }
 
        return status;
}
 
static bool subsweep_start_points_strictly_increases(const acc_config_t *sensor_config)
{
        bool increasing = true;
 
        uint8_t num_subsweeps = acc_config_num_subsweeps_get(sensor_config);
 
        for (uint8_t subsweep_idx = 1U; subsweep_idx < num_subsweeps; subsweep_idx++)
        {
                int32_t prev_start = acc_config_subsweep_start_point_get(sensor_config, subsweep_idx - 1U);
                int32_t curr_start = acc_config_subsweep_start_point_get(sensor_config, subsweep_idx);
 
                if (curr_start <= prev_start)
                {
                        increasing = false;
                        break;
                }
        }
 
        return increasing;
}
 
static bool has_overlapping_subsweeps(const acc_config_t *sensor_config, uint8_t *offending_subsweep)
{
        bool    overlap_found = false;
        uint8_t num_subsweeps = acc_config_num_subsweeps_get(sensor_config);
 
        for (uint8_t subsweep_idx = 0U; subsweep_idx < num_subsweeps - 1U; subsweep_idx++)
        {
                float curr_end   = subsweep_approx_end_m(sensor_config, subsweep_idx);
                float next_start = subsweep_approx_start_m(sensor_config, subsweep_idx + 1U);
 
                if (curr_end >= next_start)
                {
                        *offending_subsweep = subsweep_idx;
                        overlap_found       = true;
                        break;
                }
        }
 
        return overlap_found;
}
 
static bool is_invalid_start_point(const acc_config_t *sensor_config)
{
        const int32_t minimum_allowed_start_point = 32;
 
        int32_t min_start_point = acc_config_subsweep_start_point_get(sensor_config, 0U);
 
        return min_start_point < minimum_allowed_start_point;
}
 
static bool is_outside_measuring_interval(const acc_config_t *sensor_config, float distance_m)
{
        float min_measured_point_m = 1000.0f;
        float max_measured_point_m = -1000.0f;
 
        uint8_t num_subsweeps = acc_config_num_subsweeps_get(sensor_config);
 
        for (uint8_t subsweep_idx = 0U; subsweep_idx < num_subsweeps; subsweep_idx++)
        {
                float current_begin_m = subsweep_approx_start_m(sensor_config, subsweep_idx);
                float current_end_m   = subsweep_approx_end_m(sensor_config, subsweep_idx);
 
                if (current_begin_m < min_measured_point_m)
                {
                        min_measured_point_m = current_begin_m;
                }
 
                if (current_end_m > max_measured_point_m)
                {
                        max_measured_point_m = current_end_m;
                }
        }
 
        return distance_m < min_measured_point_m || distance_m > max_measured_point_m;
}
 
static float subsweep_approx_start_m(const acc_config_t *sensor_config, uint8_t subsweep_idx)
{
        return acc_algorithm_get_distance_m(acc_config_subsweep_step_length_get(sensor_config, subsweep_idx),
                                            acc_config_subsweep_start_point_get(sensor_config, subsweep_idx),
                                            ACC_APPROX_BASE_STEP_LENGTH_M,
                                            0U);
}
 
static float subsweep_approx_end_m(const acc_config_t *sensor_config, uint8_t subsweep_idx)
{
        return acc_algorithm_get_distance_m(acc_config_subsweep_step_length_get(sensor_config, subsweep_idx),
                                            acc_config_subsweep_start_point_get(sensor_config, subsweep_idx),
                                            ACC_APPROX_BASE_STEP_LENGTH_M,
                                            acc_config_subsweep_num_points_get(sensor_config, subsweep_idx) - 1U);
}
 
static float point_in_sweep_to_distance_m(const acc_config_t *sensor_config, const acc_processing_metadata_t *metadata, uint16_t point_idx)
{
        uint8_t num_subsweeps = acc_config_num_subsweeps_get(sensor_config);
        float   distance_m    = 0.0f;
 
        for (uint8_t subsweep_idx = 0U; subsweep_idx < num_subsweeps; subsweep_idx++)
        {
                uint16_t start = metadata->subsweep_data_offset[subsweep_idx];
                uint16_t end   = start + metadata->subsweep_data_length[subsweep_idx];
 
                if (point_idx >= start && point_idx < end)
                {
                        distance_m = acc_algorithm_get_distance_m(acc_config_subsweep_step_length_get(sensor_config, subsweep_idx),
                                                                  acc_config_subsweep_start_point_get(sensor_config, subsweep_idx),
                                                                  acc_processing_points_to_meter(1),
                                                                  point_idx - start);
                        break;
                }
        }
 
        return distance_m;
}
 
static void write_to_ring_buffer(float *buffer, uint16_t capacity, uint16_t *length, uint16_t *write_idx, float elem)
{
        buffer[*write_idx] = elem;
 
        *write_idx = (*write_idx + 1U) % capacity;
 
        if (*length < capacity)
        {
                *length = *length + 1U;
        }
}
 
static uint16_t copy_non_nan_floats(const float *in, float *out, uint16_t length)
{
        uint16_t num_normal = 0U;
 
        for (uint16_t i = 0U; i < length; i++)
        {
                if (fpclassify(in[i]) != FP_NAN)
                {
                        out[num_normal] = in[i];
                        num_normal++;
                }
        }
 
        return num_normal;
}
 
static void set_level_numbers(float filtered_distance, const waste_level_processing_config_t *processing_config, waste_level_result_t *result)
{
        float bin_end_m   = processing_config->bin_end_m;
        float bin_start_m = processing_config->bin_start_m;
 
        float fill_level_m         = bin_end_m - filtered_distance;
        float fill_level_percent_f = acc_algorithm_clip_f32(100.0f * fill_level_m / (bin_end_m - bin_start_m), 0.0f, 100.0f);
 
        fill_level_percent_f       += 0.5f; // for rounding
        uint8_t fill_level_percent  = (uint8_t)fill_level_percent_f;
 
        result->level_m       = fill_level_m;
        result->level_percent = fill_level_percent;
}