dengcy
/
micro_climate
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: dengcy:3749418cee0b9bf2a8bd4fc7e89c567b5e8e3bcb
Branches Tags
dengcy:master
dengcy:Test
..
compare: dengcy:4387a506e843776792602e6cbf9c11270671dafa
Branches Tags
dengcy:master
dengcy:Test

2 Commits
3749418cee ... 4387a506e8

Author SHA1 Message Date
95384 4387a506e8 修改探头间声音传播距离为115.12，tof减小2us 2024-11-22 18:35:31 +08:00
95384 8f8d6066a1 线性插值可用，添加一阶低通滤波器，线程资源可能需要调整 2024-11-22 14:30:58 +08:00
7 changed files with 310 additions and 49 deletions
Show Stats Expand all files Collapse all files

7
App/Inc/anemometer_dev.h
View File

@ -38,8 +38,13 @@
#define DRIVE_NUM 2
// 传播距离 风速计算公式中的L参数
#define DISTANCE ((float32_t)120000.0)
//探头表面距离115.12mm(57.56mm*2)，换能器探头到探头表面距离暂定2mm(1mm*2)
//#define DISTANCE ((float32_t)115120.0 + (float32_t)2000.0)
#define DISTANCE ((float32_t)115120.0)
// 传播距离 风速计算公式中的L参数
//#define DISTANCE ((float32_t)120000.0)
// 富奥通结构 L = 118946
// #define DISTANCE 118946

101
App/Src/anemometer_dev.c
View File

@ -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);
@ -430,8 +434,8 @@ void wind_task(void const * argument)
tof_error_log_NS = 0;
g_error_log.tof_error_NS = 0;
// 计算成us
tofx = ((tofx-FIR_PHASE_DELAY)/ADC_SAMP_RATE_MHz)+REV_MUTE_DELAY_US-1.0f/DRIVE_FREQ_MHz+0.0001f;
tofy = ((tofy-FIR_PHASE_DELAY)/ADC_SAMP_RATE_MHz)+REV_MUTE_DELAY_US-1.0f/DRIVE_FREQ_MHz+0.0001f;
tofx = ((tofx-FIR_PHASE_DELAY)/ADC_SAMP_RATE_MHz)+REV_MUTE_DELAY_US-1.0f/DRIVE_FREQ_MHz+0.0001f - 2.0;
tofy = ((tofy-FIR_PHASE_DELAY)/ADC_SAMP_RATE_MHz)+REV_MUTE_DELAY_US-1.0f/DRIVE_FREQ_MHz+0.0001f - 2.0;
// 通过各通道渡越时间求时间差
dtof = tofx-tofy;
@ -488,8 +492,8 @@ void wind_task(void const * argument)
tof_error_log_WE = 0;
g_error_log.tof_error_WE = 0;
// 计算成us
tofx = ((tofx-FIR_PHASE_DELAY)/ADC_SAMP_RATE_MHz)+REV_MUTE_DELAY_US-1.0f/DRIVE_FREQ_MHz+0.000001f;
tofy = ((tofy-FIR_PHASE_DELAY)/ADC_SAMP_RATE_MHz)+REV_MUTE_DELAY_US-1.0f/DRIVE_FREQ_MHz+0.000001f;
tofx = ((tofx-FIR_PHASE_DELAY)/ADC_SAMP_RATE_MHz)+REV_MUTE_DELAY_US-1.0f/DRIVE_FREQ_MHz+0.000001f - 2.0;
tofy = ((tofy-FIR_PHASE_DELAY)/ADC_SAMP_RATE_MHz)+REV_MUTE_DELAY_US-1.0f/DRIVE_FREQ_MHz+0.000001f - 2.0;
// 通过各通道渡越时间求时间差
dtof = tofx-tofy;
@ -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)
{
y = 0 + (x - 0) * (y1 - 0) / (x1 - 0);
}
if(x>=x1&&x<x2)
{
y = y1 + (x - x1) * (y2 - y1) / (x2 - x1);
}
if(x>=x2&&x<x3)
{
y = y2 + (x - x2) * (y3 - y2) / (x3 - x2);
}
if(x>=x3&&x<x4)
{
y = y3 + (x - x3) * (y4 - y3) / (x4 - x3);
}
// if(x>=x4&&x<x5)
if(x>=x4)
{
y = y4 + (x - x4) * (y5 - y4) / (x5 - x4);
}
return x;
}
// 小于0点返回0
if(x<x1)
{
return 0;
}
// 在前1,2点中间 或者 只有两个点，在2点后
if((x>=x1&&x<x2&&num_point>=2)||(x>=x2&&num_point==2))
{
y = y1 + (x - x1) * (y2 - y1) / (x2 - x1);
return y;
}
// 在前2,3点中间 或者 只有三个点，在3点后
else if((x>=x2&&x<x3&&num_point>=3)||(x>=x3&&num_point==3))
{
y = y2 + (x - x2) * (y3 - y2) / (x3 - x2);
return y;
}
// 在前3,4点中间 或者 只有四个点，在4点后
else if((x>=x3&&x<x4&&num_point>=4)||(x>=x4&&num_point==4))
{
y = y3 + (x - x3) * (y4 - y3) / (x4 - x3);
return y;
}
// 在前4,5点中间 或者 只有五个点，在5点后
else if((x>=x4&&x<x5&&num_point>=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();

196
App/Src/frt_protocol.c
View File

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

23
Drivers/Filter/LowPassFilter.c Normal file
View File

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

20
Drivers/Filter/LowPassFilter.h Normal file
View File

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

6
EWARM/micro_climate.ewp
View File

@ -1232,6 +1232,12 @@
<file>
<name>$PROJ_DIR$\..\Drivers\Filter\FIR.h</name>
</file>
<file>
<name>$PROJ_DIR$\..\Drivers\Filter\LowPassFilter.c</name>
</file>
<file>
<name>$PROJ_DIR$\..\Drivers\Filter\LowPassFilter.h</name>
</file>
</group>
<group>
<name>HP203B</name>

6
EWARM/micro_climate.ewt
View File

@ -1547,6 +1547,12 @@
<file>
<name>$PROJ_DIR$\..\Drivers\Filter\FIR.h</name>
</file>
<file>
<name>$PROJ_DIR$\..\Drivers\Filter\LowPassFilter.c</name>
</file>
<file>
<name>$PROJ_DIR$\..\Drivers\Filter\LowPassFilter.h</name>
</file>
</group>
<group>
<name>HP203B</name>