From 8475514ee8d291b17036cffada7110c6543fe934 Mon Sep 17 00:00:00 2001 From: YunHorn Technology Date: Thu, 15 Aug 2024 13:31:50 +0800 Subject: [PATCH] revised for accurate distance by ONLY ONE peak --- Core/Inc/yunhorn_sts_sensors.h | 1 + Core/Src/yunhorn_sts_distance_rss.c | 6 +++--- Core/Src/yunhorn_sts_presence_rss.c | 16 ++++++++-------- Core/Src/yunhorn_sts_process.c | 15 +++++++++++++++ LoRaWAN/App/lora_app.c | 3 ++- LoRaWAN/App/lora_app.h | 7 ++++--- STM32CubeIDE/.cproject | 1 + 7 files changed, 34 insertions(+), 15 deletions(-) diff --git a/Core/Inc/yunhorn_sts_sensors.h b/Core/Inc/yunhorn_sts_sensors.h index c513944..9097f63 100644 --- a/Core/Inc/yunhorn_sts_sensors.h +++ b/Core/Inc/yunhorn_sts_sensors.h @@ -754,6 +754,7 @@ void STS_SENSOR_Distance_Test_Process(void); void STS_PRESENCE_SENSOR_Function_Test_Process(uint8_t *self_test_result, uint8_t count); void STS_PRESENCE_SENSOR_Distance_Measure_Process(void); +float KalmanFilter(float inData); /* USER CODE BEGIN Private defines */ /* diff --git a/Core/Src/yunhorn_sts_distance_rss.c b/Core/Src/yunhorn_sts_distance_rss.c index 85a2a38..82b3804 100644 --- a/Core/Src/yunhorn_sts_distance_rss.c +++ b/Core/Src/yunhorn_sts_distance_rss.c @@ -111,8 +111,8 @@ int sts_distance_rss_detector_distance(void) } bool success = true; - const int iterations = 2; //5; - uint16_t number_of_peaks = 5; + const int iterations = 10; //5; + uint16_t number_of_peaks = 1; acc_detector_distance_result_t result[number_of_peaks]; acc_detector_distance_result_info_t result_info; float tmp_distance = 0.0f; @@ -127,7 +127,7 @@ int sts_distance_rss_detector_distance(void) break; } for(uint8_t j=0; j< result_info.number_of_peaks; j++) - tmp_distance += result[j].distance_m; + tmp_distance += result[j].distance_m; //KalmanFilter(result[j].distance_m); print_distances(result, result_info.number_of_peaks); } diff --git a/Core/Src/yunhorn_sts_presence_rss.c b/Core/Src/yunhorn_sts_presence_rss.c index eeed422..b540838 100644 --- a/Core/Src/yunhorn_sts_presence_rss.c +++ b/Core/Src/yunhorn_sts_presence_rss.c @@ -662,11 +662,11 @@ void STS_YunhornCheckStandardDeviation(void) // 1) VARIANCE for (j = 0; j < SAMPLE_DATASET_NUM; j++) { - 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 += (uint32_t)pow(sts_motion_dataset[j].presence_distance - average_presence_distance,2)/(SAMPLE_DATASET_NUM-1); + variance_presence_score += (uint32_t)pow(sts_motion_dataset[j].presence_score - average_presence_score,2)/(SAMPLE_DATASET_NUM-1); } - variance_presence_distance /= (uint32_t)(SAMPLE_DATASET_NUM-1); - variance_presence_score /= (uint32_t)(SAMPLE_DATASET_NUM-1); + //variance_presence_distance /= (uint32_t)(SAMPLE_DATASET_NUM-1); + //variance_presence_score /= (uint32_t)(SAMPLE_DATASET_NUM-1); //STANDARD VARIANCE sigma standard_variance_presence_distance = (uint32_t)sqrt(variance_presence_distance); @@ -686,10 +686,10 @@ void STS_YunhornCheckStandardDeviation(void) for (j = 0; j < (SAMPLE_DATASET_NUM-1); j++) { - variance_roc_distance += (uint32_t)(pow(roc_distance[j] - average_roc_distance,2)); + variance_roc_distance += (uint32_t)(pow(roc_distance[j] - average_roc_distance,2)/SAMPLE_DATASET_NUM); } // average - variance_roc_distance /= (uint32_t)(SAMPLE_DATASET_NUM); + //variance_roc_distance /= (uint32_t)(SAMPLE_DATASET_NUM); //???? standard_variance_roc_distance = (uint32_t)sqrt((uint32_t)variance_roc_distance); @@ -707,10 +707,10 @@ void STS_YunhornCheckStandardDeviation(void) for (j = 0; j < (SAMPLE_DATASET_NUM-2); j++) { - variance_roc_acc += (uint32_t)pow((uint32_t)((uint32_t)roc_acc[j] - (uint32_t)average_roc_acc),2); + variance_roc_acc += (uint32_t)pow((uint32_t)((uint32_t)roc_acc[j] - (uint32_t)average_roc_acc),2)/(SAMPLE_DATASET_NUM-1); } - variance_roc_acc /= (uint32_t)(SAMPLE_DATASET_NUM-1); + //variance_roc_acc /= (uint32_t)(SAMPLE_DATASET_NUM-1); standard_variance_roc_acc = (uint32_t)sqrt((uint32_t)variance_roc_acc); diff --git a/Core/Src/yunhorn_sts_process.c b/Core/Src/yunhorn_sts_process.c index e0449a3..32905e1 100644 --- a/Core/Src/yunhorn_sts_process.c +++ b/Core/Src/yunhorn_sts_process.c @@ -1322,6 +1322,21 @@ void OnSensor4StateChanged(void) } +//卡尔曼滤波 +float KalmanFilter(float inData) +{ + static float prevData = 0; //先前数值 + static float p = 10, q = 0.001, r = 0.001, kGain = 0; // q控制误差 r控制响应速度 + + p = p + q; + kGain = p / ( p + r ); //计算卡尔曼增益 + inData = prevData + ( kGain * ( inData - prevData ) ); //计算本次滤波估计值 + p = ( 1 - kGain ) * p; //更新测量方差 + prevData = inData; + return inData; //返回滤波值 + + +} /* USER CODE BEGIN EF */ diff --git a/LoRaWAN/App/lora_app.c b/LoRaWAN/App/lora_app.c index 28af51c..a404280 100644 --- a/LoRaWAN/App/lora_app.c +++ b/LoRaWAN/App/lora_app.c @@ -949,6 +949,7 @@ static void SendTxData(void) if ((heart_beat_timer != 0L)) // sensor data OVERWRITE heart-beat message, 2024-05-12 { heart_beat_timer=0; + sensor_data_ready = 0; if (sts_work_mode == STS_DUAL_MODE) AppData.Port = (uint8_t)YUNHORN_STS_O6_LORA_APP_HTBT_PORT; //LORAWAN_USER_APP_PORT+1; else if (sts_work_mode == STS_UNI_MODE) @@ -1124,7 +1125,7 @@ static void OnTxTimerEvent(void *context) /* USER CODE BEGIN OnTxTimerEvent_1 */ /* USER CODE END OnTxTimerEvent_1 */ - if ((sensor_data_ready ==1)|| (sts_reed_hall_changed_flag)) //||(sts_rss_result_changed_flag)||(sts_fall_rising_detected_result_changed_flag)) + if ((sensor_data_ready ==1)) //|| (sts_reed_hall_changed_flag)) //||(sts_rss_result_changed_flag)||(sts_fall_rising_detected_result_changed_flag)) { UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_LoRaSendOnTxTimerOrButtonEvent), CFG_SEQ_Prio_0); diff --git a/LoRaWAN/App/lora_app.h b/LoRaWAN/App/lora_app.h index 679adab..a56d02a 100644 --- a/LoRaWAN/App/lora_app.h +++ b/LoRaWAN/App/lora_app.h @@ -73,8 +73,9 @@ extern "C" { /*! * LoRaWAN default class */ +#ifndef STS_BAT #define LORAWAN_DEFAULT_CLASS CLASS_C -#ifdef STS_BAT +#elif defined(STS_BAT) #define LORAWAN_DEFAULT_CLASS CLASS_A #endif /*! @@ -92,9 +93,9 @@ extern "C" { * LoRaWAN Default Data Rate * @note Please note that LORAWAN_DEFAULT_DATA_RATE is used only when LORAWAN_ADR_STATE is disabled */ - +#ifndef STS_BAT #define LORAWAN_DEFAULT_DATA_RATE DR_0 -#ifdef STS_BAT +#elif defined(STS_BAT) #define LORAWAN_DEFAULT_DATA_RATE DR_5 #endif /*! diff --git a/STM32CubeIDE/.cproject b/STM32CubeIDE/.cproject index 93edae5..40fb73e 100644 --- a/STM32CubeIDE/.cproject +++ b/STM32CubeIDE/.cproject @@ -154,6 +154,7 @@