From 8aec93c16722bedb9acd84ee358ccb76d2b36527 Mon Sep 17 00:00:00 2001 From: YunHorn Technology Date: Fri, 28 Mar 2025 15:26:34 +0800 Subject: [PATCH] workable STS_O6 20250328 --- Core/Inc/yunhorn_sts_prd_conf.h | 2 + Core/Src/yunhorn_sts_distance_rss.c | 2 +- Core/Src/yunhorn_sts_presence_rss.c | 165 ++++++++++++++++++---------- Core/Src/yunhorn_sts_process.c | 54 +++++++-- LoRaWAN/App/lora_app.c | 8 +- 5 files changed, 157 insertions(+), 74 deletions(-) diff --git a/Core/Inc/yunhorn_sts_prd_conf.h b/Core/Inc/yunhorn_sts_prd_conf.h index feaa12e..1961868 100644 --- a/Core/Inc/yunhorn_sts_prd_conf.h +++ b/Core/Inc/yunhorn_sts_prd_conf.h @@ -271,10 +271,12 @@ #ifdef STS_O6 #define sts_senddataport (YUNHORN_STS_O6_LORA_APP_DATA_PORT) #define sts_sendhtbtport (YUNHORN_STS_O6_LORA_APP_HTBT_PORT) +#define YUNHORN_STS_PRD_STRING "STS_O6" #endif #ifdef STS_O7 #define sts_senddataport (YUNHORN_STS_O7_LORA_APP_DATA_PORT) #define sts_sendhtbtport (YUNHORN_STS_O7_LORA_APP_HTBT_PORT) +#define YUNHORN_STS_PRD_STRING "STS_O7" #endif #if defined(STS_O6)||defined(STS_O7) #define sts_appctrlport (YUNHORN_STS_O7_USER_APP_CTRL_PORT) diff --git a/Core/Src/yunhorn_sts_distance_rss.c b/Core/Src/yunhorn_sts_distance_rss.c index 3b26cfd..a0e4fe5 100644 --- a/Core/Src/yunhorn_sts_distance_rss.c +++ b/Core/Src/yunhorn_sts_distance_rss.c @@ -113,7 +113,7 @@ int sts_distance_rss_detector_distance(void) } bool success = true; - const int iterations = 5; //5; + const int iterations = 1; //5; uint16_t number_of_peaks = 1; // FSB first significant Bin acc_detector_distance_result_t result[number_of_peaks]; acc_detector_distance_result_info_t result_info; diff --git a/Core/Src/yunhorn_sts_presence_rss.c b/Core/Src/yunhorn_sts_presence_rss.c index f87ca46..f29cc4e 100644 --- a/Core/Src/yunhorn_sts_presence_rss.c +++ b/Core/Src/yunhorn_sts_presence_rss.c @@ -76,6 +76,8 @@ #define DEFAULT_INTRA_FRAME_TIME_CONST (0) //default 0.0 unit(seconds) #define DEFAULT_INTRA_FRAME_WEIGHT (0) //default 0.6 for normal slow tracking 1.0 for fast tracking +// 2025 03 26 *** if detection toggles too often, increase the following, if too sluggish, decrease it instead +//#define DEFAULT_OUTPUT_TIME_CONST (0.8f) //default 0.5 unit(seconds) [5] #define DEFAULT_OUTPUT_TIME_CONST (0.5f) //default 0.5 unit(seconds) [5] //#define DEFAULT_OUTPUT_TIME_CONST (0.4f) //default 0.5 unit(seconds) [5] @@ -145,6 +147,7 @@ volatile uint16_t last_average_presence_distance=0; volatile uint16_t sts_fall_rising_pattern_factor1=0, sts_fall_rising_pattern_factor2=0, sts_fall_rising_pattern_factor3=0; volatile uint16_t sts_roc_acc_standard_variance=0; extern volatile uint8_t sts_presence_fall_detection; +static uint8_t sts_rss_init_ok=0; /* USER CODE END Includes */ /* External variables ---------------------------------------------------------*/ @@ -341,8 +344,9 @@ static void print_current_configuration(acc_detector_presence_configuration_t pr float sts_run_f_intra_frame_weight = sts_run_filter.intra_frame_weight; float sts_run_f_output_time_const = sts_run_filter.output_time_const; - APP_LOG(TS_OFF, VLEVEL_M, "\r\nStart: %4d (mm) Length: %4d (mm) Threshold: %4d (*) Gain= %2d (%) UpdateRate=%2d Profile= %1d \r\n", - (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, "\r\nWork_mode:%2d Start: %4d (mm) Length: %4d (mm) Threshold: %4d (*) Gain= %2d (%) UpdateRate=%4d Profile= %d \r\n", + 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 " "(4)IntraFrameTimeConst:%4d (5)IntraWeight:%4u (5)OutputTime:%4u \r\n", @@ -392,59 +396,80 @@ int sts_presence_rss_fall_rise_detection(void) return EXIT_FAILURE; } - switch (sts_rss_config_updated_flag) - { - case STS_RSS_CONFIG_NON: - APP_LOG(TS_OFF, VLEVEL_H,"\r\n##### YUNHORN STS *** Non *** cfg \n"); - return EXIT_SUCCESS; - break; - case STS_RSS_CONFIG_DEFAULT: - set_default_configuration(presence_configuration); - APP_LOG(TS_OFF, VLEVEL_H,"\r\n##### YUNHORN STS *** Default *** cfg applied\n"); - break; - case STS_RSS_CONFIG_SIMPLE: - sts_rss_set_current_configuration_simple(presence_configuration); - APP_LOG(TS_OFF, VLEVEL_H,"\r\n##### YUNHORN STS *** Simple *** cfg applied\n"); - break; - case STS_RSS_CONFIG_FULL: - sts_rss_set_current_configuration_full(presence_configuration); - APP_LOG(TS_OFF, VLEVEL_H,"\r\n######### YUNHORN STS *** FULL *** cfg applied\n"); - break; - case STS_RSS_CONFIG_FALL_DETECTION: - set_default_fall_rise_configuration(presence_configuration); - APP_LOG(TS_OFF, VLEVEL_H,"\r\n######### YUNHORN STS *** FALL DETECTION *** cfg applied\n"); - break; -#if 1 - case STS_RSS_CONFIG_DEFAULT|STS_RSS_CONFIG_SIMPLE: - set_default_configuration(presence_configuration); - APP_LOG(TS_OFF, VLEVEL_H,"\r\n##### YUNHORN STS *** Default *** cfg applied\n"); - sts_rss_set_current_configuration_simple(presence_configuration); - APP_LOG(TS_OFF, VLEVEL_H,"\r\n##### YUNHORN STS *** Simple *** cfg applied\n"); - break; -#endif - case STS_RSS_CONFIG_DEFAULT|STS_RSS_CONFIG_FULL: - set_default_configuration(presence_configuration); - APP_LOG(TS_OFF, VLEVEL_H,"\r\n##### YUNHORN STS *** Default *** cfg applied\n"); - sts_rss_set_current_configuration_full(presence_configuration); - APP_LOG(TS_OFF, VLEVEL_H,"\r\n######### YUNHORN STS *** FULL *** cfg applied\n"); - break; - case STS_RSS_CONFIG_FULL|STS_RSS_CONFIG_FALL_DETECTION: - set_default_fall_rise_configuration(presence_configuration); - APP_LOG(TS_OFF, VLEVEL_H,"\r\n######### YUNHORN STS *** FALL DETECTION *** cfg applied\n"); - sts_rss_set_current_configuration_full(presence_configuration); - APP_LOG(TS_OFF, VLEVEL_H,"\r\n######### YUNHORN STS *** FULL *** cfg applied\n"); - break; - case STS_RSS_CONFIG_SIMPLE|STS_RSS_CONFIG_FALL_DETECTION: - set_default_fall_rise_configuration(presence_configuration); - APP_LOG(TS_OFF, VLEVEL_H,"\r\n##### YUNHORN STS *** Default *** cfg applied\n"); - sts_rss_set_current_configuration_simple(presence_configuration); - APP_LOG(TS_OFF, VLEVEL_H,"\r\n##### YUNHORN STS *** Simple *** cfg applied\n"); - break; - default: - break; + if ((sts_rss_config_updated_flag != STS_RSS_CONFIG_NON) || (sts_rss_init_ok != 1)) + { + APP_LOG(TS_OFF, VLEVEL_H, "\r\n ------ ---------- rss cfg update flag=%02x \r\n", sts_rss_config_updated_flag); + + + if (sts_rss_init_ok==0) + { + if (sts_work_mode ==STS_UNI_MODE) sts_rss_config_updated_flag |= STS_RSS_CONFIG_FALL_DETECTION; + else if ((sts_work_mode == STS_RSS_MODE) || (sts_work_mode == STS_DUAL_MODE)) sts_rss_config_updated_flag |= STS_RSS_CONFIG_DEFAULT; + set_default_configuration(presence_configuration); + APP_LOG(TS_OFF, VLEVEL_H, "\r\n First Time RSS init, update flag=%02x\r\n", sts_rss_config_updated_flag); + print_current_configuration(presence_configuration); + sts_rss_init_ok = 1; + } + + APP_LOG(TS_OFF, VLEVEL_H, "\r\n Update flag=%02x, workmode=%2d \r\n", sts_rss_config_updated_flag, sts_work_mode); + switch (sts_rss_config_updated_flag) + { + #if 0 + case STS_RSS_CONFIG_NON: + APP_LOG(TS_OFF, VLEVEL_H,"\r\n##### YUNHORN STS *** Non *** cfg \n"); + // return EXIT_SUCCESS; + break; + #endif + case STS_RSS_CONFIG_DEFAULT: + set_default_configuration(presence_configuration); + APP_LOG(TS_OFF, VLEVEL_H,"\r\n##### YUNHORN STS *** Default *** cfg applied\n"); + break; + case STS_RSS_CONFIG_SIMPLE: + sts_rss_set_current_configuration_simple(presence_configuration); + APP_LOG(TS_OFF, VLEVEL_H,"\r\n##### YUNHORN STS *** Simple *** cfg applied\n"); + break; + case STS_RSS_CONFIG_FULL: + sts_rss_set_current_configuration_full(presence_configuration); + APP_LOG(TS_OFF, VLEVEL_H,"\r\n######### YUNHORN STS *** FULL *** cfg applied\n"); + break; + case STS_RSS_CONFIG_FALL_DETECTION: + set_default_fall_rise_configuration(presence_configuration); + APP_LOG(TS_OFF, VLEVEL_H,"\r\n######### YUNHORN STS *** FALL DETECTION *** cfg applied\n"); + break; + #if 1 + case STS_RSS_CONFIG_DEFAULT|STS_RSS_CONFIG_SIMPLE: + set_default_configuration(presence_configuration); + APP_LOG(TS_OFF, VLEVEL_H,"\r\n##### YUNHORN STS *** Default *** cfg applied\n"); + sts_rss_set_current_configuration_simple(presence_configuration); + APP_LOG(TS_OFF, VLEVEL_H,"\r\n##### YUNHORN STS *** Simple *** cfg applied\n"); + break; + #endif + + case STS_RSS_CONFIG_DEFAULT|STS_RSS_CONFIG_FULL: + set_default_configuration(presence_configuration); + APP_LOG(TS_OFF, VLEVEL_H,"\r\n##### YUNHORN STS *** Default *** cfg applied\n"); + sts_rss_set_current_configuration_full(presence_configuration); + APP_LOG(TS_OFF, VLEVEL_H,"\r\n######### YUNHORN STS *** FULL *** cfg applied\n"); + break; + case STS_RSS_CONFIG_FULL|STS_RSS_CONFIG_FALL_DETECTION: + set_default_fall_rise_configuration(presence_configuration); + APP_LOG(TS_OFF, VLEVEL_H,"\r\n######### YUNHORN STS *** FALL DETECTION *** cfg applied\n"); + sts_rss_set_current_configuration_full(presence_configuration); + APP_LOG(TS_OFF, VLEVEL_H,"\r\n######### YUNHORN STS *** FULL *** cfg applied\n"); + break; + case STS_RSS_CONFIG_SIMPLE|STS_RSS_CONFIG_FALL_DETECTION: + set_default_fall_rise_configuration(presence_configuration); + APP_LOG(TS_OFF, VLEVEL_M,"\r\n##### YUNHORN STS *** Default *** cfg applied\n"); + sts_rss_set_current_configuration_simple(presence_configuration); + APP_LOG(TS_OFF, VLEVEL_M,"\r\n##### YUNHORN STS *** Simple *** cfg applied\n"); + break; + default: + break; + } + + // sts_rss_config_updated_flag = STS_RSS_CONFIG_NON; //update finished, set to 0 } - //sts_rss_config_updated_flag = STS_RSS_CONFIG_DEFAULT; //update finished, set to 0 acc_detector_presence_handle_t handle = acc_detector_presence_create(presence_configuration); if (handle == NULL) @@ -457,6 +482,8 @@ int sts_presence_rss_fall_rise_detection(void) } + APP_LOG(TS_OFF, VLEVEL_H,"\r\n============= Start Scan\n"); + print_current_configuration(presence_configuration); acc_detector_presence_configuration_destroy(&presence_configuration); @@ -535,14 +562,14 @@ 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(5); //--- around 1000ms in total + acc_integration_sleep_ms(2); //--- around 1000ms in total } deactivated = acc_detector_presence_deactivate(handle); acc_detector_presence_destroy(&handle); acc_rss_deactivate(); - APP_LOG(TS_OFF, VLEVEL_M, "\r\n First Half --- Motion Count = %u \r\n", motion_count); + APP_LOG(TS_OFF, VLEVEL_H, "\r\n First Half --- Motion Count = %u \r\n", motion_count); //acc_detector_presence_deactivate(handle); } @@ -623,14 +650,14 @@ int sts_presence_rss_fall_rise_detection(void) // ~12 ms per RSS scan // acc_integration_sleep_ms(1000 / DEFAULT_UPDATE_RATE_PRESENCE); // 15 ms, DEFAULT_UPDATE_RATE); //acc_integration_sleep_ms(10); //--- around 1500 ms in total - acc_integration_sleep_ms(5); //--- around 1000ms in total + acc_integration_sleep_ms(2); //--- around 1000ms in total } deactivated = acc_detector_presence_deactivate(handle); acc_detector_presence_destroy(&handle); acc_rss_deactivate(); - APP_LOG(TS_OFF, VLEVEL_M, "\r\n Second Half --- Motion Count Sum to = %u \r\n", motion_count); + APP_LOG(TS_OFF, VLEVEL_H, "\r\n Second Half --- Motion Count Sum to = %u \r\n", motion_count); //APP_LOG(TS_OFF, VLEVEL_L,"Second Half, Fall Rise Detection, Motion Count = %u \r\n", (int)motion_count); @@ -654,17 +681,35 @@ int sts_presence_rss_fall_rise_detection(void) // uint8_t pre_sts_rss_result = (average_result > (DEFAULT_UPDATE_RATE_PRESENCE/5))? 1: 0; // sts_rss_result=STS_RSS_Filter(pre_sts_rss_result); - sts_rss_result = (average_result > (DEFAULT_UPDATE_RATE_PRESENCE/5))? 1: 0; + //APP_LOG(TS_OFF, VLEVEL_M, "\r\nAverage Result=%d Distance=%d, Score=%d MotionCount=%d \r\n", +// (int)average_result, (int)average_distance, (int)average_score, (int)motion_count); + + // sts_rss_result = (average_result > (DEFAULT_UPDATE_RATE_PRESENCE/5))? 1: 0; + // sts_rss_result = (average_result > 0)? 1: 0; + + uint8_t pre_sts_rss_result=0; + if (sts_work_mode == STS_UNI_MODE) { + pre_sts_rss_result = (average_result > 0)? 1: 0; + } else { + pre_sts_rss_result = (average_result > 0)? 1: 0; + } + sts_rss_result = STS_RSS_Filter(pre_sts_rss_result); + + // APP_LOG(TS_OFF, VLEVEL_M, "\r\nMotionCount=%4d Overall Motion=%d \r\n", (int)motion_count, (int)sts_rss_result); + //APP_LOG(TS_OFF, VLEVEL_M, "\r\nAverage Result=%d Distance=%d, Score=%d MotionCount=%d ---Overall Result=%d \r\n", + // (int)average_result, (int)average_distance, (int)average_score, (int)motion_count, (int)sts_rss_result); + APP_LOG(TS_OFF, VLEVEL_M, "\r\nMotion Status: %d %d (mm) %d %d Rated-> %d \r\n",(int)average_result, (int)average_distance, (int)average_score, (int)motion_count, (int)sts_rss_result); +#if 0 if (sts_rss_result) //if (average_score !=0) //if (sts_rss_result) { - APP_LOG(TS_OFF, VLEVEL_M,"\r\n######## MotionLevel=%4u (level) Gain=%2u (%) Start=%4u (mm) Length=%4u (mm)\r\n", + APP_LOG(TS_OFF, VLEVEL_L,"\r\n######## MotionLevel=%4u (level) Gain=%2u (%) Start=%4u (mm) Length=%4u (mm)\r\n", (int)(1000.0f*sts_presence_rss_config.default_threshold),(int)(100.0f*sts_presence_rss_config.default_receiver_gain), (int)(1000.0f*sts_presence_rss_config.default_start_m), (int)(1000.0f*sts_presence_rss_config.default_length_m)); APP_LOG(TS_OFF, VLEVEL_M,"\r\n######## Motion: %u Distance=%u mm, Score=%u Average_result=%u out of %u \r\n", (uint8_t)sts_rss_result,(int) sts_presence_rss_distance, (int)(sts_presence_rss_score), (int)average_result, (int)iterations); } - +#endif // RSS feature 1: Motion, No-motion process diff --git a/Core/Src/yunhorn_sts_process.c b/Core/Src/yunhorn_sts_process.c index f2f2987..e3e8c5d 100644 --- a/Core/Src/yunhorn_sts_process.c +++ b/Core/Src/yunhorn_sts_process.c @@ -273,6 +273,7 @@ void STS_YunhornSTSEventP2_Process(void) //STS_Lamp_Bar_Refresh(); //TODO XXX eliminate refresh every second.... try if ((sts_work_mode >= STS_RSS_MODE) && (sts_work_mode <= STS_TOF_RSS_MODE)) { +#if 0 switch (sts_work_mode) { case STS_RSS_MODE: sts_rss_config_updated_flag |= STS_RSS_CONFIG_DEFAULT; @@ -286,7 +287,7 @@ void STS_YunhornSTSEventP2_Process(void) default: break; } - +#endif int res = sts_presence_rss_fall_rise_detection(); if (res == 0) { @@ -1442,15 +1443,16 @@ void Radar_Filtering_clutter(volatile uint8_t *color) } #endif -#define PRESET_DENOMINATOR 8 -#define PRESET_NUMERATOR 5 +#if 0 +#define PRESET_DENOMINATOR 5 +#define PRESET_NUMERATOR (PRESET_DENOMINATOR - 3) +static uint8_t Motion_Changed_Flag = 0; +static uint8_t numerator = 0; +static uint8_t denominator = 0; uint8_t STS_RSS_Filter(uint8_t pre_sts_rss_result) { _Bool pNew_Motion_Flag = 0; - static uint8_t Motion_Changed_Flag = 0; - static uint8_t numerator = 0; - static uint8_t denominator = 0; uint8_t Motion_Flag = sts_rss_result, xReturn=0; pNew_Motion_Flag = pre_sts_rss_result; @@ -1458,9 +1460,11 @@ uint8_t STS_RSS_Filter(uint8_t pre_sts_rss_result) Motion_Changed_Flag = 1; } + denominator++; + if(Motion_Changed_Flag == 1) { - denominator++; + if(pNew_Motion_Flag != Motion_Flag){ numerator++; } @@ -1487,7 +1491,43 @@ uint8_t STS_RSS_Filter(uint8_t pre_sts_rss_result) } return xReturn; } +#endif +#define FILTER_LEN 8 +#define SLIDING_WIN_LEN 3 +static uint8_t motion_read[FILTER_LEN]={0}; +static uint8_t idx_filter=0; +uint8_t STS_RSS_Filter(uint8_t pre_sts_rss_result) +{ + uint8_t j=0; + uint8_t sum_sliding_win=0; + motion_read[idx_filter] = pre_sts_rss_result; + + idx_filter = (idx_filter + 1) % FILTER_LEN; + + switch(idx_filter) + { + case 1: + sum_sliding_win = motion_read[idx_filter-1] + motion_read[FILTER_LEN-1] + motion_read[FILTER_LEN-2]; + break; + case 2: + sum_sliding_win = motion_read[idx_filter-1] + motion_read[idx_filter-2] + motion_read[FILTER_LEN-1]; + break; + case 0: + sum_sliding_win = motion_read[FILTER_LEN-1] + motion_read[FILTER_LEN-2] + motion_read[FILTER_LEN-3]; + break; + default: + sum_sliding_win = motion_read[idx_filter-1] + motion_read[idx_filter-2] + motion_read[idx_filter-3]; + break; + } + uint8_t sum_filter = 0; + for (j=0; j 5))? 1:0; + +} /* USER CODE BEGIN EF */ /* USER CODE END EF */ diff --git a/LoRaWAN/App/lora_app.c b/LoRaWAN/App/lora_app.c index 847854d..fc4c607 100644 --- a/LoRaWAN/App/lora_app.c +++ b/LoRaWAN/App/lora_app.c @@ -114,7 +114,7 @@ volatile sts_cfg_nvm_t sts_cfg_nvm = { 0x01, //inter frame slow cutoff,0x01=1[1]*0.01=0.01f 0x00, //intra frame time const [0]=0 Lower to reduce sensitivity, higher to increase sensitivity 0x0A, //intra frame weight, 0x00=[0]*0.1=0.0F 0x0A=10, 10*0.1=1 FOR FAST MOVEMENT TRACKING FALL DETECTION - 0x05, //output time const 0x05=[5]*0.1=0.5 + 0x09, //output time const 0x05=[5]*0.1=0.5 0.5--> 0.9 2025-03-26 TODO XXXXXX 0x02, //downsampling factor [2]=2 0x03, //power saving mode ACTIVE [3] = 3U 0x00, //reserve --P[17] @@ -1426,10 +1426,6 @@ static void OnJoinRequest(LmHandlerJoinParams_t *joinParams) heart_beat_timer = 1; //SendTxData(); - UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_LoRaSendOnTxTimerOrButtonEvent), CFG_SEQ_Prio_0); - - UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_LoRaSendOnTxTimerOrButtonEvent), CFG_SEQ_Prio_0); - //UTIL_TIMER_Start(&TxTimer); UTIL_TIMER_Start(&STSDurationCheckTimer); OnYunhornSTSHeartBeatPeriodicityChanged(HeartBeatPeriodicity); @@ -3035,7 +3031,7 @@ uint32_t STS_Get_Date_Time_Stamp(void) UnixEpoch.Seconds -= 18; /*removing leap seconds*/ SysTimeLocalTime(UnixEpoch.Seconds, &localtime); - APP_LOG(TS_OFF, VLEVEL_M, "UTC TIME:%02dh%02dm%02ds on %02d/%02d/%04d\r\n", + APP_LOG(TS_OFF, VLEVEL_H, "UTC TIME:%02dh%02dm%02ds on %02d/%02d/%04d\r\n", localtime.tm_hour, localtime.tm_min, localtime.tm_sec, localtime.tm_mday, localtime.tm_mon + 1, localtime.tm_year + 1900);