修改探头间声音传播距离为115.12，tof减小2us

App/Inc/anemometer_dev.h

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

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);
@@ -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;
+      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;
+      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;
+      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;
+      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;
+    uint8_t num_point = 0;
-    uint16_t y1 = g_stConfigInfo.linear_point_1_y;
+    float x1 = ((float )g_stConfigInfo.linear_point_1_x)/10;
-    uint16_t x2 = g_stConfigInfo.linear_point_2_x;
+    float y1 = ((float )g_stConfigInfo.linear_point_1_y)/10;
-    uint16_t y2 = g_stConfigInfo.linear_point_2_y;
+    float x2 = ((float )g_stConfigInfo.linear_point_2_x)/10;
-    uint16_t x3 = g_stConfigInfo.linear_point_3_x;
+    float y2 = ((float )g_stConfigInfo.linear_point_2_y)/10;
-    uint16_t y3 = g_stConfigInfo.linear_point_3_y;
+    float x3 = ((float )g_stConfigInfo.linear_point_3_x)/10;
-    uint16_t x4 = g_stConfigInfo.linear_point_4_x;
+    float y3 = ((float )g_stConfigInfo.linear_point_3_y)/10;
-    uint16_t y4 = g_stConfigInfo.linear_point_4_y;
+    float x4 = ((float )g_stConfigInfo.linear_point_4_x)/10;
-    uint16_t x5 = g_stConfigInfo.linear_point_5_x;
+    float y4 = ((float )g_stConfigInfo.linear_point_4_y)/10;
-    uint16_t y5 = g_stConfigInfo.linear_point_5_y;
+    float x5 = ((float )g_stConfigInfo.linear_point_5_x)/10;
-    if(g_stConfigInfo.linear_enable == 0x01)
+    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;
-        {
+    }
-//            有问题就返回初始值
+//    小于0点返回0
-            return x;
+    if(x<x1)
-        }
+    {
-        if(x>=0&&x<x1)
+        return 0;
-        {
+    }
-            y = 0 + (x - 0) * (y1 - 0) / (x1 - 0);
+//    在前1,2点中间 或者 只有两个点，在2点后
-        }
+    if((x>=x1&&x<x2&&num_point>=2)||(x>=x2&&num_point==2))
-        if(x>=x1&&x<x2)
+    {
-        {
+        y = y1 + (x - x1) * (y2 - y1) / (x2 - x1);
-            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 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();

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 > 0x1C && start_reg_addr < 0x1E)||\
-       (start_reg_addr > 0x21)
+       (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);

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

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

EWARM/micro_climate.ewp

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

EWARM/micro_climate.ewt

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