sundp
/
O7
1
0
Fork 0
Code Issues Pull Requests 1 Packages Projects Releases Wiki Activity

add sensor install height detection and store ----good so far

This commit is contained in:
Yunhorn 2024-06-17 21:03:48 +08:00
parent f427cdf2ec
commit 7927dd9bb1
5 changed files with 102 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Core/Inc/yunhorn_sts_sensors.h
View File

@ -570,7 +570,7 @@ enum nvm_order {
NVM_CFG_START_END=31, //31, p[19]
NVM_RESERVE02, //32
NVM_RESERVE03, //33
NVM_RESERVE04, //34
NVM_SENSOR_INSTALL_HEIGHT, //34
NVM_ALARM_PARAMETER05, //35
NVM_ALARM_MUTE_RESET_TIMER, //36
NVM_ALARM_LAMP_BAR_FLASHING_COLOR, //37
@ -604,7 +604,7 @@ typedef struct sts_cfg_nvm {
uint8_t reserve02;
uint8_t reserve03;
uint8_t reserve04;
uint8_t sensor_install_height_in_10cm;
uint8_t alarm_parameter05;
uint8_t alarm_mute_reset_timer_in_10sec; //60(0x3C) sec alarm_mute_or_reset_expire_timer_in_sec
uint8_t alarm_lamp_bar_flashing_color; //Lamp Bar Flashing color define, 0x20, 2==STS_RED, 0 = STS_DARK, 0x23, 2=STS_RED, 3=STS_BLUE

43
Core/Src/yunhorn_sts_distance_rss.c
View File

@ -33,15 +33,23 @@
#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
//volatile distance_measure_cfg_t distance_cfg={1.5, 2.0, 1, 63, 2, 10, 0.5, 1.3, 0.2};
volatile distance_measure_cfg_t distance_cfg={1.5, 2.0, 2, 63, 2, 10, 0.5, 1.3, 0.2};
extern float sts_distance_rss_distance;
//volatile distance_measure_cfg_t distance_cfg={1.5, 3.3, 2, 63, 4, 10, 0.8182f, 0.4, 0.2};
extern float sts_distance_rss_distance, sts_sensor_install_height;
static void sts_distance_rss_update_configuration(acc_detector_distance_configuration_t distance_configuration);
@ -109,14 +117,16 @@ int sts_distance_rss_detector_distance(void)
APP_LOG(TS_OFF, VLEVEL_M, "acc_detector_distance_get_next() failed\n");
break;
}
for(uint8_t j=0; j< number_of_peaks; j++)
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/number_of_peaks);
sts_distance_rss_distance = (uint16_t)(1000*tmp_distance/result_info.number_of_peaks);
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
if (sts_distance_rss_distance > 2000) sts_sensor_install_height = sts_distance_rss_distance;
bool deactivated = acc_detector_distance_deactivate(distance_handle);
@ -132,7 +142,27 @@ int sts_distance_rss_detector_distance(void)
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);
@ -149,8 +179,7 @@ static void sts_distance_rss_update_configuration(acc_detector_distance_configur
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);

17
Core/Src/yunhorn_sts_presence_rss.c
View File

