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_Distance_Measure_Process(void);
float KalmanFilter(float inData);
/* 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;
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);
}

16
Core/Src/yunhorn_sts_presence_rss.c
View File

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

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

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

7
LoRaWAN/App/lora_app.h
View File

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

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