diff --git a/Core/Inc/yunhorn_sts_sensors.h b/Core/Inc/yunhorn_sts_sensors.h index 62ddb8d..657d27d 100644 --- a/Core/Inc/yunhorn_sts_sensors.h +++ b/Core/Inc/yunhorn_sts_sensors.h @@ -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; diff --git a/Core/Src/yunhorn_sts_presence_rss.c b/Core/Src/yunhorn_sts_presence_rss.c index c8c4a97..075049f 100644 --- a/Core/Src/yunhorn_sts_presence_rss.c +++ b/Core/Src/yunhorn_sts_presence_rss.c @@ -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 diff --git a/STM32CubeIDE/Release/STS_O7.bin b/STM32CubeIDE/Release/STS_O7.bin index e521787..434c1cc 100644 Binary files a/STM32CubeIDE/Release/STS_O7.bin and b/STM32CubeIDE/Release/STS_O7.bin differ