/* USER CODE BEGIN Header */ /** ****************************************************************************** * @file yunhorn_sts_presence_rss.c * * @author Yunhorn (r) Technology Limited Application Team * * @brief Yunhorn (r) SmarToilets (r) Product configuration file. * ****************************************************************************** * @attention * * Copyright (c) 2022 Yunhorn Technology Limited. * Copyright (c) 2022 Shenzhen Yunhorn Technology Co., Ltd. * All rights reserved. * * This software is licensed under terms that can be found in the LICENSE file * in the root directory of this software component. * If no LICENSE file comes with this software, it is provided AS-IS. * ****************************************************************************** */ /* USER CODE END Header */ /* Includes ------------------------------------------------------------------*/ #include #include #include "math.h" #include "main.h" /* USER CODE BEGIN Includes */ #include "acc_definitions_common.h" #include "acc_detector_presence.h" #include "acc_hal_definitions.h" #include "acc_hal_integration.h" #include "acc_integration.h" #include "acc_rss.h" #include "acc_version.h" #include "sys_app.h" #include "yunhorn_sts_prd_conf.h" #include "yunhorn_sts_sensors.h" /* #define DEFAULT_START_M (0.2f) #define DEFAULT_LENGTH_M (1.4f) #define DEFAULT_POWER_SAVE_MODE ACC_POWER_SAVE_MODE_SLEEP */ #define DEFAULT_UPDATE_RATE (10) #define DEFAULT_DETECTION_THRESHOLD (2.0f) #define DEFAULT_NBR_REMOVED_PC_2 (0) #define DEFAULT_PROFILE ACC_SERVICE_PROFILE_4 //ACC_SERVICE_PROFILE_4 #define DEFAULT_UPDATE_RATE (10) #define DEFAULT_POWER_SAVE_MODE ACC_POWER_SAVE_MODE_ACTIVE #define DEFAULT_SENSOR_ID (1) #define DEFAULT_START_M (0.8f) //(0.80f) //default 0.2 unit(meter) [1] #define DEFAULT_LENGTH_M (2.5f) // (2.0f)) //default 1.0 unit(meter) [2] #define DEFAULT_ZONE_LENGTH (0.4f) //default 0.4 unit(meter) #define DEFAULT_UPDATE_RATE_WAKEUP (2.0f) //default 80 unit(hz) #define DEFAULT_UPDATE_RATE_TRACKING (10.0f) //default 80 unit(hz) [7] #define DEFAULT_UPDATE_RATE_PRESENCE (65.0F) //(65.0f) //default 80 unit(hz) //#define DEFAULT_UPDATE_RATE_PRESENCE (80.0F) //(65.0f) //default 80 unit(hz) #define DEFAULT_HWAAS (63) //default 10 unit(hz) #define DEFAULT_THRESHOLD (1.2f) //default 1.5 level float [3] //acc_detector_presence_configuration_filter_parameters_t #define DEFAULT_INTER_FRAME_DEVIATION_TIME_CONST (0.5f) //default 0.5 unit(seconds) [6] #define DEFAULT_INTER_FRAME_FAST_CUTOFF (10.0f) //default 20.0 unit(hz) [8] #define DEFAULT_INTER_FRAME_SLOW_CUTOFF (0.01f) //(0.01f) 0.2 hz unit(hz) [9] #define DEFAULT_INTRA_FRAME_TIME_CONST (0) //default 0.0 unit(seconds) #define DEFAULT_INTRA_FRAME_WEIGHT (0) //default 0.6 #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] #define DEFAULT_NBR_REMOVED_PC (0) //default 0 int [10] #define DEFAULT_DOWNSAMPLING_FACTOR (2) //default 1 #define DEFAULT_RECEIVER_GAIN (0.65f) //default 0.9 gain mdB [4] //#define DEFAULT_RECEIVER_GAIN (0.65f) //default 0.9 gain mdB [4] #define DEFAULT_MOTION_DATASET_LEN (128) //MOTION DATASET/PATTERN COLLECTION #define DEFAULT_MOTION_FEATURE_LEN (10) //MOTION FEATURE IDENDIFIED #define DEFAULT_UNCONSCIOUS_THRESHOLD (250) #ifndef MIN # define MIN(a,b) ((a) < (b) ? (a) : (b)) #endif //sensor installation height segment from ground // sensor at ceiling sts_sensor_install_height_in_10cm // // // HS5= distance = sts_sensor_install_height_in_10cm - 5*DEFAULT_ZONE_LENGTH -> (26-4*5)*10cm // // HS4= distance = sts_sensor_install_height_in_10cm - 4*DEFAULT_ZONE_LENGTH -> (26-4*4)*10cm // // HS3= distance = sts_sensor_install_height_in_10cm - 3*DEFAULT_ZONE_LENGTH -> (26-4*3)*10cm // // HS2= distance = sts_sensor_install_height_in_10cm - 2*DEFAULT_ZONE_LENGTH -> (26-4*2)*10cm // // HS1= distance = sts_sensor_install_height_in_10cm - 1*DEFAULT_ZONE_LENGTH -> (26-4)*10cm // // HS0= ------ Ground -distance = sts_sensor_install_height_in_10cm, say 26*10cm #define HS0 (1<<0) // ground up 0*zone(0.4) #define HS1 (1<<1) // ground up 0*zone(0.4) #define HS2 (1<<2) // ground up 1*zone(0.4) #define HS3 (1<<3) #define HS5 (1<<4) extern volatile uint8_t sts_fall_detection_acc_threshold, sts_fall_detection_depth_threshold, sts_occupancy_overtime_threshold; 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 uint16_t 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); static void print_result(acc_detector_presence_result_t result); volatile uint8_t motion_detected_count=0; volatile uint8_t motion_in_hs_zone[12][10]={0}; //0.4*12=4.8meter high, past 10 measures volatile uint8_t detected_hs_zone=0;; volatile uint16_t motion_count=0, motion_feature_count=0; typedef struct { uint16_t presence_score; // 1000* uint16_t presence_distance; // 1000*, in mm } STS_presence_result_t; volatile uint8_t sts_rss_config_updated_flag = 0; static STS_presence_result_t sts_motion_dataset[DEFAULT_MOTION_DATASET_LEN]; static STS_PRESENCE_Motion_Featuer_t sts_motion_feature[DEFAULT_MOTION_FEATURE_LEN]; volatile uint8_t sts_fall_rising_detected_result = STS_PRESENCE_NORMAL; volatile uint8_t sts_fall_rising_detected_result_changed_flag =0; volatile uint8_t last_sts_fall_rising_detected_result= STS_PRESENCE_NORMAL; volatile float last_average_presence_distance; 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; /* USER CODE END Includes */ /* External variables ---------------------------------------------------------*/ /* USER CODE BEGIN EV */ /* USER CODE END EV */ /* Private typedef -----------------------------------------------------------*/ /* USER CODE BEGIN PTD */ /* USER CODE END PTD */ /* Private define ------------------------------------------------------------*/ /* USER CODE BEGIN PD */ /* USER CODE END PD */ /* Private macro -------------------------------------------------------------*/ /* USER CODE BEGIN PM */ /* USER CODE END PM */ /* Private variables ---------------------------------------------------------*/ /* USER CODE BEGIN PV */ /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------*/ /* USER CODE BEGIN PFP */ #if (defined(YUNHORN_STS_O7_ENABLED) && defined(USE_ACCONEER_A111)) #endif /* USER CODE END PFP */ /* Exported functions --------------------------------------------------------*/ /** * @brief Set default values in presence configuration * * @param[in] presence_configuration The presence configuration to set default values in */ static void set_default_configuration(acc_detector_presence_configuration_t presence_configuration) { acc_detector_presence_configuration_service_profile_set(presence_configuration, DEFAULT_PROFILE); acc_detector_presence_configuration_sensor_set(presence_configuration, DEFAULT_SENSOR_ID); 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_service_sparse_configuration_sweeps_per_frame_set(sparse_configuration, sweeps_per_frame); 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_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; // 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; // no effect if intra-frame-weight set to 0 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; // if detection toggles too often, increase the following, if too sluggish, decrease it instead 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_power_save_mode_set(presence_configuration, ACC_POWER_SAVE_MODE_ACTIVE); } /** * @brief Set default values in fall_rise detection configuration * * @param[in] fall_rise_configuration The fall rise configuration to set default values in */ static void set_default_fall_rise_configuration(acc_detector_presence_configuration_t presence_configuration) { acc_detector_presence_configuration_sensor_set(presence_configuration, DEFAULT_SENSOR_ID); acc_detector_presence_configuration_service_profile_set(presence_configuration, DEFAULT_PROFILE); 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_service_sparse_configuration_sweeps_per_frame_set(sparse_configuration, sweeps_per_frame); 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_filter_parameters_t filter = acc_detector_presence_configuration_filter_parameters_get(presence_configuration); // if intra-frame-weight set to 1.0, then the following inter-frame parameters have no effect filter.inter_frame_deviation_time_const = DEFAULT_INTER_FRAME_DEVIATION_TIME_CONST; filter.inter_frame_fast_cutoff = 10.0f; //DEFAULT_INTER_FRAME_FAST_CUTOFF; filter.inter_frame_slow_cutoff = 0.5f; //DEFAULT_INTER_FRAME_SLOW_CUTOFF; // For fast movement, decrease the time constant // filter.intra_frame_time_const = DEFAULT_INTRA_FRAME_TIME_CONST; filter.intra_frame_time_const = 0.2f; // filter.intra_frame_weight = DEFAULT_INTRA_FRAME_WEIGHT; // FOR fast movement tracking set intra-frame-weight to 1.0 filter.intra_frame_weight = 1.0f; // if detection toggles too often, increase the following, if too sluggish, decrease it instead 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_power_save_mode_set(presence_configuration, ACC_POWER_SAVE_MODE_ACTIVE); } static void sts_rss_set_current_configuration_full(acc_detector_presence_configuration_t presence_configuration) { acc_detector_presence_configuration_sensor_set(presence_configuration, DEFAULT_SENSOR_ID); // acc_service_profile_t presence_profile = sts_presence_rss_config.default_profile; // acc_detector_presence_configuration_service_profile_set(presence_configuration, presence_profile); acc_detector_presence_configuration_update_rate_set(presence_configuration, sts_presence_rss_config.default_update_rate_presence); //DEFAULT_UPDATE_RATE_2); acc_detector_presence_configuration_detection_threshold_set(presence_configuration, sts_presence_rss_config.default_threshold);//DEFAULT_DETECTION_THRESHOLD_2); acc_detector_presence_configuration_start_set(presence_configuration, sts_presence_rss_config.default_start_m); acc_detector_presence_configuration_length_set(presence_configuration, sts_presence_rss_config.default_length_m); //DEFAULT_LENGTH_M_2); acc_detector_presence_configuration_downsampling_factor_set(presence_configuration, sts_presence_rss_config.default_downsampling_factor); acc_detector_presence_configuration_receiver_gain_set(presence_configuration, sts_presence_rss_config.default_receiver_gain); acc_detector_presence_configuration_filter_parameters_t filter = acc_detector_presence_configuration_filter_parameters_get( presence_configuration); filter.inter_frame_deviation_time_const = sts_presence_rss_config.default_inter_frame_deviation_time_const; filter.inter_frame_fast_cutoff = sts_presence_rss_config.default_inter_frame_fast_cutoff; filter.inter_frame_slow_cutoff = sts_presence_rss_config.default_inter_frame_slow_cutoff; filter.intra_frame_time_const = sts_presence_rss_config.default_intra_frame_time_const; filter.intra_frame_weight = sts_presence_rss_config.default_intra_frame_weight; filter.output_time_const = sts_presence_rss_config.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, sts_presence_rss_config.default_nbr_removed_pc); acc_detector_presence_configuration_hw_accelerated_average_samples_set(presence_configuration, sts_presence_rss_config.default_hwaas); } static void sts_rss_set_current_configuration_simple(acc_detector_presence_configuration_t presence_configuration) { acc_detector_presence_configuration_sensor_set(presence_configuration, DEFAULT_SENSOR_ID); acc_detector_presence_configuration_update_rate_set(presence_configuration, DEFAULT_UPDATE_RATE_PRESENCE); acc_detector_presence_configuration_start_set(presence_configuration, sts_presence_rss_config.default_start_m); acc_detector_presence_configuration_length_set(presence_configuration, sts_presence_rss_config.default_length_m); //DEFAULT_LENGTH_M_2); acc_detector_presence_configuration_detection_threshold_set(presence_configuration, sts_presence_rss_config.default_threshold);//DEFAULT_DETECTION_THRESHOLD_2); acc_detector_presence_configuration_receiver_gain_set(presence_configuration, sts_presence_rss_config.default_receiver_gain); } static void print_result(acc_detector_presence_result_t result) { if (result.presence_detected) { //uint32_t detected_zone = (uint32_t)((float)(result.presence_distance - DEFAULT_START_M) / (float)DEFAULT_ZONE_LENGTH); // 2024-08-05 uint32_t detected_zone = (uint32_t)((float)(result.presence_distance) / (float)DEFAULT_ZONE_LENGTH); APP_LOG(TS_OFF, VLEVEL_H,"Motion in zone: %u, distance: %d, score: %d\n", (unsigned int)detected_zone, (int)(result.presence_distance * 1000.0f), (int)(result.presence_score * 1000.0f)); } else { APP_LOG(TS_OFF, VLEVEL_H,"No motion, score: %u\n", (int)(result.presence_score * 1000.0f)); } } /* * * OUTPUT: sts_rss_result = STS_PRESENCE_MOTION, STS_PRESENCE_NO_MOTION * sts_rss_result */ int sts_presence_rss_fall_rise_detection(void) { const acc_hal_t *hal = acc_hal_integration_get_implementation(); if (!acc_rss_activate(hal)) { APP_LOG(TS_OFF, VLEVEL_H,"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_H,"Failed to create configuration\n"); acc_rss_deactivate(); return EXIT_FAILURE; } switch (sts_rss_config_updated_flag) { 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; default: break; } 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) { APP_LOG(TS_OFF, VLEVEL_H,"Failed to create detector\n"); acc_detector_presence_configuration_destroy(&presence_configuration); acc_detector_presence_destroy(&handle); acc_rss_deactivate(); return EXIT_FAILURE; } // BEFORE MERGE FIRST AND SECOND HALF FALL RISE DETECTION if (!acc_detector_presence_activate(handle)) { APP_LOG(TS_OFF, VLEVEL_H, "Failed to activate detector \n"); return false; } bool success = true; const int iterations = (DEFAULT_UPDATE_RATE_PRESENCE+1); acc_detector_presence_result_t result; uint8_t average_result = 0; float average_distance =0.0f; float average_score =0.0f; //uint8_t k=0; uint16_t motion_in_zone[10]={0x0}; uint16_t detected_zone=0; for (uint8_t k=0;k<12;k++) motion_in_hs_zone[k][motion_detected_count]=0; UTIL_MEM_set_8(sts_motion_dataset, 0x0, sizeof(sts_motion_dataset)); motion_count =0; //memset((void*)motion_in_hs_zone, 0x0, 12*10); //past 10 times of detection with 5 zones from ground to ceiling //for (k=0; k<5; k++) {motion_in_zone[k]=0;} #if 1 for (int i = 0; i < (iterations/2); i++) { success = acc_detector_presence_get_next(handle, &result); if (!success) { APP_LOG(TS_OFF, VLEVEL_H,"acc_detector_presence_get_next() failed\n"); break; } //print_result(result); if (!result.data_saturated) { //APP_LOG(TS_OFF, VLEVEL_H,"\n%u ", i); //print_result(result); if (result.presence_detected) { //print_result(result); average_result++; average_distance += result.presence_distance; average_score += result.presence_score; //detected_zone = (uint16_t)((float)(result.presence_distance - DEFAULT_START_M) / (float)DEFAULT_ZONE_LENGTH); //2024-08-05 detected_zone = (uint16_t)((float)(result.presence_distance) / (float)DEFAULT_ZONE_LENGTH); motion_in_zone[detected_zone]++; // new add 2024-06-18 //detected_hs_zone = (uint16_t)((float)(sts_sensor_install_height/1000.0f - (result.presence_distance))/(float)DEFAULT_ZONE_LENGTH); //if (detected_hs_zone == 0) //APP_LOG(TS_OFF, VLEVEL_L, "\r\nPresence_Distance=%u \r\n", (int)result.presence_distance*1000.0); //APP_LOG(TS_OFF, VLEVEL_L, "\r\nHS_ZONE=%u", (int)detected_hs_zone); detected_hs_zone = 10 - detected_zone; motion_in_hs_zone[detected_hs_zone][(motion_detected_count)]++; sts_motion_dataset[motion_count].presence_distance = 1000*result.presence_distance; sts_motion_dataset[motion_count].presence_score = 1000*result.presence_score; motion_count++; } } //acc_integration_sleep_ms(1000 / DEFAULT_UPDATE_RATE_PRESENCE); // 15ms, DEFAULT_UPDATE_RATE); //acc_integration_sleep_ms(1); } APP_LOG(TS_OFF, VLEVEL_H, "\r\n First Half --- Motion Count = %u \r\n", motion_count); acc_detector_presence_deactivate(handle); #endif // ******** Second Half detection of fall down and rise up //if (sts_presence_fall_detection == TRUE) //{ set_default_fall_rise_configuration(presence_configuration); if (!acc_detector_presence_reconfigure(&handle, presence_configuration)) { APP_LOG(TS_OFF, VLEVEL_H,"Failed to reconfigure detector\n"); acc_detector_presence_configuration_destroy(&presence_configuration); acc_detector_presence_destroy(&handle); acc_rss_deactivate(); return EXIT_FAILURE; } if (!acc_detector_presence_activate(handle)) { APP_LOG(TS_OFF, VLEVEL_H, "Failed to activate detector \n"); return false; } acc_detector_presence_configuration_destroy(&presence_configuration); // set to full length of iteration for (int i = 0; i < (iterations/2); i++) { success = acc_detector_presence_get_next(handle, &result); if (!success) { APP_LOG(TS_OFF, VLEVEL_H,"acc_detector_presence_get_next() failed\n"); break; } if (!result.data_saturated) { //APP_LOG(TS_OFF, VLEVEL_H,"\n%u ", i); //print_result(result); if (result.presence_detected) { //print_result(result); average_result++; average_distance += result.presence_distance; average_score += result.presence_score; //detected_zone = (uint16_t)((float)(result.presence_distance - DEFAULT_START_M) / (float)DEFAULT_ZONE_LENGTH); // 2024-08-05 detected_zone = (uint16_t)((float)(result.presence_distance) / (float)DEFAULT_ZONE_LENGTH); motion_in_zone[detected_zone]++; // new add 2024-06-18 //detected_hs_zone = (uint16_t)((float)(sts_sensor_install_height/1000.0f - (result.presence_distance))/(float)DEFAULT_ZONE_LENGTH); //if (detected_hs_zone == 0) //APP_LOG(TS_OFF, VLEVEL_L, "\r\nPresence_Distance=%u \r\n", (int)result.presence_distance*1000.0); //APP_LOG(TS_OFF, VLEVEL_L, "\r\nHS_ZONE=%u", detected_hs_zone); detected_hs_zone = 10 - detected_zone; motion_in_hs_zone[detected_hs_zone][(motion_detected_count)]++; sts_motion_dataset[motion_count].presence_distance = 1000*result.presence_distance; sts_motion_dataset[motion_count].presence_score = 1000*result.presence_score; motion_count++; } } //acc_integration_sleep_ms(1000 / DEFAULT_UPDATE_RATE_PRESENCE); // 15 ms, DEFAULT_UPDATE_RATE); //acc_integration_sleep_ms(1); } 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); motion_count = motion_count%DEFAULT_MOTION_DATASET_LEN; // get all required number of motion data if ((sts_presence_fall_detection == TRUE)&& (motion_count>10)) STS_YunhornCheckStandardDeviation(); // RSS feature 1: Motion, No-motion process sts_rss_result = (average_result > 3)? 1: 0; APP_LOG(TS_OFF, VLEVEL_H, "\r\n MotionDetect Average result=%u : RSS result -- = %u \r\n", average_result, sts_rss_result); if (sts_rss_result) { LED1_ON; OnSensorRSS3AStateChanged(); } else { LED1_OFF; } // RSS feature 2: Fall Detection process /* TODO XXXX 2024-06-06 * Fall Detection settings */ if (sts_fall_rising_detected_result) { LED1_ON; OnSensorRSS3BStateChanged(); } else { LED1_OFF; } // RSS feature 3: No motion, or stay still , or unconscious process uint8_t thiscnt= motion_detected_count; if (motion_detected_count++ == 10) { motion_detected_count=0; } else { OnSensorRSS3CStateChanged(); } #ifndef MIN #define MIN( a, b ) ( ( ( a ) < ( b ) ) ? ( a ) : ( b ) ) #endif #ifndef MAX #define MAX( a, b ) ( ( ( a ) > ( b ) ) ? ( a ) : ( b ) ) #endif //#ifdef LOG_RSS APP_LOG(TS_OFF, VLEVEL_H,"\r\nSensor at Ceiling Height: %u mm\r\n",(uint16_t)sts_sensor_install_height); APP_LOG(TS_OFF, VLEVEL_H,"\r\n|Motion Distance Zone| ##### |Height cm|\r\n"); uint8_t kk = (uint8_t)(sts_sensor_install_height/400+1); #if 0 APP_LOG(TS_OFF, VLEVEL_L,"\r\n|-----------Ceiling-------Sensor---V-----|\r\n"); for (uint8_t k=0; k<=kk; k++) { if (motion_in_hs_zone[kk-k][thiscnt]>0) { APP_LOG(TS_OFF, VLEVEL_L,"\r\n| %2u | %4u | %4u |\r\n", kk-k, (uint8_t)motion_in_hs_zone[kk-k][thiscnt], (int)(kk-k+1)*40); } else { APP_LOG(TS_OFF, VLEVEL_L,"\r\n| %2u | | |\r\n", kk-k); } } APP_LOG(TS_OFF, VLEVEL_L,"\r\n|-----------Floor Ground-----------^-----|\r\n"); #endif //#endif average_distance = (1000.0f*average_distance)/average_result; // in meters average_score = (1000.0f*average_score)/average_result; sts_presence_rss_distance = average_distance; sts_presence_rss_score = average_score; if (average_score) //if (average_score !=0) //if (sts_rss_result) { #ifdef LOG_RSS APP_LOG(TS_OFF, VLEVEL_L,"\r\n######## Motion: %u Distance=%u mm, Score=%u Average_result=%u out of %u \r\n", (uint8_t)sts_rss_result,(int) average_distance, (int)(average_score), (int)average_result, (int)iterations); #endif #ifdef LOG_RSS for (uint8_t j=0; j<5; j++) { 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) average_distance, (int)(average_score), (int)average_result, (int)iterations); } #endif } bool deactivated = acc_detector_presence_deactivate(handle); acc_detector_presence_destroy(&handle); acc_rss_deactivate(); if (deactivated && success) { //APP_LOG(TS_OFF, VLEVEL_M,"Application finished OK\n"); return EXIT_SUCCESS; } return EXIT_FAILURE; } void STS_YunhornCheckStandardDeviation(void) { uint16_t i,j; // sts_sensor_install_height <--- average_presence_distance should be approaching this distance - 50cm uint32_t sum_presence_distance = 0, sum_presence_score=0; //presence score act as magnetic or amplitude of motion uint32_t average_presence_distance = 0, average_presence_score=0; uint32_t variance_presence_distance = 0, variance_presence_score=0; uint32_t standard_variance_presence_distance = 0, standard_variance_presence_score=0; // ROC -- rate of change uint32_t roc_distance[DEFAULT_MOTION_DATASET_LEN]={0}, sum_roc_distance=0, average_roc_distance=0, variance_roc_distance=0, standard_variance_roc_distance=0; uint32_t roc_acc[DEFAULT_MOTION_DATASET_LEN]={0}, sum_roc_acc=0.0f, average_roc_acc=0.0f, variance_roc_acc=0.0f, standard_variance_roc_acc=0.0f;; //act as speed of change at given time slot acc_integration_sleep_ms(1000 / DEFAULT_UPDATE_RATE_PRESENCE); uint8_t SAMPLE_DATASET_NUM = MIN(motion_count,DEFAULT_MOTION_DATASET_LEN ); APP_LOG(TS_OFF, VLEVEL_M, "\r\n Sample dataset for deviation process =%u \r\n",SAMPLE_DATASET_NUM); // initial state sts_fall_rising_detected_result = STS_PRESENCE_NORMAL; //SUM of samples for(i= 0; i< SAMPLE_DATASET_NUM; i++) { sum_presence_distance += (uint32_t)sts_motion_dataset[i].presence_distance; sum_presence_score += (uint32_t)sts_motion_dataset[i].presence_score; } // AVERAGE of all samples ( u (miu) ) average_presence_distance = ((uint32_t)sum_presence_distance/(uint32_t)(SAMPLE_DATASET_NUM)); average_presence_score = ((uint32_t)sum_presence_score/(uint32_t)(SAMPLE_DATASET_NUM)); // 1) VARIANCE for (j = 0; j < SAMPLE_DATASET_NUM; j++) { variance_presence_distance += (uint32_t)pow(sts_motion_dataset[j].presence_distance - average_presence_distance,2); variance_presence_score += (uint32_t)pow(sts_motion_dataset[j].presence_score - average_presence_score,2); } variance_presence_distance /= (uint32_t)(SAMPLE_DATASET_NUM-1); variance_presence_score /= (uint32_t)(SAMPLE_DATASET_NUM-1); //STANDARD VARIANCE sigma standard_variance_presence_distance = (uint32_t)sqrt(variance_presence_distance); standard_variance_presence_score = (uint32_t)sqrt(variance_presence_score); APP_LOG(TS_OFF, VLEVEL_M, "\r\n Standard Variance_Presence_Distance=%u \r\n Standard Variance Presence Score=%u\r\n", standard_variance_presence_distance, standard_variance_presence_score); // ROC distance // SUM for(i= 0; i< (SAMPLE_DATASET_NUM-1); i++) { roc_distance[i] = (uint32_t)(labs(sts_motion_dataset[i+1].presence_distance - sts_motion_dataset[i].presence_distance)); sum_roc_distance += ((uint32_t)roc_distance[i]); } average_roc_distance = (uint32_t)sum_roc_distance/(SAMPLE_DATASET_NUM-1); for (j = 0; j < (SAMPLE_DATASET_NUM-1); j++) { variance_roc_distance += (uint32_t)(pow(roc_distance[j] - average_roc_distance,2)); } // average variance_roc_distance /= (uint32_t)(SAMPLE_DATASET_NUM); //???? standard_variance_roc_distance = (uint32_t)sqrt((uint32_t)variance_roc_distance); // ROC Acceleration for(i= 0; i< (SAMPLE_DATASET_NUM-2); i++) { roc_acc[i] = (uint32_t)(labs((uint32_t)roc_distance[i+1] - (uint32_t)roc_distance[i])); sum_roc_acc += ((uint32_t)roc_acc[i]); } average_roc_acc = (uint32_t)sum_roc_acc/(SAMPLE_DATASET_NUM-2); for (j = 0; j < (SAMPLE_DATASET_NUM-2); j++) { variance_roc_acc += (uint32_t)pow((uint32_t)((uint32_t)roc_acc[j] - (uint32_t)average_roc_acc),2); } variance_roc_acc /= (uint32_t)(SAMPLE_DATASET_NUM-1); standard_variance_roc_acc = (uint32_t)sqrt((uint32_t)variance_roc_acc); //Normallize to m/s --- * DEFAULT_MOTION_DATASET_LEN for One single second #if 0 average_roc_distance *= (uint32_t)DEFAULT_MOTION_DATASET_LEN; variance_roc_distance *= (uint32_t)DEFAULT_MOTION_DATASET_LEN; standard_variance_roc_distance *= (uint32_t)DEFAULT_MOTION_DATASET_LEN; average_roc_acc *= (uint32_t)DEFAULT_MOTION_DATASET_LEN; variance_roc_acc *= (uint32_t)DEFAULT_MOTION_DATASET_LEN; standard_variance_roc_acc *= (uint32_t)DEFAULT_MOTION_DATASET_LEN; #endif // print result //#ifdef LOG_RSS APP_LOG(TS_OFF, VLEVEL_M, "\r\n---Sensor Install Height=%6u-----\r\n-------------Distance Average =%6u; Standard_Variance =%6u \r\n", (int)sts_sensor_install_height, (int)(average_presence_distance), (int)(standard_variance_presence_distance)); //#endif //#ifdef LOG_RSS APP_LOG(TS_OFF, VLEVEL_M, "\r\n\n-------------Motion Average =%6u; Standard_Variance =%6u \r\n", (int)(average_presence_score), (int)(standard_variance_presence_score)); //#endif //#ifdef LOG_RSS APP_LOG(TS_OFF, VLEVEL_M, "\r\n-------------ROC Dist Average =%6u; Standard_Variance =%6u \r\n", (int)(average_roc_distance), (int)(standard_variance_roc_distance)); APP_LOG(TS_OFF, VLEVEL_M, "\r\n-------------ROC ACC Average =%6u; Standard_Variance =%6u \r\n", (int)(average_roc_acc), (int)(standard_variance_roc_acc)); //sts_fall_rising_pattern_factor1 = (int)(standard_variance_roc_distance); sts_fall_rising_pattern_factor1 = (int)(average_roc_distance); sts_fall_rising_pattern_factor2 = (int)(fabs(average_presence_distance - last_average_presence_distance)); sts_fall_rising_pattern_factor3 = (int)(standard_variance_presence_score); APP_LOG(TS_OFF, VLEVEL_L,"\r\nAvg-Dist =%6u, Last_AVG-Dist =%6u \r\n", (int)(average_presence_distance), (int)(last_average_presence_distance)); APP_LOG(TS_OFF, VLEVEL_L,"\r\nStandard Variance Presence_score=%u \r\n", (int)(standard_variance_presence_score)); APP_LOG(TS_OFF, VLEVEL_M, "\r\nThreshold 1: Acc = %6u ---- Measure 1 = %6u ---- \r\n", (int)(sts_fall_detection_acc_threshold), (int)(sts_fall_rising_pattern_factor1/10)); APP_LOG(TS_OFF, VLEVEL_M, "\r\nThreshold 2: Dis = %6u cm --- Measure 2 = %6u cm ---- \r\n", (int)(sts_fall_detection_depth_threshold), (int)(sts_fall_rising_pattern_factor2/10)); //#endif // ******************************************* // *********** detection situation suggestion if ((standard_variance_presence_score > DEFAULT_UNCONSCIOUS_THRESHOLD) && (standard_variance_presence_score <= sts_unconscious_threshold)) { sts_fall_rising_detected_result = STS_PRESENCE_STAYSTILL; //sts_fall_rising_detected_result = STS_PRESENCE_NO_MOVEMENT; } // // Considering factor #1 --- fall down speed, say > 0.3g or in less than 0.3 seconds // if ( sts_fall_rising_pattern_factor1 > (uint16_t)sts_fall_detection_acc_threshold) { // if ((average_presence_distance > (sts_motion_feature[motion_feature_count].p_dist_avg + sts_motion_feature[motion_feature_count].p_dist_v))) // if ((average_presence_distance > (sts_motion_feature[motion_feature_count].p_dist_avg + sts_motion_feature[motion_feature_count].p_dist_v))) // if ((average_presence_distance > (last_average_presence_distance + sts_fall_detection_depth_threshold*0.1))) // // Considering factor #2 --- fall down depth, say > 30 cm 40 cm in given factor 1 time unit // if ((sts_fall_rising_pattern_factor2 > sts_fall_detection_depth_threshold ) && (average_presence_distance > 1000*DEFAULT_START_M)) { sts_fall_rising_detected_result = STS_PRESENCE_FALL; //last_sts_fall_rising_detected_result = sts_fall_rising_detected_result; } motion_feature_count ++; if (motion_feature_count == DEFAULT_MOTION_FEATURE_LEN) motion_feature_count = 0; sts_motion_feature[motion_feature_count].p_dist_avg = average_presence_distance; sts_motion_feature[motion_feature_count].p_dist_v = variance_presence_distance; sts_motion_feature[motion_feature_count].p_dist_standard = standard_variance_presence_distance; sts_motion_feature[motion_feature_count].m_score_avg = standard_variance_presence_score; sts_motion_feature[motion_feature_count].roc_avg = average_roc_distance; sts_motion_feature[motion_feature_count].roc_standard = standard_variance_roc_distance; sts_motion_feature[motion_feature_count].fall_rising = sts_fall_rising_detected_result; sts_roc_acc_standard_variance = (uint8_t) standard_variance_roc_acc; if ( sts_fall_rising_detected_result == STS_PRESENCE_FALL ) { APP_LOG(TS_OFF, VLEVEL_M, "\r\n\n\n >>>>>>>>>>>>>>>>> Suspicious Object FALL DOWN detected \r\n"); } //if (average_presence_distance > DEFAULT_START_M) { // last_average_presence_distance = average_presence_distance; } } if ((last_sts_fall_rising_detected_result == STS_PRESENCE_FALL)) { if ((average_presence_distance < (sts_motion_feature[motion_feature_count].p_dist_avg + sts_motion_feature[motion_feature_count].p_dist_standard))) { sts_fall_rising_detected_result = STS_PRESENCE_RISING; if ( sts_fall_rising_detected_result == STS_PRESENCE_RISING ) { APP_LOG(TS_OFF, VLEVEL_M, "\r\n\n\n >>>>>>>>>>>>>>>>> Suspicious Object RISING UP detected \r\n"); } last_sts_fall_rising_detected_result = sts_fall_rising_detected_result; } else if ((average_presence_distance + sts_motion_feature[motion_feature_count].p_dist_standard) > last_average_presence_distance) { sts_fall_rising_detected_result = STS_PRESENCE_LAYDOWN; } } last_sts_fall_rising_detected_result = sts_fall_rising_detected_result; last_average_presence_distance = average_presence_distance; } /* USER CODE BEGIN EF */ /* USER CODE END EF */ /* Private Functions Definition -----------------------------------------------*/ /* USER CODE BEGIN PrFD */ /* USER CODE END PrFD */