sundp
/
O7
1
0
Fork 0
Code Issues Pull Requests 1 Packages Projects Releases Wiki Activity

--- fall detection demo ---

This commit is contained in:
Yunhorn 2024-08-08 19:24:37 +08:00
parent 80aed3dcaa
commit ae712e0eca
3 changed files with 70 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Core/Inc/yunhorn_sts_sensors.h
View File

@ -263,12 +263,12 @@ enum sts_sensor_result_t {
typedef struct typedef struct
{ {
float p_dist_avg; uint16_t p_dist_avg;
float p_dist_v; uint16_t p_dist_v;
float p_dist_standard; uint16_t p_dist_standard;
float m_score_avg; uint16_t m_score_avg;
float roc_avg; uint16_t roc_avg;
float roc_standard; uint16_t roc_standard;
uint8_t fall_rising; uint8_t fall_rising;
} STS_PRESENCE_Motion_Featuer_t; } STS_PRESENCE_Motion_Featuer_t;

121
Core/Src/yunhorn_sts_presence_rss.c
View File

@ -121,7 +121,13 @@ 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 motion_in_hs_zone[12][10]={0}; //0.4*12=4.8meter high, past 10 measures
volatile uint8_t detected_hs_zone=0;; volatile uint8_t detected_hs_zone=0;;
volatile uint16_t motion_count=0, motion_feature_count=0; volatile uint16_t motion_count=0, motion_feature_count=0;
static acc_detector_presence_result_t sts_motion_dataset[DEFAULT_MOTION_DATASET_LEN]; typedef struct
{
uint32_t presence_score; // 1000*
uint32_t presence_distance; // 1000*, in mm
} STS_presence_result_t;
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]; 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 = STS_PRESENCE_NORMAL;
volatile uint8_t sts_fall_rising_detected_result_changed_flag =0; volatile uint8_t sts_fall_rising_detected_result_changed_flag =0;
@ -426,8 +432,8 @@ int sts_presence_rss_fall_rise_detection(void)
motion_in_hs_zone[detected_hs_zone][(motion_detected_count)]++; motion_in_hs_zone[detected_hs_zone][(motion_detected_count)]++;
} }
sts_motion_dataset[motion_count].presence_distance = result.presence_distance; sts_motion_dataset[motion_count].presence_distance = 1000*result.presence_distance;
sts_motion_dataset[motion_count].presence_score = result.presence_score; sts_motion_dataset[motion_count].presence_score = 1000*result.presence_score;
if (sts_presence_fall_detection == TRUE) if (sts_presence_fall_detection == TRUE)
{ {
@ -504,8 +510,8 @@ int sts_presence_rss_fall_rise_detection(void)
if (sts_presence_fall_detection == TRUE) if (sts_presence_fall_detection == TRUE)
{ {
sts_motion_dataset[motion_count].presence_distance = result.presence_distance; sts_motion_dataset[motion_count].presence_distance = 1000*result.presence_distance;
sts_motion_dataset[motion_count].presence_score = result.presence_score; sts_motion_dataset[motion_count].presence_score = 1000*result.presence_score;
if (motion_count ++ == DEFAULT_MOTION_DATASET_LEN) if (motion_count ++ == DEFAULT_MOTION_DATASET_LEN)
{ {
@ -606,114 +612,115 @@ int sts_presence_rss_fall_rise_detection(void)
void STS_YunhornCheckStandardDeviation(void) void STS_YunhornCheckStandardDeviation(void)
{ {
uint16_t i,j; // sts_sensor_install_height <--- average_presence_distance should be approaching this distance - 50cm 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; //presence score act as magnetic or amplitude of motion uint32_t 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; uint32_t average_presence_distance = 0, average_presence_score=0;
float variance_presence_distance = 0, variance_presence_score=0; uint32_t variance_presence_distance = 0, variance_presence_score=0;
float standard_variance_presence_distance = 0, standard_variance_presence_score=0; uint32_t standard_variance_presence_distance = 0, standard_variance_presence_score=0;
// ROC -- rate of change // 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; 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;
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;; 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); //act as speed of change at given time slot acc_integration_sleep_ms(1000 / DEFAULT_UPDATE_RATE_PRESENCE);
//SUM //SUM of samples
for(i= 0; i< DEFAULT_MOTION_DATASET_LEN; i++) for(i= 0; i< DEFAULT_MOTION_DATASET_LEN; i++)
{ {
sum_presence_distance += (float)sts_motion_dataset[i].presence_distance; sum_presence_distance += (uint32_t)sts_motion_dataset[i].presence_distance;
sum_presence_score += (float)sts_motion_dataset[i].presence_score; sum_presence_score += (uint32_t)sts_motion_dataset[i].presence_score;
} }
// AVERAGE // AVERAGE of all samples ( u (miu) )
average_presence_distance = ((float)sum_presence_distance/(float)DEFAULT_MOTION_DATASET_LEN); average_presence_distance = ((uint32_t)sum_presence_distance/(uint32_t)(DEFAULT_MOTION_DATASET_LEN));
average_presence_score = ((float)sum_presence_score/(float)DEFAULT_MOTION_DATASET_LEN); average_presence_score = ((uint32_t)sum_presence_score/(uint32_t)(DEFAULT_MOTION_DATASET_LEN));
// VARIANCE // 1) VARIANCE
for (j = 0; j < DEFAULT_MOTION_DATASET_LEN; j++) for (j = 0; j < DEFAULT_MOTION_DATASET_LEN; j++)
{ {
variance_presence_distance += (float)pow(sts_motion_dataset[j].presence_distance - average_presence_distance,2); variance_presence_distance += (uint32_t)pow(sts_motion_dataset[j].presence_distance - average_presence_distance,2);
variance_presence_score += (float)pow(sts_motion_dataset[j].presence_score - average_presence_score,2); variance_presence_score += (uint32_t)pow(sts_motion_dataset[j].presence_score - average_presence_score,2);
} }
variance_presence_distance /= (float)DEFAULT_MOTION_DATASET_LEN; variance_presence_distance /= (uint32_t)(DEFAULT_MOTION_DATASET_LEN-1);
variance_presence_score /= (float)DEFAULT_MOTION_DATASET_LEN; variance_presence_score /= (uint32_t)(DEFAULT_MOTION_DATASET_LEN-1);
//STANDARD VARIANCE //STANDARD VARIANCE sigma
standard_variance_presence_distance = (float)pow(variance_presence_distance,0.5); standard_variance_presence_distance = (uint32_t)sqrt(variance_presence_distance);
standard_variance_presence_score = (float)pow(variance_presence_score,0.5); standard_variance_presence_score = (uint32_t)sqrt(variance_presence_score);
// ROC distance // ROC distance
// SUM // SUM
for(i= 0; i< (DEFAULT_MOTION_DATASET_LEN-1); i++) for(i= 0; i< (DEFAULT_MOTION_DATASET_LEN-1); i++)
{ {
roc_distance[i] = (float)(labs(sts_motion_dataset[i+1].presence_distance - sts_motion_dataset[i].presence_distance)); roc_distance[i] = (uint32_t)(labs(sts_motion_dataset[i+1].presence_distance - sts_motion_dataset[i].presence_distance));
sum_roc_distance += ((float)roc_distance[i]); sum_roc_distance += ((uint32_t)roc_distance[i]);
} }
average_roc_distance = (float)sum_roc_distance/(float)(DEFAULT_MOTION_DATASET_LEN-1); average_roc_distance = (uint32_t)sum_roc_distance/(float)(DEFAULT_MOTION_DATASET_LEN-1);
for (j = 0; j < (DEFAULT_MOTION_DATASET_LEN-1); j++) for (j = 0; j < (DEFAULT_MOTION_DATASET_LEN-1); j++)
{ {
variance_roc_distance += (float)(pow((float)roc_distance[j] - (float)average_roc_distance,2.0f)); variance_roc_distance += (uint32_t)(pow((float)roc_distance[j] - (uint32_t)average_roc_distance,2));
} }
// average
variance_roc_distance /= (uint32_t)(DEFAULT_MOTION_DATASET_LEN-1);
variance_roc_distance /= (float)(DEFAULT_MOTION_DATASET_LEN-1); //????
standard_variance_roc_distance = (uint32_t)sqrt((uint32_t)variance_roc_distance);
standard_variance_roc_distance = (float)pow((float)variance_roc_distance,0.5f);
// ROC Acceleration // ROC Acceleration
for(i= 0; i< (DEFAULT_MOTION_DATASET_LEN-2); i++) for(i= 0; i< (DEFAULT_MOTION_DATASET_LEN-2); i++)
{ {
roc_acc[i] = (float)(labs((float)roc_distance[i+1] - (float)roc_distance[i])); roc_acc[i] = (uint32_t)(labs((uint32_t)roc_distance[i+1] - (uint32_t)roc_distance[i]));
sum_roc_acc += ((float)roc_acc[i]); sum_roc_acc += ((uint32_t)roc_acc[i]);
} }
average_roc_acc = (float)sum_roc_acc/(float)(DEFAULT_MOTION_DATASET_LEN-2); average_roc_acc = (uint32_t)sum_roc_acc/(float)(DEFAULT_MOTION_DATASET_LEN-2);
for (j = 0; j < (DEFAULT_MOTION_DATASET_LEN-2); j++) for (j = 0; j < (DEFAULT_MOTION_DATASET_LEN-2); j++)
{ {
variance_roc_acc += (float)pow((float)((float)roc_acc[j] - (float)average_roc_acc),2.0f); variance_roc_acc += (uint32_t)pow((uint32_t)((uint32_t)roc_acc[j] - (uint32_t)average_roc_acc),2.0f);
} }
variance_roc_acc /= (float)(DEFAULT_MOTION_DATASET_LEN-2); variance_roc_acc /= (uint32_t)(DEFAULT_MOTION_DATASET_LEN-2);
standard_variance_roc_acc = (float)pow((float)variance_roc_acc,0.5f); standard_variance_roc_acc = (uint32_t)sqrt((uint32_t)variance_roc_acc);
//Normallize to m/s --- * DEFAULT_MOTION_DATASET_LEN for One single second //Normallize to m/s --- * DEFAULT_MOTION_DATASET_LEN for One single second
average_roc_distance *= (float)DEFAULT_MOTION_DATASET_LEN; average_roc_distance *= (uint32_t)DEFAULT_MOTION_DATASET_LEN;
variance_roc_distance *= (float)DEFAULT_MOTION_DATASET_LEN; variance_roc_distance *= (uint32_t)DEFAULT_MOTION_DATASET_LEN;
standard_variance_roc_distance *= (float)DEFAULT_MOTION_DATASET_LEN; standard_variance_roc_distance *= (uint32_t)DEFAULT_MOTION_DATASET_LEN;
average_roc_acc *= (float)DEFAULT_MOTION_DATASET_LEN; average_roc_acc *= (uint32_t)DEFAULT_MOTION_DATASET_LEN;
variance_roc_acc *= (float)DEFAULT_MOTION_DATASET_LEN; variance_roc_acc *= (uint32_t)DEFAULT_MOTION_DATASET_LEN;
standard_variance_roc_acc *= (float)DEFAULT_MOTION_DATASET_LEN; standard_variance_roc_acc *= (uint32_t)DEFAULT_MOTION_DATASET_LEN;
// print result // print result
#ifdef LOG_RSS
APP_LOG(TS_OFF, VLEVEL_L, "\r\n---Sensor Install Height=%6u-----Distance Average =%6u; Variance = %6u ; Standard =%6u \r\n",
(int)sts_sensor_install_height, (int)(average_presence_distance*1000.0f), (int)(variance_presence_distance*1000.0f), (int)(standard_variance_presence_distance*1000.0f));
#endif
//#ifdef LOG_RSS //#ifdef LOG_RSS
APP_LOG(TS_OFF, VLEVEL_M, "-------------Motion Average =%6u; Variance = %6u ; Standard =%6u \r\n", APP_LOG(TS_OFF, VLEVEL_L, "\r\n---Sensor Install Height=%6u-----Distance Average =%6u; Variance = %6u ; Standard =%6u \r\n",
(int)(average_presence_score*1000.0f), (int)(variance_presence_score*1000.0f), (int)(standard_variance_presence_score*1000.0f)); (int)sts_sensor_install_height, (int)(average_presence_distance), (int)(variance_presence_distance), (int)(standard_variance_presence_distance));
//#endif //#endif
#ifdef LOG_RSS //#ifdef LOG_RSS
APP_LOG(TS_OFF, VLEVEL_L, "-------------ROC Dist Average =%6u; Variance = %6u ; Standard =%6u \r\n", APP_LOG(TS_OFF, VLEVEL_M, "\r\n\n-------------Motion Average =%6u; Variance = %6u ; Standard =%6u \r\n",
(int)(average_presence_score), (int)(variance_presence_score), (int)(standard_variance_presence_score));
//#endif
//#ifdef LOG_RSS
APP_LOG(TS_OFF, VLEVEL_L, "\r\n-------------ROC Dist Average =%6u; Variance = %6u ; Standard =%6u \r\n",
(int)(average_roc_distance), (int)(variance_roc_distance), (int)(standard_variance_roc_distance)); (int)(average_roc_distance), (int)(variance_roc_distance), (int)(standard_variance_roc_distance));
APP_LOG(TS_OFF, VLEVEL_L, "-------------ROC ACC Average =%6u; Variance = %6u ; Standard =%6u \r\n", APP_LOG(TS_OFF, VLEVEL_L, "\r\n-------------ROC ACC Average =%6u; Variance = %6u ; Standard =%6u \r\n",
(int)(average_roc_acc), (int)(variance_roc_acc), (int)(standard_variance_roc_acc)); (int)(average_roc_acc), (int)(variance_roc_acc), (int)(standard_variance_roc_acc));
sts_fall_rising_pattern_factor1 = (int)(standard_variance_roc_distance); sts_fall_rising_pattern_factor1 = (int)(standard_variance_roc_distance);
sts_fall_rising_pattern_factor2 = (int)(fabs(average_presence_distance - fmax(0,last_average_presence_distance))*100.0f); // in cm sts_fall_rising_pattern_factor2 = (int)(fabs(average_presence_distance - fmax(0,last_average_presence_distance))*100.0f); // in cm
APP_LOG(TS_OFF, VLEVEL_L,"Avg-Dist =%6u, Last_AVG-Dist =%6u \r\n", (int)(average_presence_distance*1000.0f), (int)(last_average_presence_distance*1000.0f)); 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, "Threshold 1: \r\nAcc = %6u \r\nMeasure 1 = %6u ---- \r\n", APP_LOG(TS_OFF, VLEVEL_L, "\r\nThreshold 1: \r\nAcc = %6u \r\nMeasure 1 = %6u ---- \r\n",
(int)(sts_fall_detection_acc_threshold), (int)(sts_fall_rising_pattern_factor1)); (int)(sts_fall_detection_acc_threshold), (int)(sts_fall_rising_pattern_factor1));
APP_LOG(TS_OFF, VLEVEL_L, "Threshold 2: \r\nDis = %6u cm \r\nMeasure 2 = %6u cm ---- \r\n", APP_LOG(TS_OFF, VLEVEL_L, "\r\nThreshold 2: \r\nDis = %6u cm \r\nMeasure 2 = %6u cm ---- \r\n",
(int)(sts_fall_detection_depth_threshold), (int)(sts_fall_rising_pattern_factor2)); (int)(sts_fall_detection_depth_threshold), (int)(sts_fall_rising_pattern_factor2));
#endif //#endif
// ******************************************* // *******************************************
// *********** detection situation suggestion // *********** detection situation suggestion

BIN
STM32CubeIDE/Release/STS_O7.bin
View File

Binary file not shown.