revised for accurate distance by ONLY ONE peak

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 */
 /* 

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;
+	const int                           iterations      = 10; //5;
-	uint16_t                            number_of_peaks = 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);
 	}
 

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_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);
+		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_distance /= (uint32_t)(SAMPLE_DATASET_NUM-1);
-	variance_presence_score    /= (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);
 

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 */
 

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);
 

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
 /*!

STM32CubeIDE/.cproject

@@ -154,6 +154,7 @@
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.1119592399" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.value.os" valueType="enumerated"/>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols.2024044405" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="CORE_CM4"/>
+									<listOptionValue builtIn="false" value="STS_BAT"/>
 									<listOptionValue builtIn="false" value="CLOCK_SYNC"/>
 									<listOptionValue builtIn="false" value="RM2_1"/>
 									<listOptionValue builtIn="false" value="STS_O7"/>