From 0d56e7e26958794cfff5df5f6cca7b52b33c51d6 Mon Sep 17 00:00:00 2001 From: YunHorn Technology Date: Tue, 9 Jul 2024 14:41:12 +0800 Subject: [PATCH] ---- add KalmanFilter ---- --- Core/Inc/sts_weight_scale.h | 1 + Core/Src/gpio.c | 2 +- Core/Src/sts_weight_scale.c | 20 ++++++++++++++++++++ STM32CubeIDE/.settings/language.settings.xml | 4 ++-- 4 files changed, 24 insertions(+), 3 deletions(-) diff --git a/Core/Inc/sts_weight_scale.h b/Core/Inc/sts_weight_scale.h index 2223e8b..debde98 100644 --- a/Core/Inc/sts_weight_scale.h +++ b/Core/Inc/sts_weight_scale.h @@ -47,6 +47,7 @@ typedef struct void sts_weight_scale_init(void); void sts_weight_scale(sts_sensor_t *sensor_data); +uint32_t KalmanFilter(uint32_t inData); void HX711_Init(void); void Init_HX711pin(void); uint32_t HX711_Read(void); diff --git a/Core/Src/gpio.c b/Core/Src/gpio.c index 914db3c..7cb5eb7 100644 --- a/Core/Src/gpio.c +++ b/Core/Src/gpio.c @@ -84,7 +84,7 @@ void MX_GPIO_Init(void) // HX711_SCK GPIO_InitStruct.Pin = HX711_SCK_PIN; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; - GPIO_InitStruct.Pull = GPIO_PULLUP; + GPIO_InitStruct.Pull = GPIO_PULLUP; //GPIO_NOPULL GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; HAL_GPIO_Init(HX711_SCK_PORT, &GPIO_InitStruct); diff --git a/Core/Src/sts_weight_scale.c b/Core/Src/sts_weight_scale.c index bdee0a7..dbc84a0 100644 --- a/Core/Src/sts_weight_scale.c +++ b/Core/Src/sts_weight_scale.c @@ -148,6 +148,7 @@ uint32_t HX711_Read(void) //增益128 void Get_GrossWeight(void) { gross_weight = HX711_Read(); + gross_weight = KalmanFilter(gross_weight); APP_LOG(TS_OFF, VLEVEL_M, "\r\n ##### Gross Weight =%d <<<<<< \r\n",gross_weight); } @@ -167,11 +168,30 @@ void Get_NetWeight(void) //因为不同的传感器特性曲线不一样，因此，每一个传感器需要矫正这里的GapValue这个除数。 //当发现测试出来的重量偏大时，增加该数值。 + net_weight = KalmanFilter(net_weight); APP_LOG(TS_OFF, VLEVEL_M, "\r\n ##### Net Weight =%d \r\n", net_weight);//如果测试出来的重量偏小时，减小改数值。 } } +//卡尔曼滤波 +uint32_t KalmanFilter(uint32_t 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; //返回滤波值 + + +} + +//原文链接：https://blog.csdn.net/m0_63629044/article/details/138615848 + //延时nus //nus为要延时的us数. static uint8_t fac_us=0;//us延时倍乘数 diff --git a/STM32CubeIDE/.settings/language.settings.xml b/STM32CubeIDE/.settings/language.settings.xml index 90666d3..7768bdb 100644 --- a/STM32CubeIDE/.settings/language.settings.xml +++ b/STM32CubeIDE/.settings/language.settings.xml @@ -5,7 +5,7 @@ - + @@ -16,7 +16,7 @@ - +