@ -89,7 +89,8 @@ extern volatile uint8_t sts_fall_detection_acc_threshold, sts_fall_detection_dep
volatile uint8_t sts_unconscious_state=0;
volatile uint16_t sts_unconscious_threshold=1280, sts_unconscious_duration=0;
extern volatile uint8_t sts_rss_result, sts_rss_config_updated_flag, last_sts_rss_result;
extern volatile float sts_distance_rss_distance;
extern volatile float sts_distance_rss_distance, sts_sensor_install_height;
volatile float sts_presence_rss_distance, sts_presence_rss_score;
volatile STS_OO_RSS_SensorTuneDataTypeDef sts_presence_rss_config;
//static void update_configuration(acc_detector_presence_configuration_t presence_configuration);
@ -538,7 +539,7 @@ int sts_presence_rss_fall_rise_detection(void)
void STS_YunhornCheckStandardDeviation(void)
{
uint16_t i,j;
uint16_t i,j; // sts_sensor_install_height <--- average_presence_distance should be approaching this distance - 50cm
float sum_presence_distance = 0, sum_presence_score=0;
float average_presence_distance = 0, average_presence_score=0;
float variance_presence_distance = 0, variance_presence_score=0;
@ -622,10 +623,10 @@ void STS_YunhornCheckStandardDeviation(void)
standard_variance_roc_acc *= (float)DEFAULT_MOTION_DATASET_LEN;
// print result
#ifdef LOG_RSS
APP_LOG(TS_OFF, VLEVEL_L, "\r\n-------------Distance Average =%6u; Variance = %6u ; Standard =%6u \r\n",
(int)(average_presence_distance*1000.0f), (int)(variance_presence_distance*1000.0f), (int)(standard_variance_presence_distance*1000.0f));
#endif
//#ifdef LOG_RSS
APP_LOG(TS_OFF, VLEVEL_L, "\r\n---Sensor Install Height=%6u-----Distance Average =%6u; Variance = %6u ; Standard =%6u \r\n",
(int)sts_sensor_install_height, (int)(average_presence_distance*1000.0f), (int)(variance_presence_distance*1000.0f), (int)(standard_variance_presence_distance*1000.0f));
//#endif
#ifdef LOG_RSS
APP_LOG(TS_OFF, VLEVEL_M, "-------------Motion Average =%6u; Variance = %6u ; Standard =%6u \r\n",
(int)(average_presence_score*1000.0f), (int)(variance_presence_score*1000.0f), (int)(standard_variance_presence_score*1000.0f));
@ -648,7 +649,9 @@ void STS_YunhornCheckStandardDeviation(void)
(int)(sts_fall_detection_depth_threshold), (int)(sts_fall_rising_pattern_factor2));
#endif
// *********** detection suggestion
// *******************************************
// *********** detection situation suggestion
if (standard_variance_presence_score <= MIN(DEFAULT_UNCONSCIOUS_THRESHOLD, sts_unconscious_threshold)) {
sts_fall_rising_detected_result = STS_PRESENCE_STAYSTILL;
}

26
Core/Src/yunhorn_sts_process.c
View File

@ -48,7 +48,7 @@ volatile uint8_t last_sts_reed_hall_result;
volatile uint8_t sts_soap_level_state;
volatile STS_OO_SensorStatusDataTypeDef sts_o7_sensorData;
volatile STS_PRESENCE_SENSOR_Event_Status_t sts_o7_event_status;
volatile float sts_distance_rss_distance;
volatile float sts_distance_rss_distance, sts_sensor_install_height=0;//in mm
extern volatile float sts_presence_rss_distance, sts_presence_rss_score;
extern volatile uint8_t sts_hall1_read,sts_hall2_read;
volatile uint8_t sts_reed_hall_1_result=STS_Status_Door_Open,sts_reed_hall_2_result=STS_Status_SOS_Release, last_sts_reed_hall_1_result=STS_Status_Door_Open, last_sts_reed_hall_2_result=STS_Status_SOS_Release;
@ -608,7 +608,8 @@ void STS_PRESENCE_SENSOR_Init(void)
STS_SENSOR_Power_ON(0);
STS_PRESENCE_SENSOR_REEDSWITCH_HALL_Init();
//STS_PRESENCE_SENSOR_TOF_Init();
STS_PRESENCE_SENSOR_RSS_Init();
//HAL_Delay(2000);
//STS_PRESENCE_SENSOR_Distance_Measure_Process();
mems_int1_detected=0;
}
@ -630,6 +631,19 @@ void STS_PRESENCE_SENSOR_RSS_Init(void)
STS_SENSOR_Power_ON(0);
if ((sts_distance_rss_distance==0)&&(sts_sensor_install_height==0))
{
uint8_t exit_status =0;
exit_status=sts_distance_rss_detector_distance();
if (exit_status ==0) {
APP_LOG(TS_ON, VLEVEL_M, "##### RSS Installation Height =%u \r\n", (uint16_t)sts_distance_rss_distance);
}
else {
APP_LOG(TS_ON, VLEVEL_M, "##### RSS Installation Height Error \r\n");
}
sts_sensor_install_height=sts_distance_rss_distance;
}
STS_PRESENCE_SENSOR_NVM_CFG();
sts_rss_config_updated_flag = STS_RSS_CONFIG_DEFAULT;
@ -677,12 +691,12 @@ void STS_PRESENCE_SENSOR_Distance_Measure_Process(void)
APP_LOG(TS_OFF, VLEVEL_L, "\r\n ****start_m=%u length_m=%u profile=%u hwaas=%u \r\n",
(unsigned int)(distance_cfg.start_m*1000),(unsigned int)(distance_cfg.length_m*1000),
(unsigned int)(distance_cfg.acc_profile),(unsigned int)(distance_cfg.hwaas));
//do
do
{
exit_status = sts_distance_rss_detector_distance();
//HAL_Delay(100);
//i++;
} //while ((exit_status == EXIT_FAILURE) && (i < 1));
HAL_Delay(100);
i++;
} while ((exit_status == EXIT_FAILURE) && (i < 2));
}

