dengcy
/
micro_climate
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
micro_climate/App/Src/anemometer_dev.c

872 lines
28 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

#include "anemometer_dev.h"
#include "FreeRTOS.h"
#include "filter.h"
#include "inflash.h"
#include "uart_dev.h"
#include "fdacoefs.h"
#include "sht30.h"
#include "hp203b.h"
#define AVE_TIME 600 //滑动平均时间，秒，最大600
#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;
}
}
SlidingWindow_10min win_10min = {0};
//求和函数
float sum(float arr[], int n)
{
float total = 0;
for(int i = 0; i < n; i++)
{
total += arr[i];
}
return total;
}
int times_1s_3sec = 0;
int times_1s_1min = 0;
void my_update_mcs_param(float new_wind_speed, float new_wind_dirction)
{
// 十分钟滑动平均值
win_10min.speed_data[win_10min.index] = new_wind_speed; //添加新数据
win_10min.direction_data[win_10min.index] = new_wind_dirction;
if(win_10min.count < /* g_stConfigInfo.speed_average_time */ AVE_TIME)
{
win_10min.count++;
}
if(win_10min.count > /* g_stConfigInfo.speed_average_time */600){win_10min.count = AVE_TIME/* g_stConfigInfo.speed_average_time */;}
//计算10min风速滑动平均值
win_10min.ave_speed_data[win_10min.index] = sum(win_10min.speed_data, win_10min.count) / win_10min.count;
//计算10min风向滑动平均值，风向滑动平均值需要过零算法
float temp_sin_sum = 0;
float temp_cos_sum = 0;
for(int i = 0; i < win_10min.count; i++)
{
temp_sin_sum += sinf(win_10min.direction_data[i] * PI/180);
temp_cos_sum += cosf(win_10min.direction_data[i] * PI/180);
}
win_10min.ave_direction_data[win_10min.index] = atanf(temp_sin_sum / (temp_cos_sum + 0.0001)) * 180/PI;
// 不同象限不一样
// 1象限真实角度=本身
// 2象限真实角度=+180
// 3象限真实角度=+180
// 4象限真实角度=+360
if((temp_sin_sum > 0 && temp_cos_sum < 0) || (temp_sin_sum < 0 && temp_cos_sum < 0))
{
win_10min.ave_direction_data[win_10min.index] += 180;
}else if (temp_sin_sum < 0 && temp_cos_sum > 0)
{
win_10min.ave_direction_data[win_10min.index] += 360;
}
//默认第一个数据为最大或者最小
float temp_min_direction = win_10min.ave_direction_data[0];
float temp_max_direction = win_10min.ave_direction_data[0];
float temp_min_speed = win_10min.ave_speed_data[0];
float temp_max_speed = win_10min.ave_speed_data[0];
//统计
for (int i = 0; i < win_10min.count; i++) {
if (win_10min.ave_direction_data[i] < temp_min_direction) {
temp_min_direction = win_10min.ave_direction_data[i]; // 更新风向最小值
}
if (win_10min.ave_direction_data[i] > temp_max_direction) {
temp_max_direction = win_10min.ave_direction_data[i]; // 更新风向最大值
}
if (win_10min.ave_speed_data[i] < temp_min_speed) {
temp_min_speed = win_10min.ave_speed_data[i]; // 更新风速最小值
}
if (win_10min.ave_speed_data[i] > temp_max_speed) {
temp_max_speed = win_10min.ave_speed_data[i]; // 更新风速最大值
}
}
g_stMcs_Para.min_wind_direction = temp_min_direction;
g_stMcs_Para.average_wind_direction = win_10min.ave_direction_data[win_10min.index];
g_stMcs_Para.max_wind_direction = temp_max_direction;
g_stMcs_Para.min_wind_speed = temp_min_speed;
g_stMcs_Para.average_wind_speed = win_10min.ave_speed_data[win_10min.index];
g_stMcs_Para.max_wind_speed = temp_max_speed;
win_10min.index = (win_10min.index + 1) % AVE_TIME; //更新索引
}
void tem_hum_update_task(void const * argument)
{
int time_s_temp_humi = 0;
get_temp_humi_data(&g_stMcs_Para.temperature, &g_stMcs_Para.humidity);//开机先采集一次
while(1)
{
osDelay(1000);
time_s_temp_humi ++;
if (time_s_temp_humi >= g_stConfigInfo.temp_hum_update_time)
{
get_temp_humi_data(&g_stMcs_Para.temperature, &g_stMcs_Para.humidity);
time_s_temp_humi = 0;
}
my_update_mcs_param(av_speed, av_angle);
//采集HP203B数据(大气压)
Hp203bReadPressure();
}
}
Reference in New Issue View Git Blame Copy Permalink