254 lines
10 KiB
C
254 lines
10 KiB
C
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
// Copyright (c) Acconeer AB, 2019-2021
|
||
|
|
// 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 <stdio.h>
|
||
|
|
|
||
|
|
#include "acc_definitions_common.h"
|
||
|
|
#include "acc_detector_presence.h"
|
||
|
|
#include "acc_hal_definitions.h"
|
||
|
|
#include "acc_hal_integration.h"
|
||
|
|
#include "acc_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"
|
||
|
|
/*
|
||
|
|
#define DEFAULT_START_M (0.2f)
|
||
|
|
#define DEFAULT_LENGTH_M (1.4f)
|
||
|
|
#define DEFAULT_POWER_SAVE_MODE ACC_POWER_SAVE_MODE_SLEEP
|
||
|
|
*/
|
||
|
|
#define DEFAULT_UPDATE_RATE (10)
|
||
|
|
#define DEFAULT_DETECTION_THRESHOLD (2.0f)
|
||
|
|
#define DEFAULT_NBR_REMOVED_PC_2 (0)
|
||
|
|
|
||
|
|
#define DEFAULT_PROFILE ACC_SERVICE_PROFILE_4
|
||
|
|
#define DEFAULT_UPDATE_RATE (10)
|
||
|
|
#define DEFAULT_POWER_SAVE_MODE ACC_POWER_SAVE_MODE_ACTIVE
|
||
|
|
#define DEFAULT_SENSOR_ID (1)
|
||
|
|
|
||
|
|
#define DEFAULT_START_M (0.80f) //default 0.2 unit(meter) [1]
|
||
|
|
#define DEFAULT_LENGTH_M (2.00f) //default 1.0 unit(meter) [2]
|
||
|
|
#define DEFAULT_ZONE_LENGTH (0.4f) //default 0.4 unit(meter)
|
||
|
|
#define DEFAULT_UPDATE_RATE_WAKEUP (2.0f) //default 80 unit(hz)
|
||
|
|
#define DEFAULT_UPDATE_RATE_TRACKING (10.0f) //default 80 unit(hz) [7]
|
||
|
|
#define DEFAULT_UPDATE_RATE_PRESENCE (65.0F) //(65.0f) //default 80 unit(hz)
|
||
|
|
#define DEFAULT_HWAAS (63) //default 10 unit(hz)
|
||
|
|
#define DEFAULT_THRESHOLD (1.5f) //default 1.5 level float [3]
|
||
|
|
|
||
|
|
//acc_detector_presence_configuration_filter_parameters_t
|
||
|
|
#define DEFAULT_INTER_FRAME_DEVIATION_TIME_CONST (0.5f) //default 0.5 unit(seconds) [6]
|
||
|
|
#define DEFAULT_INTER_FRAME_FAST_CUTOFF (10.0f) //default 20.0 unit(hz) [8]
|
||
|
|
#define DEFAULT_INTER_FRAME_SLOW_CUTOFF (0.01f) //(0.01f) 0.2 hz unit(hz) [9]
|
||
|
|
#define DEFAULT_INTRA_FRAME_TIME_CONST (0) //default 0.0 unit(seconds)
|
||
|
|
#define DEFAULT_INTRA_FRAME_WEIGHT (0) //default 0.6
|
||
|
|
#define DEFAULT_OUTPUT_TIME_CONST (0.5f) //default 0.5 unit(seconds) [5]
|
||
|
|
#define DEFAULT_NBR_REMOVED_PC (0) //default 0 int [10]
|
||
|
|
|
||
|
|
#define DEFAULT_DOWNSAMPLING_FACTOR (2) //default 1
|
||
|
|
#define DEFAULT_RECEIVER_GAIN (0.45f) //default 0.9 gain mdB [4]
|
||
|
|
|
||
|
|
extern volatile uint8_t sts_rss_result, sts_rss_config_updated_flag;
|
||
|
|
volatile STS_OO_RSS_SensorTuneDataTypeDef sts_presence_rss_config;
|
||
|
|
//static void update_configuration(acc_detector_presence_configuration_t presence_configuration);
|
||
|
|
static void print_result(acc_detector_presence_result_t result);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Set default values in presence configuration
|
||
|
|
*
|
||
|
|
* @param[in] presence_configuration The presence configuration to set default values in
|
||
|
|
*/
|
||
|
|
static void set_default_configuration(acc_detector_presence_configuration_t presence_configuration)
|
||
|
|
{
|
||
|
|
acc_detector_presence_configuration_sensor_set(presence_configuration, DEFAULT_SENSOR_ID);
|
||
|
|
acc_detector_presence_configuration_update_rate_set(presence_configuration, DEFAULT_UPDATE_RATE_PRESENCE);
|
||
|
|
acc_detector_presence_configuration_detection_threshold_set(presence_configuration, DEFAULT_THRESHOLD);
|
||
|
|
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
||
|
|
//acc_service_sparse_configuration_sweeps_per_frame_set(sparse_configuration, sweeps_per_frame);
|
||
|
|
|
||
|
|
acc_detector_presence_configuration_start_set(presence_configuration, DEFAULT_START_M);
|
||
|
|
acc_detector_presence_configuration_length_set(presence_configuration, DEFAULT_LENGTH_M);
|
||
|
|
acc_detector_presence_configuration_downsampling_factor_set(presence_configuration, DEFAULT_DOWNSAMPLING_FACTOR);
|
||
|
|
acc_detector_presence_configuration_receiver_gain_set(presence_configuration, DEFAULT_RECEIVER_GAIN);
|
||
|
|
|
||
|
|
acc_detector_presence_configuration_filter_parameters_t filter = acc_detector_presence_configuration_filter_parameters_get(
|
||
|
|
presence_configuration);
|
||
|
|
filter.inter_frame_deviation_time_const = DEFAULT_INTER_FRAME_DEVIATION_TIME_CONST;
|
||
|
|
filter.inter_frame_fast_cutoff = DEFAULT_INTER_FRAME_FAST_CUTOFF;
|
||
|
|
filter.inter_frame_slow_cutoff = DEFAULT_INTER_FRAME_SLOW_CUTOFF;
|
||
|
|
filter.intra_frame_time_const = DEFAULT_INTRA_FRAME_TIME_CONST;
|
||
|
|
filter.intra_frame_weight = DEFAULT_INTRA_FRAME_WEIGHT;
|
||
|
|
filter.output_time_const = DEFAULT_OUTPUT_TIME_CONST; //0.0f;
|
||
|
|
acc_detector_presence_configuration_filter_parameters_set(presence_configuration, &filter);
|
||
|
|
acc_detector_presence_configuration_nbr_removed_pc_set(presence_configuration, DEFAULT_NBR_REMOVED_PC);
|
||
|
|
acc_detector_presence_configuration_power_save_mode_set(presence_configuration, ACC_POWER_SAVE_MODE_ACTIVE);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
static void sts_rss_set_current_configuration(acc_detector_presence_configuration_t presence_configuration)
|
||
|
|
{
|
||
|
|
acc_detector_presence_configuration_sensor_set(presence_configuration, DEFAULT_SENSOR_ID);
|
||
|
|
|
||
|
|
// acc_service_profile_t presence_profile = sts_presence_rss_config.default_profile;
|
||
|
|
// acc_detector_presence_configuration_service_profile_set(presence_configuration, presence_profile);
|
||
|
|
|
||
|
|
acc_detector_presence_configuration_update_rate_set(presence_configuration, sts_presence_rss_config.default_update_rate_presence); //DEFAULT_UPDATE_RATE_2);
|
||
|
|
acc_detector_presence_configuration_detection_threshold_set(presence_configuration, sts_presence_rss_config.default_threshold);//DEFAULT_DETECTION_THRESHOLD_2);
|
||
|
|
|
||
|
|
acc_detector_presence_configuration_start_set(presence_configuration, sts_presence_rss_config.default_start_m);
|
||
|
|
acc_detector_presence_configuration_length_set(presence_configuration, sts_presence_rss_config.default_length_m); //DEFAULT_LENGTH_M_2);
|
||
|
|
|
||
|
|
acc_detector_presence_configuration_downsampling_factor_set(presence_configuration, sts_presence_rss_config.default_downsampling_factor);
|
||
|
|
acc_detector_presence_configuration_receiver_gain_set(presence_configuration, sts_presence_rss_config.default_receiver_gain);
|
||
|
|
|
||
|
|
acc_detector_presence_configuration_filter_parameters_t filter = acc_detector_presence_configuration_filter_parameters_get(
|
||
|
|
presence_configuration);
|
||
|
|
filter.inter_frame_deviation_time_const = sts_presence_rss_config.default_inter_frame_deviation_time_const;
|
||
|
|
filter.inter_frame_fast_cutoff = sts_presence_rss_config.default_inter_frame_fast_cutoff;
|
||
|
|
filter.inter_frame_slow_cutoff = sts_presence_rss_config.default_inter_frame_slow_cutoff;
|
||
|
|
filter.intra_frame_time_const = sts_presence_rss_config.default_intra_frame_time_const;
|
||
|
|
filter.intra_frame_weight = sts_presence_rss_config.default_intra_frame_weight;
|
||
|
|
filter.output_time_const = sts_presence_rss_config.default_output_time_const; //0.0f;
|
||
|
|
acc_detector_presence_configuration_filter_parameters_set(presence_configuration, &filter);
|
||
|
|
|
||
|
|
acc_detector_presence_configuration_nbr_removed_pc_set(presence_configuration, sts_presence_rss_config.default_nbr_removed_pc);
|
||
|
|
acc_detector_presence_configuration_hw_accelerated_average_samples_set(presence_configuration, sts_presence_rss_config.default_hwaas);
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void sts_rss_get_current_configuration(acc_detector_presence_configuration_t presence_configuration)
|
||
|
|
{
|
||
|
|
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
int sts_presence_rss_presence_detection(void)
|
||
|
|
{
|
||
|
|
const acc_hal_t *hal = acc_hal_integration_get_implementation();
|
||
|
|
|
||
|
|
if (!acc_rss_activate(hal))
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_M,"Failed to activate RSS\n");
|
||
|
|
return EXIT_FAILURE;
|
||
|
|
}
|
||
|
|
|
||
|
|
acc_rss_override_sensor_id_check_at_creation(true);
|
||
|
|
|
||
|
|
acc_detector_presence_configuration_t presence_configuration = acc_detector_presence_configuration_create();
|
||
|
|
if (presence_configuration == NULL)
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_M,"Failed to create configuration\n");
|
||
|
|
acc_rss_deactivate();
|
||
|
|
return EXIT_FAILURE;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (sts_rss_config_updated_flag == 0) {
|
||
|
|
set_default_configuration(presence_configuration);
|
||
|
|
{
|
||
|
|
sts_rss_set_current_configuration(presence_configuration);
|
||
|
|
}
|
||
|
|
|
||
|
|
acc_detector_presence_handle_t handle = acc_detector_presence_create(presence_configuration);
|
||
|
|
if (handle == NULL)
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_M,"Failed to create detector\n");
|
||
|
|
acc_detector_presence_configuration_destroy(&presence_configuration);
|
||
|
|
acc_detector_presence_destroy(&handle);
|
||
|
|
acc_rss_deactivate();
|
||
|
|
return EXIT_FAILURE;
|
||
|
|
}
|
||
|
|
|
||
|
|
acc_detector_presence_configuration_destroy(&presence_configuration);
|
||
|
|
|
||
|
|
if (!acc_detector_presence_activate(handle))
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_M,"Failed to activate detector\n");
|
||
|
|
acc_detector_presence_destroy(&handle);
|
||
|
|
acc_rss_deactivate();
|
||
|
|
return EXIT_FAILURE;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool success = true;
|
||
|
|
const int iterations = 10;
|
||
|
|
acc_detector_presence_result_t result;
|
||
|
|
uint8_t average_result = 0;
|
||
|
|
for (int i = 0; i < iterations; i++)
|
||
|
|
{
|
||
|
|
success = acc_detector_presence_get_next(handle, &result);
|
||
|
|
if (!success)
|
||
|
|
{
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_M,"acc_detector_presence_get_next() failed\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
print_result(result);
|
||
|
|
|
||
|
|
if (result.presence_detected) average_result++;
|
||
|
|
acc_integration_sleep_ms(1000 / DEFAULT_UPDATE_RATE);
|
||
|
|
}
|
||
|
|
|
||
|
|
sts_rss_result = (average_result > (iterations/2))? 1: 0;
|
||
|
|
|
||
|
|
bool deactivated = acc_detector_presence_deactivate(handle);
|
||
|
|
|
||
|
|
acc_detector_presence_destroy(&handle);
|
||
|
|
|
||
|
|
acc_rss_deactivate();
|
||
|
|
|
||
|
|
if (deactivated && success)
|
||
|
|
{
|
||
|
|
//APP_LOG(TS_OFF, VLEVEL_M,"Application finished OK\n");
|
||
|
|
return EXIT_SUCCESS;
|
||
|
|
}
|
||
|
|
|
||
|
|
return EXIT_FAILURE;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
void STS_PRESENCE_RSS_update_configuration(acc_detector_presence_configuration_t presence_configuration)
|
||
|
|
{
|
||
|
|
acc_detector_presence_configuration_update_rate_set(presence_configuration, DEFAULT_UPDATE_RATE);
|
||
|
|
acc_detector_presence_configuration_detection_threshold_set(presence_configuration, DEFAULT_DETECTION_THRESHOLD);
|
||
|
|
acc_detector_presence_configuration_start_set(presence_configuration, DEFAULT_START_M);
|
||
|
|
acc_detector_presence_configuration_length_set(presence_configuration, DEFAULT_LENGTH_M);
|
||
|
|
acc_detector_presence_configuration_power_save_mode_set(presence_configuration, DEFAULT_POWER_SAVE_MODE);
|
||
|
|
acc_detector_presence_configuration_nbr_removed_pc_set(presence_configuration, DEFAULT_NBR_REMOVED_PC);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void print_result(acc_detector_presence_result_t result)
|
||
|
|
{
|
||
|
|
if (result.presence_detected)
|
||
|
|
{
|
||
|
|
uint32_t detected_zone = (uint32_t)((float)(result.presence_distance - DEFAULT_START_M) / (float)DEFAULT_ZONE_LENGTH);
|
||
|
|
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_M,"Motion in zone: %u, distance: %d, score: %d\n", (unsigned int)detected_zone,
|
||
|
|
(int)(result.presence_distance * 1000.0f),
|
||
|
|
(int)(result.presence_score * 1000.0f));
|
||
|
|
|
||
|
|
sts_rss_result = 1;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
|
||
|
|
APP_LOG(TS_OFF, VLEVEL_M,"No motion, score: %d\n", (int)(result.presence_score * 1000.0f));
|
||
|
|
|
||
|
|
sts_rss_result = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
}
|