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

revised for accurate distance by ONLY ONE peak

This commit is contained in:
Yunhorn 2024-08-15 13:31:50 +08:00
parent 872482893f
commit 8475514ee8
7 changed files with 34 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Core/Inc/yunhorn_sts_sensors.h
View File

@ -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_Function_Test_Process(uint8_t *self_test_result, uint8_t count);
void STS_PRESENCE_SENSOR_Distance_Measure_Process(void); void STS_PRESENCE_SENSOR_Distance_Measure_Process(void);
float KalmanFilter(float inData);
/* USER CODE BEGIN Private defines */ /* USER CODE BEGIN Private defines */
/* /*

6
Core/Src/yunhorn_sts_distance_rss.c
View File

@ -111,8 +111,8 @@ int sts_distance_rss_detector_distance(void)
} }
bool success = true; 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_t result[number_of_peaks];
acc_detector_distance_result_info_t result_info; acc_detector_distance_result_info_t result_info;
float tmp_distance = 0.0f; float tmp_distance = 0.0f;
@ -127,7 +127,7 @@ int sts_distance_rss_detector_distance(void)
break; break;
} }
for(uint8_t j=0; j< result_info.number_of_peaks; j++) 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); print_distances(result, result_info.number_of_peaks);
} }

16
Core/Src/yunhorn_sts_presence_rss.c
View File

@ -662,11 +662,11 @@ void STS_YunhornCheckStandardDeviation(void)
// 1) VARIANCE // 1) VARIANCE
for (j = 0; j < SAMPLE_DATASET_NUM; j++) 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 sigma
standard_variance_presence_distance = (uint32_t)sqrt(variance_presence_distance); 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++) 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 // 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); 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++) 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); standard_variance_roc_acc = (uint32_t)sqrt((uint32_t)variance_roc_acc);

15
Core/Src/yunhorn_sts_process.c
View File

@ -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 */ /* USER CODE BEGIN EF */

3
LoRaWAN/App/lora_app.c
View File

@ -949,6 +949,7 @@ static void SendTxData(void)
if ((heart_beat_timer != 0L)) // sensor data OVERWRITE heart-beat message, 2024-05-12 if ((heart_beat_timer != 0L)) // sensor data OVERWRITE heart-beat message, 2024-05-12
{ {
heart_beat_timer=0; heart_beat_timer=0;
sensor_data_ready = 0;
if (sts_work_mode == STS_DUAL_MODE) if (sts_work_mode == STS_DUAL_MODE)
AppData.Port = (uint8_t)YUNHORN_STS_O6_LORA_APP_HTBT_PORT; //LORAWAN_USER_APP_PORT+1; AppData.Port = (uint8_t)YUNHORN_STS_O6_LORA_APP_HTBT_PORT; //LORAWAN_USER_APP_PORT+1;
else if (sts_work_mode == STS_UNI_MODE) else if (sts_work_mode == STS_UNI_MODE)
@ -1124,7 +1125,7 @@ static void OnTxTimerEvent(void *context)
/* USER CODE BEGIN OnTxTimerEvent_1 */ /* USER CODE BEGIN OnTxTimerEvent_1 */
/* USER CODE END 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); UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_LoRaSendOnTxTimerOrButtonEvent), CFG_SEQ_Prio_0);

7
LoRaWAN/App/lora_app.h
View File

@ -73,8 +73,9 @@ extern "C" {
/*! /*!
* LoRaWAN default class * LoRaWAN default class
*/ */
#ifndef STS_BAT
#define LORAWAN_DEFAULT_CLASS CLASS_C #define LORAWAN_DEFAULT_CLASS CLASS_C
#ifdef STS_BAT #elif defined(STS_BAT)
#define LORAWAN_DEFAULT_CLASS CLASS_A #define LORAWAN_DEFAULT_CLASS CLASS_A
#endif #endif
/*! /*!
@ -92,9 +93,9 @@ extern "C" {
* LoRaWAN Default Data Rate * LoRaWAN Default Data Rate
* @note Please note that LORAWAN_DEFAULT_DATA_RATE is used only when LORAWAN_ADR_STATE is disabled * @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 #define LORAWAN_DEFAULT_DATA_RATE DR_0
#ifdef STS_BAT #elif defined(STS_BAT)
#define LORAWAN_DEFAULT_DATA_RATE DR_5 #define LORAWAN_DEFAULT_DATA_RATE DR_5
#endif #endif
/*! /*!

1
STM32CubeIDE/.cproject
View File

@ -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 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"> <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="CORE_CM4"/>
<listOptionValue builtIn="false" value="STS_BAT"/>
<listOptionValue builtIn="false" value="CLOCK_SYNC"/> <listOptionValue builtIn="false" value="CLOCK_SYNC"/>
<listOptionValue builtIn="false" value="RM2_1"/> <listOptionValue builtIn="false" value="RM2_1"/>
<listOptionValue builtIn="false" value="STS_O7"/> <listOptionValue builtIn="false" value="STS_O7"/>