--- fall detection demo ---
This commit is contained in:
parent
80aed3dcaa
commit
ae712e0eca
|
|
@ -263,12 +263,12 @@ enum sts_sensor_result_t {
|
|||
|
||||
typedef struct
|
||||
{
|
||||
float p_dist_avg;
|
||||
float p_dist_v;
|
||||
float p_dist_standard;
|
||||
float m_score_avg;
|
||||
float roc_avg;
|
||||
float roc_standard;
|
||||
uint16_t p_dist_avg;
|
||||
uint16_t p_dist_v;
|
||||
uint16_t p_dist_standard;
|
||||
uint16_t m_score_avg;
|
||||
uint16_t roc_avg;
|
||||
uint16_t roc_standard;
|
||||
uint8_t fall_rising;
|
||||
} STS_PRESENCE_Motion_Featuer_t;
|
||||
|
||||
|
|
|
|||
|
|
@ -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 detected_hs_zone=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];
|
||||
volatile uint8_t sts_fall_rising_detected_result = STS_PRESENCE_NORMAL;
|
||||
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)]++;
|
||||
}
|
||||
|
||||
sts_motion_dataset[motion_count].presence_distance = result.presence_distance;
|
||||
sts_motion_dataset[motion_count].presence_score = result.presence_score;
|
||||
sts_motion_dataset[motion_count].presence_distance = 1000*result.presence_distance;
|
||||
sts_motion_dataset[motion_count].presence_score = 1000*result.presence_score;
|
||||
|
||||
if (sts_presence_fall_detection == TRUE)
|
||||
{
|
||||
|
|
@ -504,8 +510,8 @@ int sts_presence_rss_fall_rise_detection(void)
|
|||
|
||||
if (sts_presence_fall_detection == TRUE)
|
||||
{
|
||||
sts_motion_dataset[motion_count].presence_distance = result.presence_distance;
|
||||
sts_motion_dataset[motion_count].presence_score = result.presence_score;
|
||||
sts_motion_dataset[motion_count].presence_distance = 1000*result.presence_distance;
|
||||
sts_motion_dataset[motion_count].presence_score = 1000*result.presence_score;
|
||||
|
||||
if (motion_count ++ == DEFAULT_MOTION_DATASET_LEN)
|
||||
{
|
||||
|
|
@ -606,114 +612,115 @@ 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; //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;
|
||||
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
|
||||
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;;
|
||||
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);
|
||||
|
||||
//SUM
|
||||
//SUM of samples
|
||||
for(i= 0; i< DEFAULT_MOTION_DATASET_LEN; i++)
|
||||
{
|
||||
sum_presence_distance += (float)sts_motion_dataset[i].presence_distance;
|
||||
sum_presence_score += (float)sts_motion_dataset[i].presence_score;
|
||||
sum_presence_distance += (uint32_t)sts_motion_dataset[i].presence_distance;
|
||||
sum_presence_score += (uint32_t)sts_motion_dataset[i].presence_score;
|
||||
}
|
||||
// AVERAGE
|
||||
average_presence_distance = ((float)sum_presence_distance/(float)DEFAULT_MOTION_DATASET_LEN);
|
||||
average_presence_score = ((float)sum_presence_score/(float)DEFAULT_MOTION_DATASET_LEN);
|
||||
// AVERAGE of all samples ( u (miu) )
|
||||
average_presence_distance = ((uint32_t)sum_presence_distance/(uint32_t)(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++)
|
||||
{
|
||||
variance_presence_distance += (float)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_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 /= (float)DEFAULT_MOTION_DATASET_LEN;
|
||||
variance_presence_score /= (float)DEFAULT_MOTION_DATASET_LEN;
|
||||
variance_presence_distance /= (uint32_t)(DEFAULT_MOTION_DATASET_LEN-1);
|
||||
variance_presence_score /= (uint32_t)(DEFAULT_MOTION_DATASET_LEN-1);
|
||||
|
||||
//STANDARD VARIANCE
|
||||
standard_variance_presence_distance = (float)pow(variance_presence_distance,0.5);
|
||||
standard_variance_presence_score = (float)pow(variance_presence_score,0.5);
|
||||
//STANDARD VARIANCE sigma
|
||||
standard_variance_presence_distance = (uint32_t)sqrt(variance_presence_distance);
|
||||
standard_variance_presence_score = (uint32_t)sqrt(variance_presence_score);
|
||||
|
||||
|
||||
// ROC distance
|
||||
// SUM
|
||||
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));
|
||||
sum_roc_distance += ((float)roc_distance[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 = (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++)
|
||||
{
|
||||
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 = (float)pow((float)variance_roc_distance,0.5f);
|
||||
//????
|
||||
standard_variance_roc_distance = (uint32_t)sqrt((uint32_t)variance_roc_distance);
|
||||
|
||||
|
||||
// ROC Acceleration
|
||||
|
||||
for(i= 0; i< (DEFAULT_MOTION_DATASET_LEN-2); i++)
|
||||
{
|
||||
roc_acc[i] = (float)(labs((float)roc_distance[i+1] - (float)roc_distance[i]));
|
||||
sum_roc_acc += ((float)roc_acc[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 = (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++)
|
||||
{
|
||||
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
|
||||
|
||||
average_roc_distance *= (float)DEFAULT_MOTION_DATASET_LEN;
|
||||
variance_roc_distance *= (float)DEFAULT_MOTION_DATASET_LEN;
|
||||
standard_variance_roc_distance *= (float)DEFAULT_MOTION_DATASET_LEN;
|
||||
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 *= (float)DEFAULT_MOTION_DATASET_LEN;
|
||||
variance_roc_acc *= (float)DEFAULT_MOTION_DATASET_LEN;
|
||||
standard_variance_roc_acc *= (float)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;
|
||||
|
||||
// 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
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "-------------Motion 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));
|
||||
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), (int)(variance_presence_distance), (int)(standard_variance_presence_distance));
|
||||
//#endif
|
||||
#ifdef LOG_RSS
|
||||
APP_LOG(TS_OFF, VLEVEL_L, "-------------ROC Dist Average =%6u; Variance = %6u ; Standard =%6u \r\n",
|
||||
//#ifdef LOG_RSS
|
||||
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));
|
||||
|
||||
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));
|
||||
|
||||
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
|
||||
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));
|
||||
|
||||
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));
|
||||
#endif
|
||||
//#endif
|
||||
|
||||
// *******************************************
|
||||
// *********** detection situation suggestion
|
||||
|
|
|
|||
Binary file not shown.
Loading…
Reference in New Issue