From 8f8d6066a1e00352496bc9410387b227d7cf810d Mon Sep 17 00:00:00 2001 From: 95384 <664090429@qq.com> Date: Fri, 22 Nov 2024 14:30:58 +0800 Subject: [PATCH] =?UTF-8?q?=E7=BA=BF=E6=80=A7=E6=8F=92=E5=80=BC=E5=8F=AF?= =?UTF-8?q?=E7=94=A8=EF=BC=8C=E6=B7=BB=E5=8A=A0=E4=B8=80=E9=98=B6=E4=BD=8E?= =?UTF-8?q?=E9=80=9A=E6=BB=A4=E6=B3=A2=E5=99=A8=EF=BC=8C=E7=BA=BF=E7=A8=8B?= =?UTF-8?q?=E8=B5=84=E6=BA=90=E5=8F=AF=E8=83=BD=E9=9C=80=E8=A6=81=E8=B0=83?= =?UTF-8?q?=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- App/Src/anemometer_dev.c | 93 +++++++++------- App/Src/frt_protocol.c | 196 ++++++++++++++++++++++++++++++++- Drivers/Filter/LowPassFilter.c | 23 ++++ Drivers/Filter/LowPassFilter.h | 20 ++++ EWARM/micro_climate.ewp | 6 + EWARM/micro_climate.ewt | 6 + 6 files changed, 300 insertions(+), 44 deletions(-) create mode 100644 Drivers/Filter/LowPassFilter.c create mode 100644 Drivers/Filter/LowPassFilter.h diff --git a/App/Src/anemometer_dev.c b/App/Src/anemometer_dev.c index 59744c2..4c96eba 100644 --- a/App/Src/anemometer_dev.c +++ b/App/Src/anemometer_dev.c @@ -7,9 +7,11 @@ #include "sht30.h" #include "hp203b.h" #include "FIR.h" +#include "LowPassFilter.h" #define AVE_TIME 600 //滑动平均时间，秒，最大600 #define COVER_TINE 10 //探头遮挡后导致的接收次数过小的报错临界次数 +#define LOW_PASS_ALPHA 0.1 //一阶低通滤波器系数 uint32_t run_time_us; @@ -371,7 +373,9 @@ void wind_task(void const * argument) arm_rfft_fast_init_f32(&s,FFT_DATA_LEN); firFilterInit(); - + // 一阶低通滤波器初始化，x，y方向分别滤波 + initLowPassFilter(&low_pass_filter_x, LOW_PASS_ALPHA); + initLowPassFilter(&low_pass_filter_y, LOW_PASS_ALPHA); for(;;) { __HAL_TIM_DISABLE(&htim16); @@ -517,6 +521,10 @@ void wind_task(void const * argument) speedi=0; arm_mean_f32(speedx,AV_SPEED_LEN,&av_speedx); arm_mean_f32(speedy,AV_SPEED_LEN,&av_speedy); + /* 一阶低通滤波器 */ + av_speedx = updateFilter(&low_pass_filter_x, av_speedx); + av_speedy = updateFilter(&low_pass_filter_y, av_speedy); + /* 一阶低通滤波器 */ av_speed = sqrtf(av_speedx*av_speedx + av_speedy*av_speedy); av_angle = acosf(av_speedx/(av_speed+0.00000001))/2/PI*360; if(av_speedy<0) @@ -712,46 +720,54 @@ float sum(float arr[], int n) float linear_interpolation(float x) { float y; //开始插值 - uint16_t x1 = g_stConfigInfo.linear_point_1_x; - uint16_t y1 = g_stConfigInfo.linear_point_1_y; - uint16_t x2 = g_stConfigInfo.linear_point_2_x; - uint16_t y2 = g_stConfigInfo.linear_point_2_y; - uint16_t x3 = g_stConfigInfo.linear_point_3_x; - uint16_t y3 = g_stConfigInfo.linear_point_3_y; - uint16_t x4 = g_stConfigInfo.linear_point_4_x; - uint16_t y4 = g_stConfigInfo.linear_point_4_y; - uint16_t x5 = g_stConfigInfo.linear_point_5_x; - uint16_t y5 = g_stConfigInfo.linear_point_5_y; - if(g_stConfigInfo.linear_enable == 0x01) + uint8_t num_point = 0; + float x1 = ((float )g_stConfigInfo.linear_point_1_x)/10; + float y1 = ((float )g_stConfigInfo.linear_point_1_y)/10; + float x2 = ((float )g_stConfigInfo.linear_point_2_x)/10; + float y2 = ((float )g_stConfigInfo.linear_point_2_y)/10; + float x3 = ((float )g_stConfigInfo.linear_point_3_x)/10; + float y3 = ((float )g_stConfigInfo.linear_point_3_y)/10; + float x4 = ((float )g_stConfigInfo.linear_point_4_x)/10; + float y4 = ((float )g_stConfigInfo.linear_point_4_y)/10; + float x5 = ((float )g_stConfigInfo.linear_point_5_x)/10; + float y5 = ((float )g_stConfigInfo.linear_point_5_y)/10; +// 插值点个数 + num_point = x5!=0?5:x4!=0?4:x3!=0?3:x2!=0?2:x1!=0?1:0; +// 未使能直接返回原始值,插值点小于2个返回原始值 + if(g_stConfigInfo.linear_enable != 0x01||num_point < 2) { - if (x1 == x2 || x2 == x3 || x3 == x4 || x4 == x5) - { -// 有问题就返回初始值 - return x; - } - if(x>=0&&x=x1&&x=x2&&x=x3&&x=x4&&x=x4) - { - y = y4 + (x - x4) * (y5 - y4) / (x5 - x4); - } + return x; + } +// 小于0点返回0 + if(x=x1&&x=2)||(x>=x2&&num_point==2)) + { + y = y1 + (x - x1) * (y2 - y1) / (x2 - x1); return y; } +// 在前2,3点中间 或者 只有三个点，在3点后 + else if((x>=x2&&x=3)||(x>=x3&&num_point==3)) + { + y = y2 + (x - x2) * (y3 - y2) / (x3 - x2); + return y; + } +// 在前3,4点中间 或者 只有四个点，在4点后 + else if((x>=x3&&x=4)||(x>=x4&&num_point==4)) + { + y = y3 + (x - x3) * (y4 - y3) / (x4 - x3); + return y; + } +// 在前4,5点中间 或者 只有五个点，在5点后 + else if((x>=x4&&x=5)||(x>=x5&&num_point==5)) + { + y = y4 + (x - x4) * (y5 - y4) / (x5 - x4); + return y; + } +// 其他情况 else { return x; @@ -855,7 +871,6 @@ void tem_hum_update_task(void const * argument) __iar_builtin_set_FAULTMASK(1); NVIC_SystemReset(); } - my_update_mcs_param(av_speed, av_angle); //采集HP203B数据(大气压) get_press_data(); diff --git a/App/Src/frt_protocol.c b/App/Src/frt_protocol.c index 178ee0a..c3634e7 100644 --- a/App/Src/frt_protocol.c +++ b/App/Src/frt_protocol.c @@ -514,38 +514,125 @@ static u_int16_t FRT_ReadRegErrorData(void *pMsg) return FRT_swap_endian_16(value); } +/** + * @brief 读线性插值原始点1 + * @param + * @retval + */ static u_int16_t FRT_ReadRegPoint_1X(void *pMsg) { + u_int16_t value=g_stConfigInfo.linear_point_1_x; + return FRT_swap_endian_16(value); } + +/** + * @brief 读线性插值原始点2 + * @param + * @retval + */ static u_int16_t FRT_ReadRegPoint_2X(void *pMsg) { + u_int16_t value=g_stConfigInfo.linear_point_2_x; + return FRT_swap_endian_16(value); } + +/** + * @brief 读线性插值原始点3 + * @param + * @retval + */ static u_int16_t FRT_ReadRegPoint_3X(void *pMsg) { + u_int16_t value=g_stConfigInfo.linear_point_3_x; + return FRT_swap_endian_16(value); } + +/** + * @brief 读线性插值原始点4 + * @param + * @retval + */ static u_int16_t FRT_ReadRegPoint_4X(void *pMsg) { + u_int16_t value=g_stConfigInfo.linear_point_4_x; + return FRT_swap_endian_16(value); } + +/** + * @brief 读线性插值原始点5 + * @param + * @retval + */ static u_int16_t FRT_ReadRegPoint_5X(void *pMsg) { + u_int16_t value=g_stConfigInfo.linear_point_5_x; + return FRT_swap_endian_16(value); } + +/** + * @brief 读线性插值矫正点1 + * @param + * @retval + */ static u_int16_t FRT_ReadRegPoint_1Y(void *pMsg) { + u_int16_t value=g_stConfigInfo.linear_point_1_y; + return FRT_swap_endian_16(value); } + +/** + * @brief 读线性插值矫正点2 + * @param + * @retval + */ static u_int16_t FRT_ReadRegPoint_2Y(void *pMsg) { + u_int16_t value=g_stConfigInfo.linear_point_2_y; + return FRT_swap_endian_16(value); } + +/** + * @brief 读线性插值矫正点3 + * @param + * @retval + */ static u_int16_t FRT_ReadRegPoint_3Y(void *pMsg) { + u_int16_t value=g_stConfigInfo.linear_point_3_y; + return FRT_swap_endian_16(value); } + +/** + * @brief 读线性插值矫正点4 + * @param + * @retval + */ static u_int16_t FRT_ReadRegPoint_4Y(void *pMsg) { + u_int16_t value=g_stConfigInfo.linear_point_4_y; + return FRT_swap_endian_16(value); } + +/** + * @brief 读线性插值矫正点5 + * @param + * @retval + */ static u_int16_t FRT_ReadRegPoint_5Y(void *pMsg) { + u_int16_t value=g_stConfigInfo.linear_point_5_y; + return FRT_swap_endian_16(value); } + +/** + * @brief 读线性插值使能状态 + * @param + * @retval + */ static u_int16_t FRT_ReadRegLinearEnable(void *pMsg) { + u_int16_t value=g_stConfigInfo.linear_enable; + return FRT_swap_endian_16(value); } /** @@ -612,23 +699,120 @@ static u_int16_t FRT_WriteRegRSSIRange(void *pMsg) return 0; } +/** + * @brief 写线性插值矫正点1 + * @param + * @retval + */ static u_int16_t FRT_WriteRegPoint_1Y(void *pMsg) { +// 获取此时XY数据 + uint16_t *pMsgPointY = (uint16_t *)pMsg; + uint16_t PointY = *pMsgPointY; + uint16_t PointX = g_stMcs_Para.average_wind_speed * 10; + +// 写入Flash + g_stConfigInfo.linear_point_1_x = PointX; + g_stConfigInfo.linear_point_1_y = PointY; + save_config_info(g_stConfigInfo); + + return 0; } + +/** + * @brief 写线性插值矫正点2 + * @param + * @retval + */ static u_int16_t FRT_WriteRegPoint_2Y(void *pMsg) { +// 获取此时XY数据 + uint16_t *pMsgPointY = (uint16_t *)pMsg; + uint16_t PointY = *pMsgPointY; + uint16_t PointX = g_stMcs_Para.average_wind_speed * 10; + +// 写入Flash + g_stConfigInfo.linear_point_2_x = PointX; + g_stConfigInfo.linear_point_2_y = PointY; + save_config_info(g_stConfigInfo); + + return 0; } + +/** + * @brief 写线性插值矫正点3 + * @param + * @retval + */ static u_int16_t FRT_WriteRegPoint_3Y(void *pMsg) { +// 获取此时XY数据 + uint16_t *pMsgPointY = (uint16_t *)pMsg; + uint16_t PointY = *pMsgPointY; + uint16_t PointX = g_stMcs_Para.average_wind_speed * 10; + +// 写入Flash + g_stConfigInfo.linear_point_3_x = PointX; + g_stConfigInfo.linear_point_3_y = PointY; + save_config_info(g_stConfigInfo); + + return 0; } + +/** + * @brief 写线性插值矫正点4 + * @param + * @retval + */ static u_int16_t FRT_WriteRegPoint_4Y(void *pMsg) { +// 获取此时XY数据 + uint16_t *pMsgPointY = (uint16_t *)pMsg; + uint16_t PointY = *pMsgPointY; + uint16_t PointX = g_stMcs_Para.average_wind_speed * 10; + +// 写入Flash + g_stConfigInfo.linear_point_4_x = PointX; + g_stConfigInfo.linear_point_4_y = PointY; + save_config_info(g_stConfigInfo); + + return 0; } + +/** + * @brief 写线性插值矫正点5 + * @param + * @retval + */ static u_int16_t FRT_WriteRegPoint_5Y(void *pMsg) { +// 获取此时XY数据 + uint16_t *pMsgPointY = (uint16_t *)pMsg; + uint16_t PointY = *pMsgPointY; + uint16_t PointX = g_stMcs_Para.average_wind_speed * 10; + +// 写入Flash + g_stConfigInfo.linear_point_5_x = PointX; + g_stConfigInfo.linear_point_5_y = PointY; + save_config_info(g_stConfigInfo); + + return 0; } + +/** + * @brief 写线性插值使能 + * @param + * @retval + */ static u_int16_t FRT_WriteRegLinearEnable(void *pMsg) { + uint16_t *pMsgData = (uint16_t *)pMsg; + uint16_t data = *pMsgData; + + g_stConfigInfo.linear_enable = data; + save_config_info(g_stConfigInfo); + + return 0; } /** @@ -712,7 +896,7 @@ void FRT_MsgProc_ReadRegister(device_handle device, void *pMsg) start_reg_addr < 0x00 ||\ (start_reg_addr > 0x08 && start_reg_addr < 0x14) ||\ (start_reg_addr > 0x17 && start_reg_addr < 0x1E) ||\ - start_reg_addr > 0x23 \ + start_reg_addr > 0x2E \ ) { @@ -724,7 +908,7 @@ void FRT_MsgProc_ReadRegister(device_handle device, void *pMsg) reg_num < 0x01 ||\ (((reg_num + start_reg_addr - 1) > 0x08) && ((reg_num + start_reg_addr - 1) < 0x14)) ||\ (((reg_num + start_reg_addr -1) > 0x17) && ((reg_num + start_reg_addr -1) < 0x1E)) ||\ - ((reg_num + start_reg_addr -1) > 0x23) \ + ((reg_num + start_reg_addr -1) > 0x2E) \ ) { @@ -774,8 +958,9 @@ void FRT_MsgProc_WriteRegister(device_handle device, void *pMsg) // 校验 if (start_reg_addr < 0x14 ||\ - (start_reg_addr > 0x1C && start_reg_addr < 0x1E)||\ - (start_reg_addr > 0x21) + (start_reg_addr > 0x1C && start_reg_addr < 0x1E)||\ + (start_reg_addr > 0x21 && start_reg_addr < 0x29)||\ + (start_reg_addr > 0x2E) ) { term_printf("start_reg_addr error:%d", start_reg_addr); @@ -784,7 +969,8 @@ void FRT_MsgProc_WriteRegister(device_handle device, void *pMsg) if (reg_num < 0x01 ||\ ((reg_num + start_reg_addr - 1) < 0x14) ||\ (((reg_num + start_reg_addr -1) > 0x1C) && ((reg_num + start_reg_addr -1) < 0x1E)) ||\ - ((reg_num + start_reg_addr -1) > 0x21) + (((reg_num + start_reg_addr -1) > 0x21) && ((reg_num + start_reg_addr -1) < 0x29)) ||\ + ((reg_num + start_reg_addr -1) > 0x2E) ) { term_printf("reg_num error:%d", reg_num); diff --git a/Drivers/Filter/LowPassFilter.c b/Drivers/Filter/LowPassFilter.c new file mode 100644 index 0000000..49c4da5 --- /dev/null +++ b/Drivers/Filter/LowPassFilter.c @@ -0,0 +1,23 @@ +#include "LowPassFilter.h" + +SL_LowPassFilter low_pass_filter_x; +SL_LowPassFilter low_pass_filter_y; + +// ʼһ׵ͨ˲ +void initLowPassFilter(SL_LowPassFilter* filter, float alpha) +{ + // ˲ϵ + filter->alpha = alpha; + // ʼһεֵΪ0 + filter->previous = 0.0f; +} + +// ˲ֵ +float updateFilter(SL_LowPassFilter* filter, float new_input) +{ + // ˲ֵ + float output = (1.0f - filter->alpha) * filter->previous + filter->alpha * new_input; + // һεֵ + filter->previous = output; + return output; +} \ No newline at end of file diff --git a/Drivers/Filter/LowPassFilter.h b/Drivers/Filter/LowPassFilter.h new file mode 100644 index 0000000..53fa63b --- /dev/null +++ b/Drivers/Filter/LowPassFilter.h @@ -0,0 +1,20 @@ +#ifndef _LOWPASSFILTER__H_ +#define _LOWPASSFILTER__H_ + + +#include "main.h" +#include "anemometer_dev.h" + +// һ׵ͨ˲ṹ +typedef struct { + float alpha; // ˲ϵ + float previous; // һεֵ +} SL_LowPassFilter; + +extern SL_LowPassFilter low_pass_filter_x; +extern SL_LowPassFilter low_pass_filter_y; + +void initLowPassFilter(SL_LowPassFilter* filter, float alpha); +float updateFilter(SL_LowPassFilter* filter, float new_input); + +#endif /*_LOWPASSFILTER__H_*/ \ No newline at end of file diff --git a/EWARM/micro_climate.ewp b/EWARM/micro_climate.ewp index 0fff44f..7e6f260 100644 --- a/EWARM/micro_climate.ewp +++ b/EWARM/micro_climate.ewp @@ -1232,6 +1232,12 @@ $PROJ_DIR$\..\Drivers\Filter\FIR.h + + $PROJ_DIR$\..\Drivers\Filter\LowPassFilter.c + + + $PROJ_DIR$\..\Drivers\Filter\LowPassFilter.h + HP203B diff --git a/EWARM/micro_climate.ewt b/EWARM/micro_climate.ewt index b8ff7da..e59bc61 100644 --- a/EWARM/micro_climate.ewt +++ b/EWARM/micro_climate.ewt @@ -1547,6 +1547,12 @@ $PROJ_DIR$\..\Drivers\Filter\FIR.h + + $PROJ_DIR$\..\Drivers\Filter\LowPassFilter.c + + + $PROJ_DIR$\..\Drivers\Filter\LowPassFilter.h + HP203B