diff --git a/Core/Inc/yunhorn_sts_sensors.h b/Core/Inc/yunhorn_sts_sensors.h index 349f490..ca214cf 100644 --- a/Core/Inc/yunhorn_sts_sensors.h +++ b/Core/Inc/yunhorn_sts_sensors.h @@ -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); diff --git a/Core/Src/main.c b/Core/Src/main.c index 38fce5e..43ee6f2 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -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 */ diff --git a/Core/Src/sys_app.c b/Core/Src/sys_app.c index f23f63b..0e67901 100644 --- a/Core/Src/sys_app.c +++ b/Core/Src/sys_app.c @@ -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(); diff --git a/Core/Src/yunhorn_sts_presence_rss.c b/Core/Src/yunhorn_sts_presence_rss.c index 5f58e47..538393a 100644 --- a/Core/Src/yunhorn_sts_presence_rss.c +++ b/Core/Src/yunhorn_sts_presence_rss.c @@ -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); diff --git a/Core/Src/yunhorn_sts_process.c b/Core/Src/yunhorn_sts_process.c index 70ed0e2..75da6d6 100644 --- a/Core/Src/yunhorn_sts_process.c +++ b/Core/Src/yunhorn_sts_process.c @@ -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) { diff --git a/LoRaWAN/App/lora_app.c b/LoRaWAN/App/lora_app.c index 229dd46..f0b2896 100644 --- a/LoRaWAN/App/lora_app.c +++ b/LoRaWAN/App/lora_app.c @@ -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) diff --git a/STM32CubeIDE/Release/STS_O7.bin b/STM32CubeIDE/Release/STS_O7.bin index b749309..d69e713 100644 Binary files a/STM32CubeIDE/Release/STS_O7.bin and b/STM32CubeIDE/Release/STS_O7.bin differ