From 7927dd9bb13b3f63480292ead430571a0564cbfa Mon Sep 17 00:00:00 2001 From: YunHorn Technology Date: Mon, 17 Jun 2024 21:03:48 +0800 Subject: [PATCH] add sensor install height detection and store ----good so far --- Core/Inc/yunhorn_sts_sensors.h | 4 +-- Core/Src/yunhorn_sts_distance_rss.c | 43 +++++++++++++++++++---- Core/Src/yunhorn_sts_presence_rss.c | 17 +++++---- Core/Src/yunhorn_sts_process.c | 26 ++++++++++---- LoRaWAN/App/lora_app.c | 53 ++++++++++++++++++----------- 5 files changed, 102 insertions(+), 41 deletions(-) diff --git a/Core/Inc/yunhorn_sts_sensors.h b/Core/Inc/yunhorn_sts_sensors.h index 291ca7e..9894799 100644 --- a/Core/Inc/yunhorn_sts_sensors.h +++ b/Core/Inc/yunhorn_sts_sensors.h @@ -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 diff --git a/Core/Src/yunhorn_sts_distance_rss.c b/Core/Src/yunhorn_sts_distance_rss.c index 21c8d47..b386231 100644 --- a/Core/Src/yunhorn_sts_distance_rss.c +++ b/Core/Src/yunhorn_sts_distance_rss.c @@ -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); diff --git a/Core/Src/yunhorn_sts_presence_rss.c b/Core/Src/yunhorn_sts_presence_rss.c index 02dd631..e9212b0 100644 --- a/Core/Src/yunhorn_sts_presence_rss.c +++ b/Core/Src/yunhorn_sts_presence_rss.c @@ -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; } diff --git a/Core/Src/yunhorn_sts_process.c b/Core/Src/yunhorn_sts_process.c index 85fea55..5986d7c 100644 --- a/Core/Src/yunhorn_sts_process.c +++ b/Core/Src/yunhorn_sts_process.c @@ -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)); } diff --git a/LoRaWAN/App/lora_app.c b/LoRaWAN/App/lora_app.c index ceed355..0d2675b 100644 --- a/LoRaWAN/App/lora_app.c +++ b/LoRaWAN/App/lora_app.c @@ -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)