772 lines
24 KiB
C
772 lines
24 KiB
C
#include "anemometer_dev.h"
|
||
#include "FreeRTOS.h"
|
||
#include "filter.h"
|
||
#include "inflash.h"
|
||
#include "uart_dev.h"
|
||
#include "fdacoefs.h"
|
||
|
||
|
||
#ifdef ENABLE_FIR_FILTER
|
||
#define NUM_TAPS 46
|
||
const float32_t firCoeffs32LP[NUM_TAPS] = {
|
||
-0.001119464869,-0.000287396746,-8.360351057e-05,0.0003625267709, 0.001120736357,
|
||
0.002262232359, 0.003853877541, 0.005952425767, 0.008596911095, 0.01180436555,
|
||
0.01556421723, 0.01983707026, 0.02455105446, 0.02960319445, 0.03486000001,
|
||
0.04016984627, 0.04535837471, 0.05024559051, 0.05465414748, 0.0584131442,
|
||
0.06137540936, 0.06342063844, 0.06446501613, 0.06446501613, 0.06342063844,
|
||
0.06137540936, 0.0584131442, 0.05465414748, 0.05024559051, 0.04535837471,
|
||
0.04016984627, 0.03486000001, 0.02960319445, 0.02455105446, 0.01983707026,
|
||
0.01556421723, 0.01180436555, 0.008596911095, 0.005952425767, 0.003853877541,
|
||
0.002262232359, 0.001120736357,0.0003625267709,-8.360351057e-05,-0.000287396746,
|
||
-0.001119464869
|
||
};
|
||
arm_fir_instance_f32 filter_s;
|
||
float32_t firState[ADC_VAL_LEN+NUM_TAPS-1];
|
||
|
||
#endif
|
||
//#define USE_CORRX_GET_DTOF 1
|
||
|
||
int16_t adc_val[ADC_VAL_LEN];
|
||
int16_t adc_val1[ADC_VAL_LEN];
|
||
|
||
#define AV_SPEED_LEN 10
|
||
float32_t speed[AV_SPEED_LEN]={0};
|
||
float32_t angle[AV_SPEED_LEN]={0};
|
||
float32_t speedx[AV_SPEED_LEN]={0};
|
||
float32_t speedy[AV_SPEED_LEN]={0};
|
||
|
||
uint32_t speedi = 0;
|
||
float32_t av_speed;
|
||
float32_t av_angle;
|
||
float32_t av_speedx= 0;
|
||
float32_t av_speedy=0;
|
||
Weather_param weather_info={0x00};
|
||
mcs_para g_stMcs_Para={0x00};
|
||
|
||
|
||
|
||
|
||
|
||
void update_mcs_param(float new_wind_speed, float new_wind_dirction);
|
||
|
||
|
||
|
||
|
||
float32_t max_val_f32;
|
||
|
||
int32_t max_val_index_f32;
|
||
|
||
// 多项式插值
|
||
// 返回值是最大值
|
||
float32_t find_maxVal_by_interpolation(float32_t a,float32_t b,float32_t c)
|
||
{
|
||
float32_t d1=0;
|
||
//到达极值点的时间Xmax
|
||
d1 = (a-c)/2.0f/(a-2.0f*b+c);
|
||
return 0.5f*a*d1*(d1-1.0f)-b*(d1-1.0f)*(d1+1.0f)+0.5f*c*d1*(d1+1.0f);
|
||
}
|
||
|
||
|
||
// 余弦插值找最大值所在的位置
|
||
// 返回值是相位
|
||
float32_t find_maxValPosition_by_sinInterpolation(float32_t a,float32_t b,float32_t c)
|
||
{
|
||
// sin 插值 寻找最大值
|
||
|
||
float32_t w_val,sin_val,y_val;
|
||
|
||
w_val = acosf((a+c)/2.0f/b);
|
||
|
||
sin_val = sinf(w_val);
|
||
|
||
y_val = atanf((a-c)/2.0f/b/sin_val);
|
||
|
||
return (0.0f-y_val)/w_val;
|
||
|
||
}
|
||
|
||
|
||
// 删除特定值之后的波形，加强互相关准确性
|
||
void remove_aftershocks(q15_t* x,uint32_t len)
|
||
{
|
||
q15_t max_val;
|
||
q15_t dc_offset;
|
||
float32_t echo_p = 0,echo_dt = 0;
|
||
uint32_t max_val_p;
|
||
uint32_t i=0;//stop_position = 0;
|
||
// 求取平均值
|
||
arm_mean_q15(x,20,&dc_offset);
|
||
|
||
// 查找数组中的最大值和最大值所在的索引
|
||
arm_max_q15(x,len,&max_val,&max_val_p);
|
||
|
||
max_val = max_val - dc_offset ;
|
||
|
||
uint16_t max_val_zero_1R5 = (max_val*15/100)+dc_offset;
|
||
// 最大值的0.45倍
|
||
uint16_t max_val_zero_4R5 = (max_val*45/100)+dc_offset;
|
||
// 最大值的0.8倍
|
||
uint16_t max_val_zero_8R0 = (max_val*80/100)+dc_offset;
|
||
|
||
//如果最大值位置大于200 则从最大值前200个位置开始寻找起始波形。
|
||
// 优化的地方，从最大值位置开始找到达波,可以最大限度排除偶然噪声干扰，
|
||
// 因为噪声在波形到达出 噪声不是很大
|
||
//优化性能，如果不需要则从数组0位置开始寻找其实波形
|
||
if(max_val_p>=40)
|
||
{
|
||
i = max_val_p-40;
|
||
}else
|
||
{
|
||
i = 0;
|
||
}
|
||
// 在最大值前寻找起始波形
|
||
for( ; i < max_val_p ; i++)
|
||
{
|
||
// 建议判断顶点，但是容易遇到偶然数据异常 类似于 28 29 28 30 29 28这种情况
|
||
//if( x[i-1] < x[i] && x[i]> x[i+1] )
|
||
// 排除以上数据异常情况，但是有可能就无法检测到30 这个顶点。
|
||
// 故需要检测下一个周期的顶点，然后再减去一个周期的时间。
|
||
if( x[i-2]<x[i-1] && x[i-1] <= x[i] && x[i]>=x[i+1] && x[i+1]>x[i+2])
|
||
{
|
||
//
|
||
// 判断顶点是否在 45% -- 80% 之间
|
||
if(x[i] >= max_val_zero_8R0 )
|
||
{
|
||
for(;i<len;i++)
|
||
{
|
||
x[i] = dc_offset;
|
||
}
|
||
return ;
|
||
}
|
||
if(x[i] >= max_val_zero_4R5 && x[i] <= max_val_zero_8R0)
|
||
{
|
||
for(i+12;i<len;i++)
|
||
{
|
||
x[i] = dc_offset;
|
||
}
|
||
return ;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
#ifdef USE_CORRX_GET_DTOF
|
||
// arm_dsp fft 中间变量
|
||
arm_rfft_fast_instance_f32 s;
|
||
|
||
|
||
// 互相关结果
|
||
float32_t corrx_out[ADC_VAL_LEN];
|
||
|
||
float32_t x_buf[ADC_VAL_LEN];
|
||
float32_t y_buf[ADC_VAL_LEN];
|
||
|
||
float32_t x_fft_O_f32[ADC_VAL_LEN] = {0};
|
||
float32_t y_fft_O_f32[ADC_VAL_LEN]= {0};
|
||
|
||
float32_t cmplx_mul_result_f32[ADC_VAL_LEN]= {0};
|
||
|
||
// 互相关算法确定两个波形之间的相位
|
||
float32_t cal_dtof(q15_t* x , q15_t* y , uint32_t len)
|
||
{
|
||
float a,b,c;
|
||
float tof;
|
||
arm_rfft_fast_init_f32(&s,len);
|
||
// 定点数转成float
|
||
arm_q15_to_float(x,x_buf,len);
|
||
// 定点数转成float
|
||
arm_q15_to_float(y,y_buf,len);
|
||
// 初始化 fft
|
||
arm_rfft_fast_init_f32(&s,len);
|
||
// fft 前一半和后一半是对称的,所以只求解了512组数据
|
||
arm_rfft_fast_f32(&s,x_buf,x_fft_O_f32,0);
|
||
// fft
|
||
arm_rfft_fast_f32(&s,y_buf,y_fft_O_f32,0);
|
||
|
||
// 复数取共轭
|
||
arm_cmplx_conj_f32(y_fft_O_f32,y_fft_O_f32,len>>1);
|
||
// fft的数据
|
||
// 时域卷积==频域相乘 1024个数据 复数只有512个
|
||
arm_cmplx_mult_cmplx_f32(x_fft_O_f32,y_fft_O_f32,cmplx_mul_result_f32,len>>1);
|
||
// ifft
|
||
arm_rfft_fast_f32(&s,cmplx_mul_result_f32,corrx_out,1);
|
||
//
|
||
//arm_abs_f32(corrx_out,corrx_out,len);
|
||
|
||
arm_max_f32(corrx_out,len,&max_val_f32,(uint32_t*)&max_val_index_f32);
|
||
|
||
b =corrx_out[max_val_index_f32];
|
||
|
||
if(max_val_index_f32 == 0)
|
||
{
|
||
a =corrx_out[len-1];
|
||
c =corrx_out[max_val_index_f32+1];
|
||
}
|
||
else
|
||
if(max_val_index_f32 == len-1)
|
||
{
|
||
a =corrx_out[max_val_index_f32-1];
|
||
c =corrx_out[0];
|
||
}else
|
||
{
|
||
a =corrx_out[max_val_index_f32-1];
|
||
c =corrx_out[max_val_index_f32+1];
|
||
}
|
||
tof = max_val_index_f32+find_maxValPosition_by_sinInterpolation(a,b,c);
|
||
// tof = max_val_index_f32;
|
||
if(tof>len/2)
|
||
tof= tof-len;
|
||
return tof;
|
||
}
|
||
#endif
|
||
|
||
uint32_t max_point_position[10];
|
||
q15_t max_point_val[10];
|
||
float cal_tof(q15_t* x,uint32_t len)
|
||
{
|
||
q15_t max_val;
|
||
float32_t echo_p = 0,echo_dt = 0;
|
||
uint32_t max_val_p;
|
||
uint32_t i=0;//stop_position = 0;
|
||
|
||
// 记录波形的极大值点和位置
|
||
|
||
uint32_t cnt =1;
|
||
|
||
// 查找数组中的最大值和最大值所在的索引
|
||
arm_max_q15(x,len,&max_val,&max_val_p);
|
||
|
||
max_point_position[0] = max_val_p;
|
||
max_point_val[0] = max_val;
|
||
|
||
// 大宇换能器参数 开始
|
||
// 最大值的0.18倍
|
||
uint16_t max_val_zero_1R5 = (max_val*15/100);
|
||
// 最大值的0.45倍
|
||
uint16_t max_val_zero_4R5 = (max_val*45/100);
|
||
// 最大值的0.8倍
|
||
uint16_t max_val_zero_8R0 = (max_val*80/100);
|
||
// 大宇换能器参数 结束
|
||
|
||
// // 无锡电声换能器参数
|
||
// // 最大值的0.18倍
|
||
// uint16_t max_val_zero_1R5 = (max_val*10/100);
|
||
// // 最大值的0.45倍
|
||
// uint16_t max_val_zero_4R5 = (max_val*35/100);
|
||
// // 最大值的0.8倍
|
||
// uint16_t max_val_zero_8R0 = (max_val*65/100);
|
||
|
||
|
||
//如果最大值位置大于200 则从最大值前200个位置开始寻找起始波形。
|
||
// 优化的地方，从最大值位置开始找到达波,可以最大限度排除偶然噪声干扰，
|
||
// 因为噪声在波形到达出 噪声不是很大
|
||
//优化性能，如果不需要则从数组0位置开始寻找其实波形
|
||
if(max_val_p>=70)
|
||
{
|
||
i = max_val_p-70;
|
||
}else
|
||
{
|
||
i = 0;
|
||
}
|
||
// 在最大值前寻找起始波形
|
||
for( ; i < max_val_p ; i++)
|
||
{
|
||
// 建议判断顶点，但是容易遇到偶然数据异常 类似于 28 29 28 30 29 28这种情况
|
||
//if( x[i-1] < x[i] && x[i]> x[i+1] )
|
||
// 排除以上数据异常情况，但是有可能就无法检测到30 这个顶点。
|
||
// 故需要检测下一个周期的顶点，然后再减去一个周期的时间。
|
||
if( x[i-2]<x[i-1] && x[i-1] <= x[i] && x[i]>=x[i+1] && x[i+1]>x[i+2])
|
||
{
|
||
|
||
max_point_position[cnt] = i;
|
||
max_point_val[cnt] = x[i];
|
||
cnt++;
|
||
// 减去偏置电压
|
||
//temp_val_zero = arr[i]-2048;
|
||
// 判断顶点是否在 15%-%45之间。
|
||
if(x[i] >= max_val_zero_1R5 && x[i] <= max_val_zero_4R5 )
|
||
{
|
||
// 如果找到 函数退出
|
||
echo_dt = (x[i-1]-x[i+1])/2.0/(x[i-1]-2*x[i]+x[i+1]);
|
||
echo_dt = find_maxValPosition_by_sinInterpolation(x[i-1],x[i],x[i+1]);
|
||
echo_p = (float32_t)i+echo_dt-0*(float32_t)(ADC_SAMP_RATE_MHz/DRIVE_FREQ_MHz);
|
||
return echo_p;
|
||
}
|
||
|
||
// 如果15% ~45%之间的数据未找到，则找45-80%的顶点。
|
||
// 判断顶点是否在 45% -- 80% 之间
|
||
if(x[i] >= max_val_zero_4R5 && x[i] <= max_val_zero_8R0)
|
||
{
|
||
// 如果找到 函数推出
|
||
echo_dt = (x[i-1]-x[i+1])/2.0/(x[i-1]-2*x[i]+x[i+1]);
|
||
echo_dt = find_maxValPosition_by_sinInterpolation(x[i-1],x[i],x[i+1]);
|
||
// 换算成第二个顶点的位置。
|
||
echo_p = (float32_t)i+echo_dt - 1*(float32_t)(ADC_SAMP_RATE_MHz/DRIVE_FREQ_MHz);
|
||
return echo_p;
|
||
}
|
||
}
|
||
}
|
||
term_printf("bad wave of echo signal \r\n");
|
||
// term_printf("maxVal:%d,maxValP:%d \r\n",max_point_val[0],max_point_position[0]);
|
||
// for(cnt = 1 ;cnt<9;cnt++)
|
||
// {
|
||
// term_printf("mV:%d,mP:%d,%d %% \r\n",max_point_val[i],max_point_position[i],(uint32_t)((max_point_val[i]*1.0f)/max_point_val[0]*100.0f));
|
||
// }
|
||
return 0;
|
||
}
|
||
|
||
|
||
|
||
/*
|
||
C B A
|
||
0 0 0 X0
|
||
0 0 1 X1
|
||
0 1 0 X2
|
||
0 1 1 X3
|
||
1 0 0 X4
|
||
1 0 1 X5
|
||
*/
|
||
void change_channel(uint32_t channel)
|
||
{
|
||
// 复位全部通道IO
|
||
HAL_GPIO_WritePin(GPIOC, GPIO_PWM_C_Pin|GPIO_PWM_B_Pin|GPIO_PWM_A_Pin, GPIO_PIN_RESET);
|
||
HAL_GPIO_WritePin(GPIOC, GPIO_RX_A_Pin|GPIO_RX_B_Pin|GPIO_RX_C_Pin, GPIO_PIN_RESET);
|
||
|
||
switch(channel)
|
||
{
|
||
case 0x01:
|
||
// N tx S rx
|
||
//HAL_GPIO_WritePin(GPIOC, GPIO_RX_A_Pin, GPIO_PIN_SET);
|
||
// 接收通道
|
||
HAL_GPIO_WritePin(GPIOC, GPIO_RX_A_Pin, GPIO_PIN_SET);
|
||
break;
|
||
|
||
case 0x02:
|
||
// N rx S tx
|
||
HAL_GPIO_WritePin(GPIOC, GPIO_PWM_A_Pin, GPIO_PIN_SET);
|
||
HAL_GPIO_WritePin(GPIOC, GPIO_RX_B_Pin, GPIO_PIN_SET);
|
||
break;
|
||
|
||
case 0x03:
|
||
// W tx E rx
|
||
HAL_GPIO_WritePin(GPIOC, GPIO_PWM_B_Pin, GPIO_PIN_SET);
|
||
HAL_GPIO_WritePin(GPIOC, GPIO_RX_A_Pin|GPIO_RX_B_Pin, GPIO_PIN_SET);
|
||
break;
|
||
|
||
case 0x04:
|
||
// W rx E tx
|
||
HAL_GPIO_WritePin(GPIOC, GPIO_PWM_A_Pin|GPIO_PWM_B_Pin, GPIO_PIN_SET);
|
||
//HAL_GPIO_WritePin(GPIOC, GPIO_RX_A_Pin, GPIO_PIN_SET);
|
||
break;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
void play_one_measure(int16_t* result_data,uint32_t len)
|
||
{
|
||
// 重新初始化PWM定时器
|
||
MX_TIM15_Init();
|
||
//MX_TIM6_Init();
|
||
// adc驱动定时器,保证每次使用前处于停止状态
|
||
HAL_TIM_Base_Stop(&htim6);
|
||
// 计数器复位
|
||
__HAL_TIM_SET_COUNTER(&htim6,0);
|
||
// 校准adc
|
||
//HAL_ADCEx_Calibration_Start(&hadc1,ADC_SINGLE_ENDED);
|
||
// adc dma初始化
|
||
HAL_ADC_Start_DMA(&hadc1,(uint32_t *)result_data,len);
|
||
//禁止全局中断
|
||
|
||
// 停止定时器
|
||
__HAL_TIM_DISABLE(&htim7);
|
||
// 重置计数器
|
||
__HAL_TIM_SET_COUNTER(&htim7,0);
|
||
__set_PRIMASK(1);
|
||
// 发送超声波驱动信号
|
||
HAL_TIM_PWM_Start(&htim15,TIM_CHANNEL_1);
|
||
// 延时 REV_MUTE_DELAY us
|
||
//HAL_GPIO_WritePin(GPIOC,GPIO_ACK_LED_Pin,0);
|
||
// 启定时器
|
||
__HAL_TIM_ENABLE(&htim7);
|
||
// 等待定时到达
|
||
while(__HAL_TIM_GET_COUNTER(&htim7)<REV_MUTE_DELAY_US);
|
||
//HAL_GPIO_WritePin(GPIOC,GPIO_ACK_LED_Pin,1);
|
||
// 关闭定时
|
||
__HAL_TIM_DISABLE(&htim7);
|
||
//HAL_TIM_PWM_Stop(&htim15,TIM_CHANNEL_1);
|
||
// 开启ADC
|
||
HAL_TIM_Base_Start(&htim6);
|
||
// 使能全局中断
|
||
__set_PRIMASK(0);
|
||
// 等待adc采集完成，且等待超声波换能器能量释放完成，避免通道之间干扰
|
||
//不能使用os_delay();
|
||
osDelay(1);
|
||
//HAL_Delay(3);
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
void calculate_param(Weather_param *parm ,uint32_t direction , int16_t *adc_buf1,int16_t *adc_buf2,uint32_t len)
|
||
{
|
||
float32_t tofx,tofy,dtof;
|
||
|
||
tofx = cal_tof(adc_buf1,len)/ADC_SAMP_RATE_MHz+REV_MUTE_DELAY_US;
|
||
tofy = cal_tof(adc_buf2,len)/ADC_SAMP_RATE_MHz+REV_MUTE_DELAY_US;
|
||
#ifdef USE_CORRX_GET_DTOF
|
||
dtof = cal_dtof(adc_buf1,adc_buf2,len)/ADC_SAMP_RATE_MHz;
|
||
#endif
|
||
if( fabsf( fabsf(tofx-tofy) - fabsf(dtof) ) > 2.0f)
|
||
{
|
||
parm->wind_c = DISTANCE/2.0f*(1.0f/tofx+1.0f/tofy);
|
||
return;
|
||
}
|
||
// 富奥通结构 L = 118946
|
||
// 新结构 L = 115960
|
||
parm->wind_c = DISTANCE/2.0f*(1.0f/tofx+1.0f/tofy);
|
||
// v = L*dtof/2/tofx/tofy/cos
|
||
if(direction == WIND_DIRECTION_X)
|
||
parm->wind_velocity_x = DISTANCE*dtof/1.41422f/tofx/tofx;
|
||
else
|
||
parm->wind_velocity_y = DISTANCE*dtof/1.41422f/tofx/tofx;
|
||
|
||
}
|
||
|
||
//#define UPDATA_FROM_SERIAL_PORT 1
|
||
//#define DEBUG_TOF_ERR 1
|
||
float32_t tofx,tofx1,tofy1,tofy,dtof,c_dtof;
|
||
char str[100];
|
||
|
||
|
||
void calculate_tof_dtof_param(Weather_param *parm ,uint32_t direction , int16_t *adc_buf1,int16_t *adc_buf2,uint32_t len)
|
||
{
|
||
|
||
int16_t dc_offset1,dc_offset2;//信号的直流偏置
|
||
// 计算直流分量 因为数据前端是没有回波的 计算50个数据求平均值 获取直流分量
|
||
arm_mean_q15(adc_buf1,50,&dc_offset1);
|
||
arm_mean_q15(adc_buf2,50,&dc_offset2);
|
||
// 信号减去直流分量
|
||
arm_offset_q15(adc_buf1,-dc_offset1,adc_buf1,len);
|
||
arm_offset_q15(adc_buf2,-dc_offset2,adc_buf2,len);
|
||
|
||
// 上传波形到上位机
|
||
#ifdef UPDATA_FROM_SERIAL_PORT
|
||
uint32_t num = len;
|
||
uart_dev_write(g_term_uart_handle,"x",1);
|
||
uart_dev_write(g_term_uart_handle,(uint8_t*)&num,4);
|
||
uart_dev_write(g_term_uart_handle,(uint8_t*)adc_buf1,2*len);
|
||
uart_dev_write(g_term_uart_handle,"\r\n",strlen("\r\n"));
|
||
|
||
uart_dev_write(g_term_uart_handle,"y",1);
|
||
uart_dev_write(g_term_uart_handle,(uint8_t*)&len,4);
|
||
uart_dev_write(g_term_uart_handle,(uint8_t*)adc_buf2,2*len);
|
||
uart_dev_write(g_term_uart_handle,"\r\n",strlen("\r\n"));
|
||
#endif
|
||
|
||
tofx = cal_tof(adc_buf1,len)/ADC_SAMP_RATE_MHz+REV_MUTE_DELAY_US-1.0f/DRIVE_FREQ_MHz;
|
||
tofy = cal_tof(adc_buf2,len)/ADC_SAMP_RATE_MHz+REV_MUTE_DELAY_US-1.0f/DRIVE_FREQ_MHz;
|
||
//remove_aftershocks(adc_buf1,len);
|
||
//remove_aftershocks(adc_buf2,len);
|
||
|
||
|
||
#ifdef ENABLE_FIR_FILTER
|
||
// 波形取绝对值
|
||
arm_abs_q15(adc_buf1,adc_buf1,len);
|
||
arm_abs_q15(adc_buf2,adc_buf2,len);
|
||
|
||
// 提取包络线 低通滤波器 -3db 50khz -40db 150khz
|
||
arm_q15_to_float(adc_buf1,x_buf,len);
|
||
arm_q15_to_float(adc_buf2,y_buf,len);
|
||
|
||
|
||
arm_fir_init_f32(&filter_s,NUM_TAPS,(float32_t *)&firCoeffs32LP[0],&firState[0],len);
|
||
arm_fir_f32(&filter_s,x_buf,x_buf,len);
|
||
arm_fir_f32(&filter_s,y_buf,y_buf,len);
|
||
|
||
arm_float_to_q15(x_buf,adc_buf1,len);
|
||
arm_float_to_q15(y_buf,adc_buf2,len);
|
||
|
||
#endif
|
||
|
||
|
||
#ifdef USE_CORRX_GET_DTOF
|
||
// 通过互相关算法计算时间差
|
||
dtof = cal_dtof(adc_buf1,adc_buf2,len)/ADC_SAMP_RATE_MHz;
|
||
#else
|
||
// 通过各通道渡越时间求时间差
|
||
dtof = tofx-tofy;
|
||
#endif
|
||
|
||
parm->wind_c = DISTANCE/2.0f*(1.0f/tofx+1.0f/tofy);
|
||
// v = L*dtof/2/tofx/tofy/cos
|
||
if(direction == WIND_DIRECTION_X)
|
||
{
|
||
parm->wind_velocity_x = 0-DISTANCE*dtof/1.41422f/tofx/tofx;
|
||
//parm->wind_velocity_x = DISTANCE*c_dtof/1.41422/tofx/tofx;
|
||
}
|
||
else
|
||
{
|
||
parm->wind_velocity_y = DISTANCE*dtof/1.41422f/tofx/tofx;
|
||
//parm->wind_velocity_y = DISTANCE*c_dtof/1.41422/tofx/tofx;
|
||
}
|
||
}
|
||
|
||
|
||
void wind_task(void const * argument)
|
||
{
|
||
/* USER CODE BEGIN wind_task */
|
||
/* Infinite loop */
|
||
//U_DataType wind_speed_data[20];
|
||
//U_DataType wind_direcion_data[20];
|
||
//int cnt=0;
|
||
int flag_init_msc_value = 1;
|
||
for(;;)
|
||
{
|
||
__HAL_TIM_DISABLE(&htim16);
|
||
// 采集X轴风速耗时 22ms，两轴采集完44ms
|
||
__HAL_TIM_SET_COUNTER(&htim16,0);
|
||
// 开启定时器
|
||
__HAL_TIM_ENABLE(&htim16);
|
||
|
||
// 通道1 通道2 测试南北风速
|
||
// 通道1发送 通道2接收
|
||
change_channel(0x01);
|
||
// 等待通道切换稳定
|
||
// adc开启采集数据有个固定的延时,这里取消等待
|
||
//HAL_Delay(0);
|
||
// 发送pwm 并启动adc采集数据
|
||
play_one_measure(adc_val,ADC_VAL_LEN);
|
||
//HAL_Delay(5);
|
||
osDelay(1);
|
||
// 通道2发送 通道1接收
|
||
change_channel(0x02);
|
||
// 等待通道切换稳定
|
||
//HAL_Delay(0);
|
||
// 发送pwm 并启动adc采集数据
|
||
play_one_measure(adc_val1,ADC_VAL_LEN);
|
||
//calculate_param(&weather_info,WIND_DIRECTION_X,adc_val,adc_val1,ADC_VAL_LEN);
|
||
calculate_tof_dtof_param(&weather_info,WIND_DIRECTION_X,adc_val,adc_val1,ADC_VAL_LEN);
|
||
|
||
// if(weather_info.wind_velocity_x > 2.0 )
|
||
// {
|
||
// term_printf("err \r\n");
|
||
// }
|
||
// 通道3 通道4 测试东西风速
|
||
// 通道3发送 通道4接收
|
||
change_channel(0x03);
|
||
// 等待通道切换稳定
|
||
//HAL_Delay(0);
|
||
// 发送pwm 并启动adc采集数据
|
||
play_one_measure(adc_val,ADC_VAL_LEN);
|
||
//HAL_Delay(5);
|
||
osDelay(1);
|
||
// 通道4发送 通道3接收
|
||
change_channel(0x04);
|
||
// 等待通道切换稳定
|
||
//HAL_Delay(0);
|
||
// 发送pwm 并启动adc采集数据
|
||
play_one_measure(adc_val1,ADC_VAL_LEN);
|
||
|
||
//calculate_param(&weather_info,WIND_DIRECTION_Y,adc_val,adc_val1,ADC_VAL_LEN);
|
||
calculate_tof_dtof_param(&weather_info,WIND_DIRECTION_Y,adc_val,adc_val1,ADC_VAL_LEN);
|
||
|
||
weather_info.wind_velocity = sqrtf(weather_info.wind_velocity_x*weather_info.wind_velocity_x + weather_info.wind_velocity_y*weather_info.wind_velocity_y);
|
||
// 分母加0.0001 保证分母不为0
|
||
weather_info.wind_angle = acosf(weather_info.wind_velocity_x/(weather_info.wind_velocity+0.0001));
|
||
|
||
// 关闭定时器
|
||
__HAL_TIM_DISABLE(&htim16);
|
||
speedx[speedi] = weather_info.wind_velocity_x;
|
||
speedy[speedi] = weather_info.wind_velocity_y;
|
||
speed[speedi] = weather_info.wind_velocity;
|
||
angle[speedi++] = weather_info.wind_angle*180/PI;
|
||
|
||
if(speedi>=AV_SPEED_LEN)
|
||
{
|
||
speedi=0;
|
||
arm_mean_f32(speedx,AV_SPEED_LEN,&av_speedx);
|
||
arm_mean_f32(speedy,AV_SPEED_LEN,&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)
|
||
av_angle = 360-av_angle;
|
||
if(fabs(av_speed)<0.1)
|
||
{
|
||
av_speed = 0;
|
||
av_angle = 0;
|
||
}
|
||
// arm_mean_f32(speed,AV_SPEED_LEN,&av_speed);
|
||
// arm_mean_f32(angle,AV_SPEED_LEN,&av_angle);
|
||
///term_printf("x:%.2f y:%.2f win_speed %.2f m/s angle %.2f \r\n",av_speedx,av_speedy,av_speed,av_angle);
|
||
}
|
||
|
||
///term_printf("win_speed %.2f \r\n",weather_info.wind_velocity);
|
||
//HAL_Delay(1);
|
||
//osDelay(3//
|
||
//kan ni osDelay(6);
|
||
#if 0
|
||
if(cnt < 10){
|
||
wind_speed_data[cnt].fValue = weather_info.wind_c;
|
||
wind_direcion_data[cnt].fValue = weather_info.wind_velocity;
|
||
cnt++;
|
||
}else{
|
||
cnt=0;
|
||
U_DataType tmp_wind_speed_value = filter_middle(wind_speed_data,10, FILTER_DATA_TYPE_FLOAT);
|
||
U_DataType tmp_wind_direction_value = filter_middle(wind_direcion_data,10, FILTER_DATA_TYPE_FLOAT);
|
||
update_mcs_param(tmp_wind_speed_value.fValue, tmp_wind_direction_value.fValue);
|
||
}
|
||
#endif
|
||
osDelay(38);
|
||
if(flag_init_msc_value== 1){
|
||
flag_init_msc_value = 0;
|
||
g_stMcs_Para.min_wind_direction = weather_info.wind_c;
|
||
g_stMcs_Para.average_wind_direction = weather_info.wind_c;
|
||
g_stMcs_Para.max_wind_direction = 0;
|
||
|
||
g_stMcs_Para.min_wind_speed = weather_info.wind_velocity;
|
||
g_stMcs_Para.average_wind_speed = weather_info.wind_velocity;
|
||
g_stMcs_Para.max_wind_speed = 0;
|
||
}
|
||
update_mcs_param(weather_info.wind_velocity, weather_info.wind_c);
|
||
}
|
||
/* USER CODE END wind_task */
|
||
}
|
||
|
||
/* 更新微气象站气象数据 */
|
||
/* 风向、风向瞬时值：3s的滑动平均值，以1s为步长
|
||
* 风速、风向平均值：以1min为步长，求xx分钟的滑动平均值，默认10分钟平均风速风向
|
||
* 最大风速、风向：从10min平均风速中选取；
|
||
*/
|
||
#define K_3S (3*1/3)
|
||
#define K_1MIN (3*3/60)
|
||
#define K_10MIN (3*60/600)
|
||
|
||
int time_1s=0,time_11s=0,time_111s=0;
|
||
int flag_time_1s=0,flag_time_3s=0,flag_time_1min=0,flag_time_10min=0;
|
||
float Yn_sp_3s_average_value=0.0,Yn_1_sp_3s_average_value=0.0,yn_3s_sp_value=0.0;
|
||
float Yn_sp_1min_average_value=0.0,Yn_1_sp_1min_average_value=0.0,yn_1min_sp_value=0.0;
|
||
float Yn_sp_10min_average_value=0.0,Yn_1_sp_10min_average_value;
|
||
|
||
float Yn_dir_3s_average_value=0.0,Yn_1_dir_3s_average_value=0.0,yn_3s_dir_value=0.0;
|
||
float Yn_dir_1min_average_value=0.0,Yn_1_dir_1min_average_value=0.0,yn_1min_dir_value=0.0;;
|
||
float Yn_dir_10min_average_value=0.0,Yn_1_dir_10min_average_value;
|
||
|
||
float max_speed_value=0.0,max_direction_value=0.0,min_speed_value,min_direction_value=0.0;
|
||
void update_mcs_param(float new_wind_speed, float new_wind_dirction)
|
||
{
|
||
static int flag1=0;
|
||
static int flag11=0;
|
||
static int flag111=0;
|
||
|
||
|
||
time_1s++;
|
||
if(time_1s==3){
|
||
time_1s=0;
|
||
flag_time_3s=1;
|
||
}
|
||
|
||
time_11s++;
|
||
if(time_11s==20){
|
||
time_11s=0;
|
||
flag_time_1min=1;
|
||
}
|
||
|
||
time_111s++;
|
||
if(time_111s==g_stConfigInfo.speed_average_time*60){
|
||
time_111s=0;
|
||
flag_time_10min = 1;
|
||
}
|
||
|
||
/* 以1s为步长，计算3s风速风向滑动平均值 */
|
||
flag_time_1s=1;
|
||
if(flag_time_1s){
|
||
|
||
if(flag1 ==0){
|
||
Yn_1_sp_3s_average_value = new_wind_speed;
|
||
Yn_1_dir_3s_average_value=new_wind_dirction;
|
||
flag1=1;
|
||
}
|
||
|
||
|
||
//flag_time_1s=0;
|
||
Yn_sp_3s_average_value = K_3S*(new_wind_speed - Yn_1_sp_3s_average_value) + Yn_1_sp_3s_average_value;
|
||
Yn_1_sp_3s_average_value = Yn_sp_3s_average_value;
|
||
|
||
Yn_dir_3s_average_value = K_3S*(new_wind_dirction - Yn_1_dir_3s_average_value) + Yn_1_dir_3s_average_value;
|
||
Yn_1_dir_3s_average_value = Yn_dir_3s_average_value;
|
||
|
||
}
|
||
|
||
/* 以3s为步长，计算1min滑动平均风速 */
|
||
if(flag_time_3s){
|
||
if(flag11 ==0){
|
||
Yn_1_sp_1min_average_value = Yn_sp_3s_average_value;
|
||
Yn_1_dir_1min_average_value = Yn_dir_3s_average_value;
|
||
flag11=1;
|
||
}
|
||
|
||
|
||
flag_time_3s=0;
|
||
Yn_sp_1min_average_value = K_1MIN*(Yn_sp_3s_average_value - Yn_1_sp_1min_average_value) + Yn_1_sp_1min_average_value;
|
||
Yn_1_sp_1min_average_value = Yn_sp_1min_average_value;
|
||
|
||
Yn_dir_1min_average_value = K_1MIN*(Yn_dir_3s_average_value - Yn_1_dir_1min_average_value) + Yn_1_dir_1min_average_value;
|
||
//Yn_1_dir_1min_average_value = Yn_dir_1min_averの‘dvv,f;age_value;
|
||
}
|
||
|
||
/* 以1min为步长，计算10min滑动平均风速 */
|
||
if(flag_time_1min){
|
||
|
||
if(flag111 ==0){
|
||
Yn_1_sp_10min_average_value = Yn_sp_1min_average_value;
|
||
Yn_1_dir_10min_average_value = Yn_dir_1min_average_value;
|
||
flag111=1;
|
||
}
|
||
|
||
flag_time_1min = 0;
|
||
Yn_sp_10min_average_value = K_10MIN*(Yn_sp_1min_average_value - Yn_1_sp_10min_average_value) + Yn_1_sp_10min_average_value;
|
||
Yn_1_sp_10min_average_value = Yn_sp_10min_average_value;
|
||
|
||
Yn_dir_10min_average_value = K_10MIN*(Yn_dir_1min_average_value - Yn_1_dir_10min_average_value) + Yn_1_dir_10min_average_value;
|
||
Yn_1_dir_10min_average_value = Yn_dir_10min_average_value;
|
||
}
|
||
|
||
/* 统计最大风速风向，及平均风速 */
|
||
if(flag_time_10min){
|
||
flag_time_10min = 0;
|
||
if(max_direction_value < Yn_dir_10min_average_value){
|
||
max_direction_value = Yn_dir_10min_average_value;
|
||
}
|
||
if(min_direction_value > Yn_dir_10min_average_value){
|
||
min_direction_value = Yn_dir_10min_average_value;
|
||
}
|
||
|
||
if(max_speed_value < Yn_sp_10min_average_value){
|
||
max_speed_value = Yn_sp_10min_average_value;
|
||
}
|
||
if(min_speed_value > Yn_sp_10min_average_value){
|
||
min_speed_value = Yn_sp_10min_average_value;
|
||
}
|
||
|
||
g_stMcs_Para.min_wind_direction = min_direction_value;
|
||
g_stMcs_Para.average_wind_direction = Yn_dir_10min_average_value;
|
||
g_stMcs_Para.max_wind_direction = max_direction_value;
|
||
|
||
g_stMcs_Para.min_wind_speed = min_speed_value;
|
||
g_stMcs_Para.average_wind_speed = Yn_sp_10min_average_value;
|
||
g_stMcs_Para.max_wind_speed = max_speed_value;
|
||
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|