53
LoRaWAN/App/lora_app.c
View File

@ -118,7 +118,7 @@ volatile sts_cfg_nvm_t sts_cfg_nvm = {
}, // above 20 bytes
0x00, //reserve2
0x00, //reserve3
0x00, //reserve4
0x20, //sensor install height in 10 cm, default 32*10=320cm, 3.2meter
0x00, //reserve5 alarm_parameter05
0x06, //reserve6 alarm_mute_or_reset_expire_timer_in_10sec, 60 seconds
@ -138,7 +138,7 @@ volatile sts_cfg_nvm_t sts_cfg_nvm = {
#if defined(STS_O6)||defined(STS_O7)
extern volatile uint8_t sensor_data_ready;
extern volatile STS_OO_SensorStatusDataTypeDef sts_o7_sensorData;
extern volatile float sts_distance_rss_distance;
extern volatile float sts_distance_rss_distance, sts_sensor_install_height;
char sts_lamp_color_code[15][15]={
"Dark",
@ -948,11 +948,11 @@ static void SendTxData(void)
if (sts_work_mode == STS_UNI_MODE)
{
APP_LOG(TS_OFF, VLEVEL_L,
"\r\n######| S1-DoorOpen | S2-Motion | S3-SOS | S4 |Distance(mm) | MotionScore| Unconscious | Over_Stay | Fall Detected|"
"\r\n######| %1d | %1d | %1d | %1d | %04d | %04d | %1d | %1d | %1d |\r\n",
"\r\n######| S1-DoorOpen | S2-Motion | S3-SOS | S4 |Distance(mm) | MotionScore| Unconscious | Over_Stay_(min) | Fall Detected|"
"\r\n######| %1d | %1d | %1d | %1d | %04d | %04d | %1d | %4d | %1d |\r\n",
sensorData.state_sensor1_on_off, sensorData.state_sensor2_on_off,sensorData.state_sensor3_on_off, sensorData.state_sensor4_on_off,
(uint16_t)sensorData.rss_presence_distance,(uint16_t)sensorData.rss_presence_score,
sensorData.unconscious_state, sensorData.unconscious_duration, sensorData.fall_state );
sensorData.unconscious_state, sensorData.over_stay_duration/60, sensorData.fall_state );
} else if (sts_work_mode == STS_DUAL_MODE) {
APP_LOG(TS_OFF, VLEVEL_L,
@ -1261,6 +1261,7 @@ static void OnJoinRequest(LmHandlerJoinParams_t *joinParams)
UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_LoRaSendOnTxTimerOrButtonEvent), CFG_SEQ_Prio_0);
HAL_Delay(3000);
UTIL_TIMER_Start(&STSLampBarColorTimer);
//UTIL_TIMER_Start(&YunhornSTSRSSWakeUpTimer);
/* USER CODE END OnJoinRequest_1 */
}
@ -1518,17 +1519,17 @@ static void OnYunhornSTSOORSSWakeUpTimerEvent(void *context)
if (p2_work_finished)
{
UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_YunhornSTSEventP2), CFG_SEQ_Prio_0);
UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_YunhornSTSEventP2), CFG_SEQ_Prio_0);
if ((STS_LoRa_WAN_Joined != 0)&&(sts_rss_result_changed_flag==1))
{
sts_rss_result_changed_flag = 0;
UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_LoRaSendOnTxTimerOrButtonEvent), CFG_SEQ_Prio_0);
//if ((last_sts_rss_result ==STS_RESULT_NO_MOTION)&& (sts_rss_result==STS_RESULT_MOTION))
{
OnSensor3StateChanged();
}
}
if ((STS_LoRa_WAN_Joined != 0)&&(sts_rss_result_changed_flag==1))
{
sts_rss_result_changed_flag = 0;
UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_LoRaSendOnTxTimerOrButtonEvent), CFG_SEQ_Prio_0);
//if ((last_sts_rss_result ==STS_RESULT_NO_MOTION)&& (sts_rss_result==STS_RESULT_MOTION))
{
OnSensor3StateChanged();
}
}
}
//UTIL_TIMER_Start(&YunhornSTSRSSWakeUpTimer);
@ -1778,6 +1779,19 @@ void USER_APP_AUTO_RESPONDER_Parse(char *tlv_buf, size_t tlv_buf_size)
APP_LOG(TS_OFF, VLEVEL_H, "\r\nRSS Measured Distance=[%u] mm \r\n", (uint16_t)sts_distance_rss_distance);
// Store valid installation height value
sts_cfg_nvm.mtmcode1 = (uint8_t)sts_mtmcode1;
sts_cfg_nvm.mtmcode2 = (uint8_t)sts_mtmcode2;
sts_cfg_nvm.version = (uint8_t)sts_version;
sts_cfg_nvm.hardware_ver = (uint8_t)sts_hardware_ver;
sts_cfg_nvm.periodicity = (uint8_t)((tlv_buf[CFG_CMD3]-0x30)*10+(tlv_buf[CFG_CMD4]-0x30));
sts_cfg_nvm.unit = (uint8_t)tlv_buf[CFG_CMD5];
sts_cfg_nvm.work_mode = (uint8_t)sts_work_mode;
sts_cfg_nvm.sts_service_mask = (uint8_t)sts_service_mask;
sts_cfg_nvm.sensor_install_height_in_10cm = sts_sensor_install_height/100; //in 10 cm
OnStoreSTSCFGContextRequest();
i=0;
//memset((void*)outbuf,0x0, sizeof(outbuf));
outbuf[i++] = (uint8_t)'D';
@ -2474,7 +2488,7 @@ void OnStoreSTSCFGContextRequest(void)
}
nvm_store_value[i++] = sts_cfg_nvm.reserve02;
nvm_store_value[i++] = sts_cfg_nvm.reserve03;
nvm_store_value[i++] = sts_cfg_nvm.reserve04;
nvm_store_value[i++] = sts_cfg_nvm.sensor_install_height_in_10cm;
nvm_store_value[i++] = sts_cfg_nvm.alarm_parameter05;
nvm_store_value[i++] = sts_cfg_nvm.alarm_mute_reset_timer_in_10sec;
nvm_store_value[i++] = sts_cfg_nvm.alarm_lamp_bar_flashing_color;
@ -2561,7 +2575,7 @@ void STS_REBOOT_CONFIG_Init(void)
}
sts_cfg_nvm.reserve02 =(uint8_t)nvm_stored_value[NVM_RESERVE02];
sts_cfg_nvm.reserve03 =(uint8_t)nvm_stored_value[NVM_RESERVE03];
sts_cfg_nvm.reserve04 =(uint8_t)nvm_stored_value[NVM_RESERVE04];
sts_cfg_nvm.sensor_install_height_in_10cm =(uint8_t)nvm_stored_value[NVM_SENSOR_INSTALL_HEIGHT];
sts_cfg_nvm.alarm_parameter05 =(uint8_t)nvm_stored_value[NVM_ALARM_PARAMETER05];
sts_cfg_nvm.alarm_mute_reset_timer_in_10sec = (uint8_t)nvm_stored_value[NVM_ALARM_MUTE_RESET_TIMER];
sts_cfg_nvm.alarm_lamp_bar_flashing_color = (uint8_t)nvm_stored_value[NVM_ALARM_LAMP_BAR_FLASHING_COLOR];
@ -2642,7 +2656,7 @@ void OnRestoreSTSCFGContextProcess(void)
} else {
sts_presence_fall_detection =0;
}
sts_sensor_install_height = sts_cfg_nvm.sensor_install_height_in_10cm*100; //10cm translate to mm
sts_fall_detection_acc_threshold = (uint8_t)sts_cfg_nvm.fall_detection_acc_threshold*10;
sts_fall_detection_depth_threshold = (uint8_t)sts_cfg_nvm.fall_detection_depth_threshold*10; //in cm
sts_fall_confirm_threshold_in_10sec = (uint8_t)sts_cfg_nvm.fall_confirm_threshold_in_10sec*10;
@ -2671,7 +2685,7 @@ void OnRestoreSTSCFGContextProcess(void)
void STS_SENSOR_Distance_Test_Process(void)
{
#if defined(STS_O6)||defined(STS_O7)
sts_distance_rss_distance =0; uint8_t i=0;
//sts_distance_rss_distance =0; uint8_t i=0;
//do {
STS_PRESENCE_SENSOR_Distance_Measure_Process();
// HAL_Delay(200);
@ -2679,6 +2693,7 @@ void STS_SENSOR_Distance_Test_Process(void)
// } while((sts_distance_rss_distance == 0)&&(i<2));
APP_LOG(TS_OFF, VLEVEL_L, "\r\nSensor Function Test: Distance Measured =%u mm\r\n", (uint16_t)sts_distance_rss_distance);
#endif
#if defined(YUNHORN_STS_R0_ENABLED)||defined(YUNHORN_STS_R5_ENABLED)