206 lines
8.8 KiB
C
206 lines
8.8 KiB
C
/* USER CODE BEGIN Header */
|
|
/**
|
|
******************************************************************************
|
|
* @file yunhorn_sts_distance_rss.c *
|
|
* @author Yunhorn (r) Technology Limited Application Team *
|
|
* @brief Yunhorn (r) SmarToilets (r) Product configuration file. *
|
|
******************************************************************************
|
|
* @attention
|
|
*
|
|
* Copyright (c) 2022 Yunhorn Technology Limited.
|
|
* Copyright (c) 2022 Shenzhen Yunhorn Technology Co., Ltd.
|
|
* All rights reserved.
|
|
*
|
|
* This software is licensed under terms that can be found in the LICENSE file
|
|
* in the root directory of this software component.
|
|
* If no LICENSE file comes with this software, it is provided AS-IS.
|
|
*
|
|
******************************************************************************
|
|
*/
|
|
/* USER CODE END Header */
|
|
|
|
/* Includes ------------------------------------------------------------------*/
|
|
#include <stdbool.h>
|
|
#include <stdio.h>
|
|
|
|
/* USER CODE BEGIN Includes */
|
|
#include <stdlib.h>
|
|
#include "acc_detector_distance.h"
|
|
#include "acc_hal_definitions.h"
|
|
#include "acc_hal_integration.h"
|
|
#include "acc_rss.h"
|
|
#include "acc_version.h"
|
|
#include "sys_app.h"
|
|
#include "yunhorn_sts_prd_conf.h"
|
|
#include "yunhorn_sts_sensors.h"
|
|
// NEW ADD 2024-06-17
|
|
#define DEFAULT_FAR_RANGE_MUR ACC_SERVICE_MUR_6
|
|
#define DEFAULT_FAR_RANGE_SERVICE_PROFILE ACC_SERVICE_PROFILE_2
|
|
#define DEFAULT_FAR_MAXIMIZE_SIGNAL_ATTENUATION false
|
|
// ===================================
|
|
#define STS_DISTANCE_START_M (0.8f)
|
|
#define STS_DISTANCE_LENGTH_M (1.4f)
|
|
#define STS_DISTANCE_PROFILE ACC_SERVICE_PROFILE_2
|
|
#define STS_DISTANCE_HWAAS (63)
|
|
#define DEFAULT_FAR_RANGE_CFAR_THRESHOLD_GUARD 0.12f
|
|
#define DEFAULT_FAR_RANGE_CFAR_THRESHOLD_WINDOW 0.03f
|
|
#ifndef MIN
|
|
#define MIN( a, b ) ( ( ( a ) < ( b ) ) ? ( a ) : ( b ) )
|
|
#endif
|
|
|
|
#ifndef MAX
|
|
#define MAX( a, b ) ( ( ( a ) > ( b ) ) ? ( a ) : ( b ) )
|
|
#endif
|
|
|
|
//volatile distance_measure_cfg_t distance_cfg={1.5, 2.0, 1, 63, 2, 10, 0.5, 1.3, 0.2};
|
|
// SPARK FUN EXAMPLE
|
|
// GOOD --- volatile distance_measure_cfg_t distance_cfg={0.4, 3.5, 4, 63, 0, 10, 0.5, 1.3, 0.2};
|
|
volatile distance_measure_cfg_t distance_cfg={0.8, 3.5, 2, 63, 2, 10, 0.5, 1.3, 0.2};
|
|
//volatile distance_measure_cfg_t distance_cfg={1.5, 3.3, 2, 63, 4, 10, 0.8182f, 0.4, 0.2};
|
|
extern volatile uint16_t sts_distance_rss_distance, sts_sensor_install_height;
|
|
|
|
static void sts_distance_rss_update_configuration(acc_detector_distance_configuration_t distance_configuration);
|
|
|
|
|
|
static void print_distances(acc_detector_distance_result_t *result, uint16_t reflection_count);
|
|
|
|
|
|
int sts_distance_rss_detector_distance(void);
|
|
|
|
int sts_distance_rss_detector_distance(void)
|
|
{
|
|
const acc_hal_t *hal = acc_hal_integration_get_implementation();
|
|
|
|
if (!acc_rss_activate(hal))
|
|
{
|
|
APP_LOG(TS_OFF, VLEVEL_L, "Failed to activate RSS\n");
|
|
return EXIT_FAILURE;
|
|
}
|
|
|
|
//acc_rss_override_sensor_id_check_at_creation(true);
|
|
|
|
acc_detector_distance_configuration_t distance_configuration = acc_detector_distance_configuration_create();
|
|
|
|
if (distance_configuration == NULL)
|
|
{
|
|
APP_LOG(TS_OFF, VLEVEL_L, "acc_detector_distance_configuration_create() failed\n");
|
|
acc_rss_deactivate();
|
|
return EXIT_FAILURE;
|
|
}
|
|
|
|
sts_distance_rss_update_configuration(distance_configuration);
|
|
|
|
acc_detector_distance_handle_t distance_handle = acc_detector_distance_create(distance_configuration);
|
|
|
|
if (distance_handle == NULL)
|
|
{
|
|
APP_LOG(TS_OFF, VLEVEL_L, "acc_detector_distance_create() failed\n");
|
|
acc_detector_distance_configuration_destroy(&distance_configuration);
|
|
acc_rss_deactivate();
|
|
return EXIT_FAILURE;
|
|
}
|
|
|
|
acc_detector_distance_configuration_destroy(&distance_configuration);
|
|
|
|
if (!acc_detector_distance_activate(distance_handle))
|
|
{
|
|
APP_LOG(TS_OFF, VLEVEL_L, "acc_detector_distance_activate() failed\n");
|
|
acc_detector_distance_destroy(&distance_handle);
|
|
acc_rss_deactivate();
|
|
return EXIT_FAILURE;
|
|
}
|
|
|
|
bool success = true;
|
|
const int iterations = 2; //5;
|
|
uint16_t number_of_peaks = 5;
|
|
acc_detector_distance_result_t result[number_of_peaks];
|
|
acc_detector_distance_result_info_t result_info;
|
|
float tmp_distance = 0.0f;
|
|
|
|
for (int i = 0; i < iterations; i++)
|
|
{
|
|
success = acc_detector_distance_get_next(distance_handle, result, number_of_peaks, &result_info);
|
|
|
|
if (!success)
|
|
{
|
|
APP_LOG(TS_OFF, VLEVEL_L, "acc_detector_distance_get_next() failed\n");
|
|
break;
|
|
}
|
|
for(uint8_t j=0; j< result_info.number_of_peaks; j++)
|
|
tmp_distance += result[j].distance_m;
|
|
print_distances(result, result_info.number_of_peaks);
|
|
}
|
|
|
|
sts_distance_rss_distance = (uint16_t)(1000*(tmp_distance/result_info.number_of_peaks)/iterations);
|
|
|
|
APP_LOG(TS_OFF, VLEVEL_M, "\r\nAverage Distance= %u mm\r\n", (uint16_t)sts_distance_rss_distance);
|
|
// ensure it's a valid installation height
|
|
//sts_sensor_install_height = (uint16_t)MAX(sts_distance_rss_distance,2000);
|
|
sts_sensor_install_height = (uint16_t)sts_distance_rss_distance;
|
|
APP_LOG(TS_OFF, VLEVEL_M, "\r\nAssume Sensor Install Height = %u mm\r\n", (uint16_t)sts_sensor_install_height);
|
|
|
|
bool deactivated = acc_detector_distance_deactivate(distance_handle);
|
|
|
|
acc_detector_distance_destroy(&distance_handle);
|
|
|
|
acc_rss_deactivate();
|
|
|
|
if (deactivated && success)
|
|
{
|
|
APP_LOG(TS_OFF, VLEVEL_L, "Application finished OK\n");
|
|
return EXIT_SUCCESS;
|
|
}
|
|
|
|
return EXIT_FAILURE;
|
|
}
|
|
|
|
static void sts_distance_rss_update_configuration(acc_detector_distance_configuration_t distance_configuration)
|
|
{
|
|
acc_detector_distance_configuration_mur_set(distance_configuration, DEFAULT_FAR_RANGE_MUR); // NEW ADD
|
|
acc_detector_distance_configuration_requested_start_set(distance_configuration, distance_cfg.start_m);
|
|
acc_detector_distance_configuration_requested_length_set(distance_configuration, distance_cfg.length_m);
|
|
acc_detector_distance_configuration_receiver_gain_set(distance_configuration,distance_cfg.gain);
|
|
acc_detector_distance_configuration_maximize_signal_attenuation_set(distance_configuration, DEFAULT_FAR_MAXIMIZE_SIGNAL_ATTENUATION); // NEW ADD
|
|
acc_detector_distance_configuration_service_profile_set(distance_configuration, distance_cfg.acc_profile);
|
|
acc_detector_distance_configuration_downsampling_factor_set(distance_configuration, distance_cfg.downsampling);
|
|
acc_detector_distance_configuration_sweep_averaging_set(distance_configuration,distance_cfg.sweep_average);
|
|
acc_detector_distance_configuration_threshold_type_set(distance_configuration, ACC_DETECTOR_DISTANCE_THRESHOLD_TYPE_CFAR);
|
|
acc_detector_distance_configuration_threshold_sensitivity_set(distance_configuration, distance_cfg.threshold);
|
|
//acc_detector_distance_configuration_hw_accelerated_average_samples_set(distance_configuration, distance_cfg.hwaas);
|
|
//acc_detector_distance_configuration_threshold_type_set(distance_configuration, DEFAULT_FAR_RANGE_THRESHOLD_TYPE); //NEW ADD
|
|
acc_detector_distance_configuration_cfar_threshold_guard_set(distance_configuration, DEFAULT_FAR_RANGE_CFAR_THRESHOLD_GUARD);
|
|
acc_detector_distance_configuration_cfar_threshold_window_set(distance_configuration,DEFAULT_FAR_RANGE_CFAR_THRESHOLD_WINDOW);
|
|
acc_detector_distance_configuration_record_background_sweeps_set(distance_configuration, 16);
|
|
}
|
|
|
|
//backup. ... previous setting ----don't delete
|
|
/*
|
|
static void sts_distance_rss_update_configuration(acc_detector_distance_configuration_t distance_configuration)
|
|
{
|
|
acc_detector_distance_configuration_requested_start_set(distance_configuration, distance_cfg.start_m);
|
|
acc_detector_distance_configuration_requested_length_set(distance_configuration, distance_cfg.length_m);
|
|
acc_detector_distance_configuration_service_profile_set(distance_configuration, distance_cfg.acc_profile);
|
|
acc_detector_distance_configuration_hw_accelerated_average_samples_set(distance_configuration, distance_cfg.hwaas);
|
|
|
|
acc_detector_distance_configuration_threshold_sensitivity_set(distance_configuration, distance_cfg.threshold);
|
|
acc_detector_distance_configuration_sweep_averaging_set(distance_configuration,distance_cfg.sweep_average);
|
|
acc_detector_distance_configuration_receiver_gain_set(distance_configuration,distance_cfg.gain);
|
|
acc_detector_distance_configuration_downsampling_factor_set(distance_configuration, distance_cfg.downsampling);
|
|
|
|
acc_detector_distance_configuration_threshold_type_set(distance_configuration, ACC_DETECTOR_DISTANCE_THRESHOLD_TYPE_CFAR);
|
|
acc_detector_distance_configuration_record_background_sweeps_set(distance_configuration, 16);
|
|
|
|
}
|
|
*/
|
|
//
|
|
static void print_distances(acc_detector_distance_result_t *result, uint16_t reflection_count)
|
|
{
|
|
APP_LOG(TS_OFF, VLEVEL_M, "Found %u peaks:\n", (unsigned int)reflection_count);
|
|
|
|
for (uint16_t i = 0; i < reflection_count; i++)
|
|
{
|
|
APP_LOG(TS_OFF, VLEVEL_M, "Amplitude %u at %u mm\n", (unsigned int)result[i].amplitude,
|
|
(unsigned int)(result[i].distance_m * 1000));
|
|
}
|
|
}
|