revised for fall detection rss cfg

This commit is contained in:
Yunhorn 2024-08-16 21:03:36 +08:00
parent 3ef3d81e8f
commit 1c82221611
4 changed files with 139 additions and 91 deletions

View File

@ -90,8 +90,9 @@ enum RSS_CFG_order{
#if defined(STS_O7)||defined(STS_O6)
enum sts_rss_config_t {
STS_RSS_CONFIG_DEFAULT=0,
enum sts_rss_config_update_t {
STS_RSS_CONFIG_NON=0,
STS_RSS_CONFIG_DEFAULT,
STS_RSS_CONFIG_SIMPLE,
STS_RSS_CONFIG_FULL,
STS_RSS_CONFIG_FALL_DETECTION
@ -212,6 +213,9 @@ typedef struct STS_OO_SensorStatusDataTypeDef
uint32_t event_sensor3_fall_stop_time;
uint32_t event_sensor3_fall_duration;
uint16_t event_sensor3_fall_distance;
uint16_t event_sensor3_fall_motionscore;
uint32_t event_sensor3_no_movement_start_time;
uint32_t event_sensor3_no_movement_stop_time;
uint32_t event_sensor3_no_movement_duration;

View File

@ -35,6 +35,14 @@
#include "sys_app.h"
#include "yunhorn_sts_prd_conf.h"
#include "yunhorn_sts_sensors.h"
#ifndef MIN
#define MIN( a, b ) ( ( ( a ) < ( b ) ) ? ( a ) : ( b ) )
#endif
#ifndef MAX
#define MAX( a, b ) ( ( ( a ) > ( b ) ) ? ( a ) : ( b ) )
#endif
/*
#define DEFAULT_START_M (0.2f)
#define DEFAULT_LENGTH_M (1.4f)
@ -113,7 +121,7 @@ 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;
extern volatile uint8_t sts_work_mode;
extern volatile STS_OO_SensorStatusDataTypeDef sts_o7_sensorData;
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);
@ -347,6 +355,10 @@ int sts_presence_rss_fall_rise_detection(void)
switch (sts_rss_config_updated_flag)
{
case STS_RSS_CONFIG_NON:
APP_LOG(TS_OFF, VLEVEL_M,"\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_M,"\r\n##### YUNHORN STS *** Default *** cfg applied\n");
@ -384,7 +396,7 @@ int sts_presence_rss_fall_rise_detection(void)
APP_LOG(TS_OFF, VLEVEL_H, "Failed to activate detector \n");
return false;
}
bool deactivated = false;
bool success = true;
const int iterations = (DEFAULT_UPDATE_RATE_PRESENCE+1);
acc_detector_presence_result_t result;
@ -407,7 +419,7 @@ int sts_presence_rss_fall_rise_detection(void)
//for (k=0; k<5; k++) {motion_in_zone[k]=0;}
if ((sts_work_mode == STS_DUAL_MODE)||(sts_work_mode == STS_RSS_MODE))
{
for (int i = 0; i < (iterations/2); i++)
for (int i = 0; i < (iterations); i++)
{
success = acc_detector_presence_get_next(handle, &result);
if (!success)
@ -448,12 +460,17 @@ 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(1000 / DEFAULT_UPDATE_RATE_PRESENCE); // 15ms, DEFAULT_UPDATE_RATE);
//acc_integration_sleep_ms(1);
}
deactivated = acc_detector_presence_deactivate(handle);
acc_detector_presence_destroy(&handle);
acc_rss_deactivate();
APP_LOG(TS_OFF, VLEVEL_H, "\r\n First Half --- Motion Count = %u \r\n", motion_count);
acc_detector_presence_deactivate(handle);
//acc_detector_presence_deactivate(handle);
}
else if ((sts_work_mode == STS_UNI_MODE))
{
@ -474,15 +491,19 @@ int sts_presence_rss_fall_rise_detection(void)
return EXIT_FAILURE;
}
#endif
// activated already
#if 0
if (!acc_detector_presence_activate(handle))
{
APP_LOG(TS_OFF, VLEVEL_H, "Failed to activate detector \n");
return false;
}
#endif
acc_detector_presence_configuration_destroy(&presence_configuration);
// set to full length of iteration
for (int i = 0; i < (iterations/2); i++)
for (int i = 0; i < (iterations); i++)
{
success = acc_detector_presence_get_next(handle, &result);
if (!success)
@ -524,25 +545,46 @@ int sts_presence_rss_fall_rise_detection(void)
}
//acc_integration_sleep_ms(1000 / DEFAULT_UPDATE_RATE_PRESENCE); // 15 ms, DEFAULT_UPDATE_RATE);
acc_integration_sleep_ms(1000 / DEFAULT_UPDATE_RATE_PRESENCE); // 15 ms, DEFAULT_UPDATE_RATE);
//acc_integration_sleep_ms(1);
}
deactivated = acc_detector_presence_deactivate(handle);
acc_detector_presence_destroy(&handle);
acc_rss_deactivate();
APP_LOG(TS_OFF, VLEVEL_H, "\r\n Second Half --- Motion Count Sum to = %u \r\n", motion_count);
APP_LOG(TS_OFF, VLEVEL_M, "\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
//sts_fall_rising_detected_result = STS_PRESENCE_NORMAL;
if ((sts_presence_fall_detection == TRUE)&& (motion_count>10))
if ((sts_presence_fall_detection == TRUE)&& (motion_count> (DEFAULT_MOTION_DATASET_LEN/2)))
{
// major fall/rise/laydown/unconscious suggestion process
STS_YunhornCheckStandardDeviation();
}
}
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;
sts_rss_result = (average_result > (DEFAULT_UPDATE_RATE_PRESENCE/2))? 1: 0;
//if (average_result) //if (average_score !=0) //if (sts_rss_result)
{
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) sts_presence_rss_distance, (int)(sts_presence_rss_score), (int)average_result, (int)iterations);
}
// 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;
@ -575,57 +617,19 @@ int sts_presence_rss_fall_rise_detection(void)
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);
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);
}
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
#ifdef LOG_RSS
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)
{
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);
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)
{
@ -737,6 +741,10 @@ void STS_YunhornCheckStandardDeviation(void)
//#ifdef LOG_RSS
APP_LOG(TS_OFF, VLEVEL_M, "\r\n\n-------------Motion Average (score)=%04u; Standard_Variance (score)=%04u \r\n",
(int)(average_presence_score), (int)(standard_variance_presence_score));
sts_o7_sensorData.event_sensor3_fall_distance =(uint16_t)average_presence_distance;
sts_o7_sensorData.event_sensor3_fall_motionscore =(uint16_t)average_presence_score;
//#endif
//#ifdef LOG_RSS
APP_LOG(TS_OFF, VLEVEL_M, "\r\n-------------ROC Dist Average (mm/s) =%6u; Standard_Variance =%6u (mm/s)\r\n",

View File

@ -67,7 +67,7 @@ volatile uint8_t sts_soap_level_state;
volatile STS_OO_SensorStatusDataTypeDef sts_o7_sensorData;
volatile uint16_t sts_distance_rss_distance=0, sts_sensor_install_height=0;//in mm
extern volatile float sts_presence_rss_distance, sts_presence_rss_score;
volatile uint8_t sts_rss_config_updated_flag = 0;
volatile uint8_t sts_rss_config_updated_flag = STS_RSS_CONFIG_NON;
extern volatile uint8_t sts_occupancy_overtime_state;
extern volatile distance_measure_cfg_t distance_cfg;
extern volatile uint16_t sts_fall_rising_pattern_factor1;
@ -273,9 +273,20 @@ 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 (sts_presence_fall_detection == FALSE) { // otherwise, has been set to full/simple/etc.
switch (sts_work_mode) {
case STS_RSS_MODE:
sts_rss_config_updated_flag = STS_RSS_CONFIG_DEFAULT;
break;
case STS_DUAL_MODE:
sts_rss_config_updated_flag = STS_RSS_CONFIG_DEFAULT;
break;
case STS_UNI_MODE:
sts_rss_config_updated_flag = STS_RSS_CONFIG_FALL_DETECTION;
break;
default:
break;
}
sts_presence_rss_fall_rise_detection();
sts_rss_result_changed_flag = (sts_rss_result == last_sts_rss_result)? 0:1;
@ -750,6 +761,9 @@ void STS_PRESENCE_SENSOR_Init_Send_Data(void)
sts_o7_sensorData.event_sensor3_fall_stop_time_stamp = 0x0;
sts_o7_sensorData.event_sensor4_start_time = 0x0;
sts_o7_sensorData.event_sensor4_duration = 0x0;
sts_o7_sensorData.event_sensor3_fall_distance = 0x0;
sts_o7_sensorData.event_sensor3_fall_motionscore = 0x0;
sts_o7_sensorData.battery_Pct = 99; // 99% as init value
sts_o7_sensorData.dutycycletimelevel = 1;
@ -760,6 +774,8 @@ void STS_PRESENCE_SENSOR_Prepare_Send_Data(STS_OO_SensorStatusDataTypeDef *senso
{
sensor_data->lamp_bar_color = sts_lamp_bar_color;
sensor_data->workmode = sts_work_mode;
// #1 for HALL_ELEMENT, DOOR CONTACT, SENSOR STATE PROCESS
sensor_data->state_sensor1_on_off = HALL1_STATE;//sts_hall1_read; //sts_hsts_reed_hall_result; // sts_hall1_read
sensor_data->state_sensor2_on_off = HALL2_STATE;//sts_hall2_read; //sts_emergency_button_pushed; //sts_hall2_read
sensor_data->state_sensor3_on_off = sts_rss_result;
@ -778,17 +794,18 @@ void STS_PRESENCE_SENSOR_Prepare_Send_Data(STS_OO_SensorStatusDataTypeDef *senso
sts_o7_sensorData.event_sensor2_stop_timestamp = 0;
}
// #2 for MOTION RELATED SENSOR STATE PROCESS, OCCUPY/VACANT/OVERSTAY/LONGSTAY/UNCONSCIOUS
if (sts_rss_result == STS_RESULT_MOTION)
{
sensor_data->rss_presence_distance = (uint16_t)(sts_presence_rss_distance)&0xFFFF;
sensor_data->rss_presence_score = (uint16_t)(sts_presence_rss_score)&0xFFFF;
sensor_data->rss_presence_distance = (uint16_t)(sts_presence_rss_distance);
sensor_data->rss_presence_score = (uint16_t)(sts_presence_rss_score);
// uint8_t sts_unconscious_state;
// uint16_t sts_unconscious_threshold, sts_unconscious_threshold_duration;
} else {
sensor_data->rss_presence_distance = 0x0;
sensor_data->rss_presence_score = 0x0;
}
//else {
// sensor_data->rss_presence_distance = 0x0;
// sensor_data->rss_presence_score = 0x0;
// }
// no_movement or unconscious duration
sensor_data->unconscious_state = ((sts_fall_rising_detected_result == STS_PRESENCE_UNCONSCIOUS) ||
@ -804,7 +821,6 @@ void STS_PRESENCE_SENSOR_Prepare_Send_Data(STS_OO_SensorStatusDataTypeDef *senso
sensor_data->unconscious_duration, sts_motionless_duration_threshold_in_min*60);
}
// Over stay, long stay duration and state
sensor_data->over_stay_state = sts_o7_sensorData.over_stay_state;
sensor_data->over_stay_duration = sts_o7_sensorData.event_sensor1_duration;
@ -815,9 +831,10 @@ void STS_PRESENCE_SENSOR_Prepare_Send_Data(STS_OO_SensorStatusDataTypeDef *senso
sts_occupancy_overtime_threshold_in_10min*60);
}
// Fall detection and duration, confirmation duration
// #3 for FALL DETECTION, FALL, RISING, LAYDOWN DURATION CONFIRMATION STATE PROCESS
sensor_data->fall_state = sts_fall_rising_detected_result;
if ((sts_fall_rising_detected_result == STS_PRESENCE_FALL)||
(sts_fall_rising_detected_result == STS_PRESENCE_RISING)||
(sts_fall_rising_detected_result == STS_PRESENCE_LAYDOWN))
{
APP_LOG(TS_OFF, VLEVEL_M, "\r\n......FALL RISING DETECTION RESULT: %25s \r\n",
@ -826,7 +843,8 @@ void STS_PRESENCE_SENSOR_Prepare_Send_Data(STS_OO_SensorStatusDataTypeDef *senso
sensor_data->fall_gravity = (uint8_t)sts_roc_acc_standard_variance;
sensor_data->event_sensor3_fall_start_time_stamp = sts_o7_sensorData.event_sensor3_fall_start_time_stamp;
sensor_data->event_sensor3_fall_stop_time_stamp = sts_o7_sensorData.event_sensor3_fall_stop_time_stamp;
sensor_data->event_sensor3_fall_distance = (uint16_t)(sts_o7_sensorData.event_sensor3_fall_distance);
sensor_data->event_sensor3_fall_motionscore = (uint16_t)(sts_o7_sensorData.event_sensor3_fall_motionscore);
} else {
sensor_data->fall_speed = 0;
sensor_data->fall_gravity = 0;
@ -834,9 +852,10 @@ void STS_PRESENCE_SENSOR_Prepare_Send_Data(STS_OO_SensorStatusDataTypeDef *senso
sensor_data->event_sensor3_fall_stop_time_stamp =0;
}
// For occupancy over time process
//SysTime_t occupy_check_time = SysTimeGetMcuTime();
#if 0
// TODO XXXX
// FOR ALARM MUTE AND RESET
if ((sts_occupancy_overtime_threshold_in_10min != 0) && (event_start_time !=0))
{
if (sts_occupancy_overtime_state == 1U) {
@ -923,7 +942,7 @@ void STS_PRESENCE_SENSOR_RSS_Init(void)
{
APP_LOG(TS_ON, VLEVEL_H, "##### YunHorn SmarToilets(r) MEMS RSS Initializing \r\n");
PME_ON;
// PME_ON;
//if ((sts_distance_rss_distance==0)&&(sts_sensor_install_height==0))
{
@ -944,9 +963,21 @@ void STS_PRESENCE_SENSOR_RSS_Init(void)
STS_PRESENCE_SENSOR_NVM_CFG();
switch (sts_work_mode)
{
case STS_RSS_MODE:
sts_rss_config_updated_flag = STS_RSS_CONFIG_DEFAULT;
break;
case STS_DUAL_MODE:
sts_rss_config_updated_flag = STS_RSS_CONFIG_DEFAULT;
break;
case STS_UNI_MODE:
sts_rss_config_updated_flag = STS_RSS_CONFIG_FALL_DETECTION;
break;
default:
break;
}
}
@ -992,7 +1023,7 @@ void STS_PRESENCE_SENSOR_Function_Test_Process(uint8_t *self_test_result, uint8_
}
HAL_Delay(1000);
memcpy(self_test_result, bring_up_result, 10);
UTIL_MEM_cpy_8(self_test_result, bring_up_result, 10);
}

View File

@ -74,7 +74,7 @@ extern volatile uint8_t sts_fall_rising_detected_result, sts_fall_rising_detecte
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=TRUE;
volatile uint8_t sts_presence_fall_detection=FALSE;
volatile uint32_t SamplingPeriodicity = 3000; //unit ms
volatile uint32_t HeartBeatPeriodicity = 120000; //unit ms
volatile uint8_t STS_LoRa_WAN_Joined = 0;
@ -999,10 +999,10 @@ static void SendTxData(void)
AppData.Buffer[i++] = (uint8_t)(sensorData.state_sensor4_on_off)&0xff; //06 Sensor head #4 status ALARM MUTE
AppData.Buffer[i++] = (uint8_t)(sensorData.state_sensor5_on_off)&0xff; //07 Sensor head #5 status ALARM RESET
AppData.Buffer[i++] = (uint8_t)(sensorData.rss_presence_distance>>8)&0xff; //08 MSB distance
AppData.Buffer[i++] = (uint8_t)(sensorData.rss_presence_distance)&0xff; //09 LSB distance
AppData.Buffer[i++] = (uint8_t)(sensorData.rss_presence_score>>8)&0xff; //10 MSB score
AppData.Buffer[i++] = (uint8_t)(sensorData.rss_presence_score)&0xff; //11 LSB score
AppData.Buffer[i++] = (uint8_t)(sensorData.event_sensor3_fall_distance>>8)&0xff; //08 MSB distance
AppData.Buffer[i++] = (uint8_t)(sensorData.event_sensor3_fall_distance)&0xff; //09 LSB distance
AppData.Buffer[i++] = (uint8_t)(sensorData.event_sensor3_fall_motionscore>>8)&0xff; //10 MSB score
AppData.Buffer[i++] = (uint8_t)(sensorData.event_sensor3_fall_motionscore)&0xff; //11 LSB score
AppData.Buffer[i++] = (uint8_t)(sensorData.unconscious_state)&0xff; //12 unconscious state detected or not
AppData.Buffer[i++] = (uint8_t)(sensorData.fall_state)&0xff; //13 fall detected or not
@ -1067,7 +1067,11 @@ static void SendTxData(void)
APP_LOG(TS_OFF, VLEVEL_L,
"\r\n######| S3-Motion |\r\n"
"\r\n######| %d |\r\n",sensorData.state_sensor3_on_off);
} else {
APP_LOG(TS_OFF, VLEVEL_M,"\r\n\n**************************************** UNKNOWN MODE \r\n");
}
} else if (sensor_data_ready == 0) {
APP_LOG(TS_OFF, VLEVEL_M,"\r\n\n**************************************** SENSOR_DATA_READY =%u \r\n", sensor_data_ready);
}
AppData.BufferSize = (uint8_t)(sts_service_mask > STS_SERVICE_MASK_L1? 0:i)&0xff;;
@ -2429,7 +2433,7 @@ void USER_APP_AUTO_RESPONDER_Parse(char *tlv_buf, uint8_t tlv_buf_size)
//sts_service_mask = STS_SERVICE_MASK_L0;
//sts_lamp_bar_color = STS_GREEN;
sts_presence_fall_detection=FALSE;
sts_rss_config_updated_flag = STS_RSS_CONFIG_DEFAULT;
sts_rss_config_updated_flag = STS_RSS_CONFIG_NON;
switch (sts_work_mode) {
case STS_NETWORK_MODE:
@ -2461,6 +2465,7 @@ void USER_APP_AUTO_RESPONDER_Parse(char *tlv_buf, uint8_t tlv_buf_size)
break;
default:
sts_cfg_nvm.sts_ioc_mask = STS_IOC_MASK_NONE;
break;
}