From 31d07717bb04e4aff97b64cf6902f9b2b79203c4 Mon Sep 17 00:00:00 2001 From: YunHorn Technology Date: Tue, 18 Jun 2024 17:08:25 +0800 Subject: [PATCH] fall detection logic, motion zone 0/1 from ground detection --- Core/Inc/yunhorn_sts_sensors.h | 2 + Core/Src/yunhorn_sts_presence_rss.c | 29 ++++++++--- Core/Src/yunhorn_sts_process.c | 75 +++++++++++++++++++++++++++-- LoRaWAN/App/lora_app.c | 6 +-- 4 files changed, 97 insertions(+), 15 deletions(-) diff --git a/Core/Inc/yunhorn_sts_sensors.h b/Core/Inc/yunhorn_sts_sensors.h index 9894799..8a69fe3 100644 --- a/Core/Inc/yunhorn_sts_sensors.h +++ b/Core/Inc/yunhorn_sts_sensors.h @@ -653,6 +653,8 @@ void STS_YunhornSTSEventP8_Process(void); void OnSensor1StateChanged(void); void OnSensor2StateChanged(void); void OnSensor3StateChanged(void); +void OnSensor3AStateChanged(void); +void OnSensor3BStateChanged(void); void OnSensor4StateChanged(void); void STS_SENSOR_Power_ON(uint8_t cnt); diff --git a/Core/Src/yunhorn_sts_presence_rss.c b/Core/Src/yunhorn_sts_presence_rss.c index 8f461e4..cd91901 100644 --- a/Core/Src/yunhorn_sts_presence_rss.c +++ b/Core/Src/yunhorn_sts_presence_rss.c @@ -396,8 +396,10 @@ int sts_presence_rss_fall_rise_detection(void) //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; - memset(motion_in_hs_zone, 0x0, 12*10); + //memset((void*)motion_in_hs_zone, 0x0, 12*10); //past 10 times of detection with 5 zones from ground to ceiling @@ -495,9 +497,10 @@ int sts_presence_rss_fall_rise_detection(void) detected_zone = (uint16_t)((float)(result.presence_distance - DEFAULT_START_M) / (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); + detected_hs_zone = (uint16_t)((float)(sts_sensor_install_height/1000.0f - (result.presence_distance))/(float)DEFAULT_ZONE_LENGTH); + //APP_LOG(TS_OFF, VLEVEL_L, "\r\nHS_ZONE=%u", detected_hs_zone); - //motion_in_hs_zone[detected_hs_zone][(motion_detected_count)]++; + motion_in_hs_zone[detected_hs_zone][(motion_detected_count)]++; } @@ -525,12 +528,14 @@ int sts_presence_rss_fall_rise_detection(void) if (motion_detected_count++ == 10) { motion_detected_count=0; + } else { + OnSensor3CStateChanged(); } sts_rss_result = (average_result > 3)? 1: 0; if (sts_rss_result) { LED1_ON; - OnSensor3StateChanged(); + OnSensor3AStateChanged(); } else { LED1_OFF; @@ -544,7 +549,17 @@ int sts_presence_rss_fall_rise_detection(void) * */ -#ifdef LOG_RSS + if (sts_fall_rising_detected_result) { + LED1_ON; + OnSensor3BStateChanged(); + } else { + LED1_OFF; + + } + + + +#ifndef LOG_RSS APP_LOG(TS_OFF, VLEVEL_L,"\r\nSensor at Ceiling Height: %4u mm\r\n",(int)sts_sensor_install_height); for (uint8_t k=0; k<12; k++) { @@ -596,11 +611,11 @@ int sts_presence_rss_fall_rise_detection(void) void STS_YunhornCheckStandardDeviation(void) { 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 sum_presence_distance = 0, sum_presence_score=0; //presence score act as magnetic or amplitude of motion float average_presence_distance = 0, average_presence_score=0; float variance_presence_distance = 0, variance_presence_score=0; float standard_variance_presence_distance = 0, standard_variance_presence_score=0; - + // ROC -- rate of change float roc_distance[DEFAULT_MOTION_DATASET_LEN]={0}, sum_roc_distance=0, average_roc_distance=0, variance_roc_distance=0, standard_variance_roc_distance=0; float 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); diff --git a/Core/Src/yunhorn_sts_process.c b/Core/Src/yunhorn_sts_process.c index 6f378c9..0c626e6 100644 --- a/Core/Src/yunhorn_sts_process.c +++ b/Core/Src/yunhorn_sts_process.c @@ -902,13 +902,78 @@ void OnSensor3StateChanged(void) last_sts_rss_time_stamp = sensor_event_time.Seconds; - if (sts_fall_rising_detected_result == STS_PRESENCE_FALL) - { - sts_o7_sensorData.event_sensor3_fall_start_time = sensor_event_time.Seconds; - sts_o7_sensorData.event_sensor3_fall_duration = 0; - } + } +/* motion sensor RSS ON-OFF */ +void OnSensor3AStateChanged(void) +{ + SysTime_t sensor_event_time = SysTimeGetMcuTime(); +#if 0 + if ((sensor_event_time.Seconds - last_sts_rss_time_stamp) < 3 ) //less than 3 seconds ... return for flipping filter + { + return ; + } +#endif + if (sts_rss_result == STS_RESULT_MOTION) + { + sts_o7_sensorData.event_sensor3_motion_start_time = sensor_event_time.Seconds; + sts_o7_sensorData.event_sensor3_motion_duration = 0; + } else if (sts_rss_result == STS_RESULT_NO_MOTION) + { + sts_o7_sensorData.event_sensor3_motion_stop_time = sensor_event_time.Seconds; + //sts_o7_sensorData.event_sensor3_motion_duration = 0; + } + + + last_sts_rss_time_stamp = sensor_event_time.Seconds; + +} + +/* motion sensor RSS ON-OFF */ +void OnSensor3BStateChanged(void) +{ + SysTime_t sensor_event_time = SysTimeGetMcuTime(); +#if 0 + if ((sensor_event_time.Seconds - last_sts_rss_time_stamp) < 3 ) //less than 3 seconds ... return for flipping filter + { + return ; + } +#endif + switch (sts_fall_rising_detected_result) + { + case STS_PRESENCE_NONE: + sts_o7_sensorData.event_sensor3_motion_stop_time = sensor_event_time.Seconds; + break; + + case STS_PRESENCE_FALL: + if (motion_in_hs_zone[0][motion_detected_count]) + sts_o7_sensorData.event_sensor3_fall_start_time = sensor_event_time.Seconds; + sts_o7_sensorData.event_sensor3_fall_duration = 0; + break; + + case STS_PRESENCE_RISING: + + break; + + case STS_PRESENCE_LAYDOWN: + case STS_PRESENCE_UNCONSCIOUS: + sts_o7_sensorData.fall_laydown_duration = 0; + break; + case STS_PRESENCE_STAYSTILL: + + break; + default: + break; + + } + + last_sts_rss_time_stamp = sensor_event_time.Seconds; + + +} + + void OnSensor4StateChanged(void) { #if 0 diff --git a/LoRaWAN/App/lora_app.c b/LoRaWAN/App/lora_app.c index 9b42411..06528d6 100644 --- a/LoRaWAN/App/lora_app.c +++ b/LoRaWAN/App/lora_app.c @@ -1118,7 +1118,7 @@ static void OnYunhornSTSDurationCheckTimerEvent(void *context) SysTime_t current_time = SysTimeGetMcuTime(); - if (sts_hall1_read==STS_Status_Door_Close) + if (STS_Status_Door_Close==sts_hall1_read) sts_o7_sensorData.event_sensor1_duration = current_time.Seconds - sts_o7_sensorData.event_sensor1_start_time; if (sts_o7_sensorData.event_sensor1_duration > sts_occupancy_overtime_threshold_in_10min*600) { @@ -1127,7 +1127,7 @@ static void OnYunhornSTSDurationCheckTimerEvent(void *context) } // to be defiend later for SOS threshold TODO XXXX - if (sts_hall2_read==STS_Status_SOS_Pushdown) + if (STS_Status_SOS_Pushdown==sts_hall2_read) sts_o7_sensorData.event_sensor2_duration = current_time.Seconds - sts_o7_sensorData.event_sensor2_start_time; if (sts_o7_sensorData.event_sensor2_duration > sts_occupancy_overtime_threshold_in_10min*600) { @@ -1520,7 +1520,7 @@ static void OnYunhornSTSOORSSWakeUpTimerEvent(void *context) { UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_YunhornSTSEventP2), CFG_SEQ_Prio_0); - APP_LOG(TS_OFF,VLEVEL_L,"\r\n RSS result changed flag=%d \r\n", sts_rss_result_changed_flag); + //APP_LOG(TS_OFF,VLEVEL_L,"\r\n RSS result changed flag=%d \r\n", sts_rss_result_changed_flag); if ((STS_LoRa_WAN_Joined != 0)&&(sts_rss_result_changed_flag==1)) { sts_rss_result_changed_flag = 0;