wip good progress, stable motion level when no one below

This commit is contained in:
Yunhorn 2025-04-29 19:29:49 +08:00
parent b9246801b2
commit 23ed469d40
7 changed files with 206 additions and 40 deletions

View File

@ -742,6 +742,11 @@ void STS_SENSOR_Upload_Message(uint8_t appDataPort, uint8_t appBufferSize, uint8
void STS_SENSOR_Auto_Responder_Process(uint8_t tlv_ver,uint8_t tlv_type, uint8_t tlv_length, uint8_t *tlv_content);
uint8_t STS_SENSOR_MEMS_Get_ID(uint8_t *devID);
//void update_configuration(acc_detector_presence_configuration_t presence_configuration);
int sts_presence_rss_detection_init(void);
int sts_presence_rss_detection_process(void);
int sts_presence_rss_detection_deinit(void);
int sts_presence_rss_presence_detection(void);
int sts_presence_rss_fall_rise_detection(void);

View File

@ -98,8 +98,21 @@ int main(void)
/* USER CODE BEGIN 2 */
STS_Sensor_Init();
/* USER CODE END 2 */
STS_Sensor_Prepare();
/* USER CODE END 2 */
#if 0
sts_presence_rss_detection_init();
do
{
sts_presence_rss_detection_process();
} while(1);
sts_presence_rss_detection_deinit();
STS_Sensor_Prepare();
#endif
/* Infinite loop */
/* USER CODE BEGIN WHILE */

View File

@ -118,11 +118,11 @@ void SystemApp_Init(void)
/*Initialize the Sensors */
EnvSensors_Init();
// LED1 Flash 3 times for normal power on
LED1_TOGGLE; HAL_Delay(500);
LED1_TOGGLE; HAL_Delay(500);
LED1_TOGGLE; HAL_Delay(500);
LED1_TOGGLE; HAL_Delay(500);
LED1_TOGGLE; HAL_Delay(500);
LED1_TOGGLE; HAL_Delay(200);
LED1_TOGGLE; HAL_Delay(200);
LED1_TOGGLE; HAL_Delay(200);
LED1_TOGGLE; HAL_Delay(200);
LED1_TOGGLE; HAL_Delay(200);
LED1_TOGGLE;
/*Init low power manager*/
UTIL_LPM_Init();

View File

@ -151,6 +151,9 @@ extern volatile uint8_t sts_presence_fall_detection;
static uint8_t sts_rss_init_ok=0;
volatile uint8_t sts_presence_singularity=0;
extern uint8_t sts_lamp_bar_color;
acc_detector_presence_handle_t rss_handle=NULL;
volatile acc_detector_presence_result_t rss_result;
volatile float sts_rss_threshold=1.6f;
/* USER CODE END Includes */
/* External variables ---------------------------------------------------------*/
@ -196,6 +199,61 @@ extern uint8_t sts_lamp_bar_color;
* @param[in] presence_configuration The presence configuration to set default values in
*/
uint16_t start_t = 500;
uint16_t end_t = 2000;
uint16_t update_t = 200;
uint8_t gain_t = 50;
uint16_t threshold = 1500;
uint16_t frame_fast_t = 200;
uint16_t frame_slow_t = 20;
uint8_t profile = 4;
uint16_t Sweeps_per_frame = 16;
uint8_t downsampling = 1;
uint8_t HWAAS = 63;
float start_m = 0.3f;
float length_m = 0.9f;
float threshold_m = 1.5f;
float gain_m = 0.7f;
float update_m = 0.0f;
float frame_fast = 20.0f;
float frame_slow = 0.1f;
void update_configuration(acc_detector_presence_configuration_t presence_configuration)
{
float start_m_t = 0.0f;
start_m = (float)start_t / 1000;
length_m = (float)(end_t - start_t) / 1000 + 0.0f;
threshold_m = (float)threshold / 1000;
gain_m = (float)gain_t / 100;
update_m = 1000.0f / (float)update_t;
frame_fast = (float)frame_fast_t / 10;
frame_slow = (float)frame_slow_t / 100;
acc_detector_presence_configuration_service_profile_set(presence_configuration, profile);
Sweeps_per_frame = acc_detector_presence_configuration_sweeps_per_frame_get(presence_configuration);
acc_detector_presence_configuration_sweeps_per_frame_set(presence_configuration, Sweeps_per_frame);
acc_detector_presence_configuration_downsampling_factor_set(presence_configuration, downsampling);
acc_detector_presence_configuration_hw_accelerated_average_samples_set(presence_configuration, HWAAS);
acc_detector_presence_configuration_update_rate_set(presence_configuration, update_m);
acc_detector_presence_configuration_detection_threshold_set(presence_configuration, threshold_m);
acc_detector_presence_configuration_start_set(presence_configuration, start_m);
acc_detector_presence_configuration_length_set(presence_configuration, length_m);
acc_detector_presence_configuration_power_save_mode_set(presence_configuration, DEFAULT_POWER_SAVE_MODE);
acc_detector_presence_configuration_receiver_gain_set(presence_configuration, gain_m);
acc_detector_presence_configuration_filter_parameters_t filter;
filter = acc_detector_presence_configuration_filter_parameters_get(presence_configuration);
filter.inter_frame_fast_cutoff = frame_fast;
filter.inter_frame_slow_cutoff = frame_slow;
acc_detector_presence_configuration_filter_parameters_set(presence_configuration, &filter);
}
static void set_default_configuration_common(acc_detector_presence_configuration_t presence_configuration)
{
acc_detector_presence_configuration_update_rate_set(presence_configuration, sts_presence_rss_config.default_update_rate_presence);
@ -226,37 +284,40 @@ static void set_default_configuration(acc_detector_presence_configuration_t pres
{
acc_detector_presence_configuration_sensor_set(presence_configuration, DEFAULT_SENSOR_ID);
acc_detector_presence_configuration_service_profile_set(presence_configuration, DEFAULT_PROFILE); // *** optional
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_detector_presence_configuration_update_rate_set(presence_configuration, 10); //DEFAULT_UPDATE_RATE_PRESENCE);
acc_detector_presence_configuration_detection_threshold_set(presence_configuration, 1.6f); //DEFAULT_THRESHOLD);
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//acc_service_sparse_configuration_sweeps_per_frame_set(sparse_configuration, sweeps_per_frame);
int sweeps_per_frame = acc_service_sparse_configuration_sweeps_per_frame_get(presence_configuration);
acc_service_sparse_configuration_sweeps_per_frame_set(presence_configuration, 16);
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_hw_accelerated_average_samples_set(presence_configuration, DEFAULT_HWAAS);
acc_detector_presence_configuration_start_set(presence_configuration, 0.5f);//DEFAULT_START_M);
acc_detector_presence_configuration_length_set(presence_configuration, 1.7f); //DEFAULT_LENGTH_M);
acc_detector_presence_configuration_downsampling_factor_set(presence_configuration, 1);
acc_detector_presence_configuration_receiver_gain_set(presence_configuration, 0.5f);
set_default_configuration_common(presence_configuration);
//set_default_configuration_common(presence_configuration);
// by set_default_configuration_common
#if 0
#if 1
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_deviation_time_const = DEFAULT_INTER_FRAME_DEVIATION_TIME_CONST;
// will be disabled if this value > 1/2 of update rate, default update rate 65, so must < 30
filter.inter_frame_fast_cutoff = DEFAULT_INTER_FRAME_FAST_CUTOFF;
filter.inter_frame_slow_cutoff = DEFAULT_INTER_FRAME_SLOW_CUTOFF;
filter.inter_frame_fast_cutoff = 18.0f; //DEFAULT_INTER_FRAME_FAST_CUTOFF;
filter.inter_frame_slow_cutoff = 0.17f; //DEFAULT_INTER_FRAME_SLOW_CUTOFF;
filter.output_time_const = 0.f;
// no effect if intra-frame-weight set to 0
filter.intra_frame_time_const = DEFAULT_INTRA_FRAME_TIME_CONST;
//filter.intra_frame_time_const = DEFAULT_INTRA_FRAME_TIME_CONST;
// for slow movement, people sit still, rest in sofa, seat, closestool, etc.
// set the intra_frame_weight to 0.0
filter.intra_frame_weight = DEFAULT_INTRA_FRAME_WEIGHT;
//filter.intra_frame_weight = DEFAULT_INTRA_FRAME_WEIGHT;
// if detection toggles too often, increase the following, if too sluggish, decrease it instead
filter.output_time_const = DEFAULT_OUTPUT_TIME_CONST; //0.0f;
//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_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);
#endif
// by set_default_configuration_common
@ -360,7 +421,7 @@ static void sts_rss_set_configuration_background_evalution(acc_detector_presence
static void sts_rss_set_current_configuration_simple(acc_detector_presence_configuration_t presence_configuration)
{
APP_LOG(TS_OFF, VLEVEL_H, "\r\nsts_rss_cfg-start: %4d ,length: %4d ,threshold: %4d ,gain: %2d ,rate: %2d ,profile: %1d \r\n",
APP_LOG(TS_OFF, VLEVEL_M, "\r\nsts_rss_cfg-start: %4d ,length: %4d ,threshold: %4d ,gain: %2d ,rate: %2d ,profile: %1d \r\n",
(int)(sts_presence_rss_config.default_start_m*1000),
(int)(sts_presence_rss_config.default_length_m*1000),
(int)(sts_presence_rss_config.default_threshold*1000),
@ -382,14 +443,14 @@ static void sts_rss_set_current_configuration_simple(acc_detector_presence_confi
static void print_current_configuration(acc_detector_presence_configuration_t presence_configuration)
{
static uint32_t cnt_0=0;
if (cnt_0++%200 !=0) return;
//if (cnt_0++%200 !=0) return;
float sts_run_start = acc_detector_presence_configuration_start_get(presence_configuration);
float sts_run_length = acc_detector_presence_configuration_length_get(presence_configuration);
float sts_run_threshold = acc_detector_presence_configuration_detection_threshold_get(presence_configuration);
float sts_run_gain = acc_detector_presence_configuration_receiver_gain_get(presence_configuration);
float sts_run_update_rate = acc_detector_presence_configuration_update_rate_get(presence_configuration);
float sts_run_profile = acc_detector_presence_configuration_service_profile_get(presence_configuration);
sts_rss_threshold = sts_run_threshold;
acc_detector_presence_configuration_filter_parameters_t sts_run_filter = acc_detector_presence_configuration_filter_parameters_get(presence_configuration);
float sts_run_f_inter_fast_cutoff = sts_run_filter.inter_frame_fast_cutoff;
@ -404,7 +465,7 @@ static void print_current_configuration(acc_detector_presence_configuration_t pr
sts_work_mode, (int)(1000.0*sts_run_start), (int)(1000.0*sts_run_length), (int)(1000.0*sts_run_threshold),
(int)(100.0*sts_run_gain),(int)sts_run_update_rate, (int)sts_run_profile);
APP_LOG(TS_OFF, VLEVEL_H, "\rn\n(1)FastCut:%4u (2)SlowCut:%4u (3)InterFrameDevTime:%4u "
APP_LOG(TS_OFF, VLEVEL_M, "\rn\n(1)FastCut:%4u (2)SlowCut:%4u (3)InterFrameDevTime:%4u "
"(4)IntraFrameTimeConst:%4d (5)IntraWeight:%4u (5)OutputTime:%4u \r\n",
(int)(1000.0*sts_run_f_inter_fast_cutoff), (int)(1000*sts_run_f_inter_slow_cutoff), (int)(1000*sts_run_f_inter_frame_dev_time_const),
(int)(1000*sts_run_f_intra_frame_time_const),(int)(1000*sts_run_f_intra_frame_weight),(int)(1000*sts_run_f_output_time_const));
@ -412,8 +473,8 @@ static void print_current_configuration(acc_detector_presence_configuration_t pr
}
uint8_t yes_count=0;
uint8_t no_count=0;
static uint8_t yes_count=0;
static uint8_t no_count=0;
#define TIME_C 4
static void sts_print_result(acc_detector_presence_result_t result)
@ -421,7 +482,7 @@ static void sts_print_result(acc_detector_presence_result_t result)
uint16_t signal=0;
uint16_t dist=0;
uint8_t i=0;
uint16_t threshold = sts_presence_rss_config.default_threshold*1000.0f;
uint16_t threshold = sts_rss_threshold*1000.0f;
signal=(int)(result.presence_score * 1000.0f);
dist =(int)(result.presence_distance * 1000.0f);
@ -490,13 +551,15 @@ static void sts_print_result(acc_detector_presence_result_t result)
{
//Out1_ON
//Out2_OFF
APP_LOG(TS_OFF, VLEVEL_M,"Motion (%5d), Distance: %4dmm\r\n", signal,dist);
sts_rss_result=1;
APP_LOG(TS_OFF, VLEVEL_M,"Motion (%5d), Distance: %4dmm Threshold:%6d Yes:%d No:%d \r\n", signal,dist,threshold, yes_count, no_count);
}
else if(no_count>(TIME_C-1))
{
//Out1_OFF
//Out2_ON
APP_LOG(TS_OFF, VLEVEL_M,"NO motion(%5d), Distance: %4dmm\r\n", signal,dist);
sts_rss_result=0;
APP_LOG(TS_OFF, VLEVEL_M,"NO motion(%5d), Distance: %4dmm Threshold:%6d Yes:%d No:%d \r\n", signal,dist,threshold, yes_count, no_count);
}
}
@ -624,6 +687,84 @@ int sts_presence_rss_background_evaluation_process(uint16_t *evaluated_distance,
return EXIT_SUCCESS;
}
int sts_presence_rss_detection_init(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;
}
//sts_rss_set_current_configuration_simple(presence_configuration);
update_configuration(presence_configuration);
//set_default_configuration(presence_configuration);
print_current_configuration(presence_configuration);
rss_handle = acc_detector_presence_create(presence_configuration);
if (rss_handle == NULL)
{
APP_LOG(TS_OFF, VLEVEL_M,"Failed to create detector\n");
acc_detector_presence_configuration_destroy(&presence_configuration);
acc_rss_deactivate();
return EXIT_FAILURE;
}
APP_LOG(TS_OFF, VLEVEL_M,"\r\n============= Start Scan\n");
//print_current_configuration(presence_configuration);
acc_detector_presence_configuration_destroy(&presence_configuration);
// BEFORE MERGE FIRST AND SECOND HALF FALL RISE DETECTION
if (!acc_detector_presence_activate(rss_handle))
{
APP_LOG(TS_OFF, VLEVEL_M, "Failed to activate detector \n");
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
int sts_presence_rss_detection_process(void)
{
bool success = true;
success = acc_detector_presence_get_next(rss_handle, &rss_result);
if (!success)
{
APP_LOG(TS_OFF, VLEVEL_M,"acc_detector_presence_get_next() failed\n");
}
sts_print_result(rss_result);
}
int sts_presence_rss_detection_deinit(void)
{
bool deactivated = acc_detector_presence_deactivate(rss_handle);
acc_detector_presence_destroy(&rss_handle);
acc_rss_deactivate();
if (deactivated)
{
APP_LOG(TS_OFF, VLEVEL_M,"Application finished OK\n");
return EXIT_SUCCESS;
} else return EXIT_FAILURE;
}
int sts_presence_rss_fall_rise_detection(void)
{
const acc_hal_t *hal = acc_hal_integration_get_implementation();
@ -725,7 +866,7 @@ int sts_presence_rss_fall_rise_detection(void)
}
bool deactivated = false;
bool success = true;
const int iterations = 10; //(DEFAULT_UPDATE_RATE_PRESENCE);
const int iterations = 1; //(DEFAULT_UPDATE_RATE_PRESENCE);
acc_detector_presence_result_t result;
uint8_t average_result = 0;
float average_distance =0.0f;
@ -748,6 +889,7 @@ int sts_presence_rss_fall_rise_detection(void)
{
for (int i = 0; i < (iterations); i++)
{
success = acc_detector_presence_get_next(handle, &result);
success = acc_detector_presence_get_next(handle, &result);
if (!success)
{
@ -792,7 +934,7 @@ int sts_presence_rss_fall_rise_detection(void)
//acc_integration_sleep_ms(1000 / DEFAULT_UPDATE_RATE_PRESENCE); // 15ms, DEFAULT_UPDATE_RATE);
//acc_integration_sleep_ms(10); // --- around 1500 ms in total
//acc_integration_sleep_ms(2); //--- around 1000ms in total
acc_integration_sleep_ms(1000 / 20);
//acc_integration_sleep_ms(1000 / 20);
}
deactivated = acc_detector_presence_deactivate(handle);

View File

@ -34,7 +34,7 @@
#include "yunhorn_sts_prd_conf.h"
#include "yunhorn_sts_sensors.h"
#include "sts_cmox_hmac_sha.h"
#include "acc_detector_presence.h"
/* USER CODE BEGIN Includes */
extern volatile sts_cfg_nvm_t sts_cfg_nvm;
@ -73,7 +73,7 @@ extern volatile distance_measure_cfg_t distance_cfg;
extern volatile uint16_t sts_fall_rising_pattern_factor1;
extern volatile uint16_t sts_roc_acc_standard_variance;
volatile uint32_t last_sts_rss_time_stamp=0;
extern acc_detector_presence_handle_t rss_handle;
// RSS fall detection
extern volatile uint8_t sts_fall_detection_acc_threshold, sts_fall_detection_depth_threshold, sts_occupancy_overtime_threshold_in_10min;
extern volatile uint8_t sts_fall_rising_detected_result, sts_fall_rising_detected_result_changed_flag;
@ -332,7 +332,11 @@ void STS_YunhornSTSEventP2_Process(void)
break;
}
#endif
int res = sts_presence_rss_fall_rise_detection();
//int res = sts_presence_rss_fall_rise_detection();
if (rss_handle == NULL) {
sts_presence_rss_detection_init();
}
int res = sts_presence_rss_detection_process();
if (res == 0)
{

View File

@ -75,7 +75,7 @@ extern volatile uint32_t event_start_time, event_stop_time;
extern volatile uint16_t sts_unconscious_threshold;
volatile uint8_t sts_occupancy_overtime_state = 0;
volatile uint8_t sts_presence_fall_detection=FALSE;
volatile uint32_t SamplingPeriodicity = 1000; //unit ms
volatile uint32_t SamplingPeriodicity = 100; //unit ms
volatile uint32_t HeartBeatPeriodicity = 120000; //unit ms
volatile uint8_t STS_LoRa_WAN_Joined = 0;
@ -753,6 +753,7 @@ void STS_Sensor_Init(void)
UTIL_TIMER_Start(&YunhornSTSSamplingCheckTimer);
#endif
// #endif
/* VVVVVV migrated to yunhorn_sts_process.c */
@ -766,6 +767,7 @@ void STS_Sensor_Prepare(void)
UTIL_TIMER_Start(&YunhornSTSRSSWakeUpTimer);
}
/* USER CODE BEGIN PB_Callbacks */
@ -1785,7 +1787,7 @@ static void OnRestoreContextRequest(void *nvm, uint32_t nvm_size)
*/
static void OnYunhornSTSOORSSWakeUpTimerEvent(void *context)
{
//UTIL_TIMER_Stop(&YunhornSTSRSSWakeUpTimer);
UTIL_TIMER_Stop(&YunhornSTSRSSWakeUpTimer);
if ((sts_work_mode == STS_RSS_MODE)||(sts_work_mode == STS_DUAL_MODE)||(sts_work_mode == STS_UNI_MODE))
{
@ -3073,7 +3075,7 @@ void OnRestoreSTSCFGContextProcess(void)
//OnYunhornSTSHeartBeatPeriodicityChanged(APP_TX_DUTYCYCLE);
//}
OnYunhornSTSSamplingPeriodicityChanged(sampling); // in m-sec unit
//OnYunhornSTSSamplingPeriodicityChanged(sampling); // in m-sec unit
#endif
#if defined(YUNHORN_STS_R0_ENABLED)||defined(YUNHORN_STS_R5_ENABLED)||defined(YUNHORN_STS_R4_ENABLED)

Binary file not shown.