#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"
#include "FIR.h"
#include "LowPassFilter.h"
#define AVE_TIME 600 //滑动平均时间，秒，最大600
#define COVER_TINE 10 //探头遮挡后导致的接收次数过小的报错临界次数
#define LOW_PASS_ALPHA 0.30 //一阶低通滤波器系数
uint32_t run_time_us;
// fft检验波形有效性
arm_rfft_fast_instance_f32 s;
#define FFT_DATA_LEN 256
// 浮点数buf
float32_t rfft_float_buf[FFT_DATA_LEN];
// fft结果
float32_t fft_out_f32[ADC_VAL_LEN] = {0};
// 波形最大点
void addValue(AverageCalculator *calc, uint16_t value) {
calc->values[calc->currentIndex] = value;
calc->currentIndex = (calc->currentIndex + 1) % MAX_VALUES;
if (calc->count < MAX_VALUES) {
calc->count++;
}
uint32_t sum = 0;
for (int i = 0; i < calc->count; i++) {
sum += calc->values[i];
}
calc->ave = sum / calc->count;
}
AverageCalculator Test1;
AverageCalculator Test2;
AverageCalculator Test3;
AverageCalculator Test4;
uint16_t wave_max_val1;
uint16_t wave_max_val2;
uint16_t wave_max_val3;
uint16_t wave_max_val4;
int16_t adc_val[ADC_VAL_LEN];
int16_t adc_val1[ADC_VAL_LEN];
#define AV_SPEED_LEN 5
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};
error_log g_error_log={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;
b=b+0.0000001f;
w_val = acosf((a+c)/2.0f/b);
sin_val = sinf(w_val)+0.0000001f;
y_val = atanf((a-c)/2.0f/b/sin_val);
// if(isnan(w_val)||isnan(sin_val)||isnan(y_val))
// {
// term_printf("isnan \r\n");
//
// }
return (0.0f-y_val)/(w_val+0.0000001f);
}
float32_t RSSI;
/*****************滤波器BUFF**********/
float32_t buf[ADC_VAL_LEN];
float32_t buf2[ADC_VAL_LEN];
/*****************滤波器BUFF**********/
float32_t cal_tof(q15_t* x,uint32_t len, int16_t *maxVal)
// float32_t cal_tof(q15_t* x,uint32_t len)
{
q15_t max_val,dc_offset;
float32_t echo_p = 0,echo_dt = 0;
uint32_t max_val_p;
uint32_t i=0;//stop_position = 0;
static uint32_t fft_200khz_pos;
// 计算直流分量 因为数据前端是没有回波的 计算50个数据求平均值 获取直流分量
arm_mean_q15(x,50,&dc_offset);
// 信号减去直流分量
arm_offset_q15(x,-dc_offset,x,len);
/*****************滤波器**********/
// arm_mean_q15(x,50,&dc_offset);
// // 信号减去直流分量
// arm_offset_q15(x,-dc_offset,x,len);
// 转换成浮点数
// arm_q15_to_float(x,buf,len);
for(i= 0 ;i<len;i++ )
{
buf[i] = (float32_t)x[i];
}
firBPFFilter(buf,buf2,len);
// // 数据转换有异常 无法还原成整数
// arm_float_to_q15(buf2,firFilterTestOutData,len);
for(i= 0 ;i<len;i++ )
{
<<<<<<< HEAD
=======
// x[i] = (int16_t)(buf2[i]*2.5f);
>>>>>>> MAX_WAVE
x[i] = (int16_t)(buf2[i]);
}
/*****************滤波器**********/
// fft 中 200khz 所在的位置。
fft_200khz_pos=(uint32_t)roundf((0.2f/(ADC_SAMP_RATE_MHz/FFT_DATA_LEN)));
// 查找数组中的最大值和最大值所在的索引
arm_max_q15(x,len,&max_val,&max_val_p);
// 最大值前后128个点数据的地址这里预防数组越界
q15_t* fft_data_q15_buf;
uint32_t fft_data_add;
if(max_val_p>=FFT_DATA_LEN/2)
fft_data_add=max_val_p-FFT_DATA_LEN/2;
else
fft_data_add = 0;
// 找到最大值前128个点的地址
fft_data_q15_buf = &(x[fft_data_add]);
// 转换成浮点数
arm_q15_to_float(fft_data_q15_buf,rfft_float_buf,FFT_DATA_LEN);
// fft进行转换。
arm_rfft_fast_f32(&s,rfft_float_buf,fft_out_f32,0);
// 对fft结果取模
arm_cmplx_mag_f32(fft_out_f32,rfft_float_buf,FFT_DATA_LEN);
// 统计200khz 附近的信号强度
RSSI = rfft_float_buf[fft_200khz_pos-2]+rfft_float_buf[fft_200khz_pos-1]+ rfft_float_buf[fft_200khz_pos]+rfft_float_buf[fft_200khz_pos+1]+rfft_float_buf[fft_200khz_pos+2];
// 如果RSSI小于
// 可变RSSI范围
// if(RSSI<0.2)
if(RSSI < ((float)g_stConfigInfo.RSSI_range/100.0))
return -1;
// 取出最大值
*maxVal = max_val;
// 可变参数
uint16_t max_val_zero_1R5 = max_val * g_stConfigInfo.transducer_cfg_1R5/100;
uint16_t max_val_zero_4R5 = max_val * g_stConfigInfo.transducer_cfg_4R5/100;
uint16_t max_val_zero_8R0 = max_val * g_stConfigInfo.transducer_cfg_8R0/100;
// // 大宇换能器参数 开始
// // 最大值的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);
//如果最大值位置大于8个周波 则从最大值前前8周波位置开始寻找起始波形。
// 优化的地方，从最大值位置开始找到达波,可以最大限度排除偶然噪声干扰，
// 因为噪声在波形到达出 噪声不是很大
//优化性能，如果不需要则从数组0位置开始寻找其实波形
if(max_val_p>=(uint32_t)(8*ADC_SAMP_RATE_MHz/DRIVE_FREQ_MHz))
{
i = max_val_p-(uint32_t)(8*ADC_SAMP_RATE_MHz/DRIVE_FREQ_MHz);
}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])
{
// 减去偏置电压
//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");
return -1;
}
/*
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);
}
//#define UPDATA_FROM_SERIAL_PORT 1
//#define DEBUG_TOF_ERR 1
float32_t tofx,tofy,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)
//{
// tofx = cal_tof(adc_buf1,len)/ADC_SAMP_RATE_MHz+REV_MUTE_DELAY_US-1.0f/DRIVE_FREQ_MHz+0.0001f;
// tofy = cal_tof(adc_buf2,len)/ADC_SAMP_RATE_MHz+REV_MUTE_DELAY_US-1.0f/DRIVE_FREQ_MHz+0.0001f;
// // 通过各通道渡越时间求时间差
// dtof = tofx-tofy;
//
// 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)
{
//错误次数统计,南北，东西分开(接受信号很小)
uint8_t tof_error_log_NS = 0;
uint8_t tof_error_log_WE = 0;
// int flag_init_msc_value = 1;
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);
// 采集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);
tofx = cal_tof(adc_val,ADC_VAL_LEN, &wave_max_val1);addValue(&Test1, wave_max_val1);
tofy = cal_tof(adc_val1,ADC_VAL_LEN, &wave_max_val2);addValue(&Test2, wave_max_val2);
// 接受信号很小
if(tofx<0||tofy<0)
{
tof_error_log_NS ++;
// 放弃本次采样，可以有效筛选雨滴等导致的异常大的风速数据
// 但是持续的遮挡会导致风速数据保持不变。
// 连续10次采样有问题判定为有遮挡，置错误标志位，将声速与风速分量置0
if(tof_error_log_NS >= COVER_TINE)
{
g_error_log.tof_error_NS = 1;
weather_info.wind_c_NS = 0;
weather_info.wind_velocity_x = 0;
}
else
{
continue;
}
// 手动设置渡越时间差为0，会在探头受遮挡的时候输出0，持续遮挡的时候也输出0，但是计算出声速将变得很大
// tofx = tofx/ADC_SAMP_RATE_MHz+REV_MUTE_DELAY_US-1.0f/DRIVE_FREQ_MHz+0.0001f;
// tofy = tofy/ADC_SAMP_RATE_MHz+REV_MUTE_DELAY_US-1.0f/DRIVE_FREQ_MHz+0.0001f;
// dtof = 0;
}else{
// 遮挡没有之后数据正常，将标志位取消，错误计数置零
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 - 2.0f;
tofy = ((tofy-FIR_PHASE_DELAY)/ADC_SAMP_RATE_MHz)+REV_MUTE_DELAY_US-1.0f/DRIVE_FREQ_MHz+0.0001f - 2.0f;
// 通过各通道渡越时间求时间差
dtof = tofx-tofy;
weather_info.wind_c_NS = (g_stConfigInfo.transducer_distace_NS * 2.0f)/2.0f*(1.0f/tofx+1.0f/tofy);
// weather_info.wind_velocity_x = 0-DISTANCE*dtof/1.41422f/tofx/tofx;
// 修正公式
weather_info.wind_velocity_x = 0-(g_stConfigInfo.transducer_distace_NS * 2.0f)*dtof/TWO_COS45/tofx/tofy;
}
// 通道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);
tofx = cal_tof(adc_val,ADC_VAL_LEN, &wave_max_val3);addValue(&Test3, wave_max_val3);
tofy = cal_tof(adc_val1,ADC_VAL_LEN, &wave_max_val4);addValue(&Test4, wave_max_val4);
// 如果测量的信号幅值过小。
if(tofx<0||tofy<0)
{
tof_error_log_WE ++;
// 放弃本次采样，可以有效筛选雨滴等导致的异常大的风速数据
// 但是持续的遮挡会导致风速数据保持不变。
// 连续10次采样有问题判定为有遮挡，置错误标志位，将声速与风速分量置0
if(tof_error_log_WE >= COVER_TINE)
{
g_error_log.tof_error_WE = 1;
weather_info.wind_c_WE = 0;
weather_info.wind_velocity_y = 0;
}
else
{
continue;
}
// 手动设置渡越时间差为0，会在探头受遮挡的时候输出0，持续遮挡的时候也输出0，但是计算出声速将变得很大
// tofx = tofx/ADC_SAMP_RATE_MHz+REV_MUTE_DELAY_US-1.0f/DRIVE_FREQ_MHz+0.0001f;
// tofy = tofy/ADC_SAMP_RATE_MHz+REV_MUTE_DELAY_US-1.0f/DRIVE_FREQ_MHz+0.0001f;
// // 通过各通道渡越时间求时间差
// dtof = 0;
}else{
// 遮挡没有之后数据正常，将标志位取消，错误计数置零
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 - 2.0f;
tofy = ((tofy-FIR_PHASE_DELAY)/ADC_SAMP_RATE_MHz)+REV_MUTE_DELAY_US-1.0f/DRIVE_FREQ_MHz+0.000001f - 2.0f;
// 通过各通道渡越时间求时间差
dtof = tofx-tofy;
weather_info.wind_c_WE = (g_stConfigInfo.transducer_distace_WE * 2.0f)/2.0f*(1.0f/tofx+1.0f/tofy);
// weather_info.wind_velocity_y = DISTANCE*dtof/1.41422f/tofx/tofx;
// 修正公式
weather_info.wind_velocity_y = (g_stConfigInfo.transducer_distace_WE * 2.0f)*dtof/TWO_COS45/tofx/tofy;
}
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.000001f));
// 关闭定时器·
__HAL_TIM_DISABLE(&htim16);
// 显示时间 单位us
run_time_us = __HAL_TIM_GET_COUNTER(&htim16) * 100;// htim16).Instance->CNT;
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_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(g_error_log.tof_error_NS || g_error_log.tof_error_WE)
{
av_speed = 0;
av_angle = 0;
}
// 360一圈
if(av_speedy<0)
av_angle = 360-av_angle;
//针对粘度进行校准
av_speed = av_speed * ((float)g_stConfigInfo.Coefficient/10000.0);
// 速度太小视为0风速
if(fabs(av_speed)<0.1)
{
av_speed = 0;
av_angle = 0;
}
/// term_printf("x:%.2f y:%.2f win_speed %.2f m/s angle %.2f \r\n",av_speedx,av_speedy,av_speed,av_angle);
}
//算法求出的瞬时风速风向
weather_info.instantaneous_wind_direction = av_angle;
weather_info.instantaneous_wind_speed = av_speed;
///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_usrConfigInfo.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};
SlidingWindow_3s win_3s = {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;
// 线性插值函数
//float linear_interpolation(float x0, float y0, float x1, float y1, float x) {
float linear_interpolation(float x) {
float y;
//开始插值
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)
{
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;
}
}
void my_update_mcs_param(float new_wind_speed, float new_wind_dirction)
{
///// 三秒滑动平均
// win_3s.speed_data[win_3s.index] = new_wind_speed;
// win_3s.direction_data[win_3s.index] = new_wind_dirction;
// if(win_3s.count < 3)
// {
// win_3s.count++;
// }
// //计算三秒风速滑动平均值当作瞬时值
// win_3s.ave_speed_data = sum(win_3s.speed_data, win_3s.count) / win_3s.count;
// //计算三秒风向滑动平均值，风向滑动平均值需要过零算法
// float temp_sin_sum_3s = 0;
// float temp_cos_sum_3s = 0;
// for(int i = 0; i < win_3s.count; i++)
// {
// temp_sin_sum_3s += sinf(win_3s.direction_data[i] * PI/180);
// temp_cos_sum_3s += cosf(win_3s.direction_data[i] * PI/180);
// }
// win_3s.ave_direction_data = atanf(temp_sin_sum_3s / (temp_cos_sum_3s + 0.00001)) * 180/PI;
// // 不同象限不一样
// // 1象限真实角度=本身
// // 2象限真实角度=+180
// // 3象限真实角度=+180
// // 4象限真实角度=+360
// if((temp_sin_sum_3s > 0 && temp_cos_sum_3s < 0) || (temp_sin_sum_3s < 0 && temp_cos_sum_3s < 0))
// {
// win_3s.ave_direction_data += 180;
// }else if (temp_sin_sum_3s < 0 && temp_cos_sum_3s > 0)
// {
// win_3s.ave_direction_data += 360;
// }
// g_stMcs_Para.instantaneous_wind_direction = win_3s.ave_direction_data;
//
// /** 线性插值 **/
// /** win_3s.ave_speed_data[win_3s.index]为风速 **/
// /** win_3s.ave_direction_data[win_3s.index]为风向 **/
// /** 风向不要插值 **/
// /** 只用插值瞬时风速，10min便会是插值后的值 **/
// g_stMcs_Para.instantaneous_wind_speed = linear_interpolation(win_3s.ave_speed_data);
// /** 线性插值 **/
//// 更新索引
// win_3s.index = (win_3s.index + 1) % 3;
/** 线性插值 **/
/** 风向不要插值 **/
/** 只用插值瞬时风速，10min便会是插值后的值 **/
g_stMcs_Para.instantaneous_wind_speed = linear_interpolation(new_wind_speed);
/** 线性插值 **/
g_stMcs_Para.instantaneous_wind_direction = new_wind_dirction;
/// 十分钟滑动平均值
win_10min.speed_data[win_10min.index] = g_stMcs_Para.instantaneous_wind_speed; //添加新数据
win_10min.direction_data[win_10min.index] = g_stMcs_Para.instantaneous_wind_direction;
if(win_10min.count < g_usrConfigInfo.speed_average_time /*AVE_TIME*/)
{
win_10min.count++;
}
if(win_10min.count > g_usrConfigInfo.speed_average_time/*AVE_TIME*/){win_10min.count = /*AVE_TIME*/g_usrConfigInfo.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];
float temp_trough_min_speed = win_10min.speed_data[0];
float temp_peak_max_speed = win_10min.speed_data[0];
float temp_trough_min_direction = win_10min.direction_data[0];
float temp_peak_max_direction = win_10min.direction_data[0];
//遍历10min内所有数据寻找最大最小极大极小
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]; // 更新风向最小值
}
else 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]; // 更新风速最小值
}
else if (win_10min.ave_speed_data[i] > temp_max_speed) {
temp_max_speed = win_10min.ave_speed_data[i]; // 更新风速最大值
}
// 极大极小
if (win_10min.speed_data[i] < temp_trough_min_speed) {
temp_trough_min_speed = win_10min.speed_data[i]; // 更新风速极小值
}
else if (win_10min.speed_data[i] > temp_peak_max_speed) {
temp_peak_max_speed = win_10min.speed_data[i]; // 更新风速极大值
}
if (win_10min.direction_data[i] < temp_trough_min_direction) {
temp_trough_min_direction = win_10min.direction_data[i]; // 更新风向极小值
}
else if (win_10min.direction_data[i] > temp_peak_max_direction) {
temp_peak_max_direction = win_10min.direction_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;
// 极大极小风速与极大极小风向
g_stMcs_Para.trough_wind_speed = temp_trough_min_speed;
g_stMcs_Para.peak_wind_speed = temp_peak_max_speed;
g_stMcs_Para.trough_wind_direction = temp_trough_min_direction;
g_stMcs_Para.peak_wind_direction = temp_peak_max_direction;
win_10min.index = (win_10min.index + 1) % /*AVE_TIME*/g_usrConfigInfo.speed_average_time;//更新索引
}
void tem_hum_update_task(void const * argument)
{
int time_s_temp_humi = 0;
uint32_t time_s_1Day = 0;
float backupTemperature;
// 开机先采集一次大气压温湿度
{
// 采集HP203B传感器数据（大气压）
if(get_HP203_data(&backupTemperature, &g_stMcs_Para.pressure) == FALSE)
{
g_error_log.temp_error_HP203B = 1;
/// 错误处理
}
// 采集SHT30传感器数据（温湿度）
if(get_temp_humi_data(&g_stMcs_Para.temperature, &g_stMcs_Para.humidity) == FALSE)
{
g_error_log.temp_error_SHT30 = 1;
/// 错误处理
g_stMcs_Para.temperature = backupTemperature;
}
}
while(1)
{
osDelay(1000);
time_s_temp_humi ++;
time_s_1Day ++;
// 温湿度大气压更新
if (time_s_temp_humi >= g_usrConfigInfo.temp_hum_update_time)
{
// 采集HP203B传感器数据（大气压）
if(get_HP203_data(&backupTemperature, &g_stMcs_Para.pressure) == FALSE)
{
g_error_log.temp_error_HP203B = 1;
/// 错误处理
}
else
{
// 没出问题清除错误日志
g_error_log.temp_error_HP203B = 0;
}
// 采集SHT30传感器数据（温湿度）
if(get_temp_humi_data(&g_stMcs_Para.temperature, &g_stMcs_Para.humidity) == FALSE)
{
g_error_log.temp_error_SHT30 = 1;
/// 错误处理
g_stMcs_Para.temperature = backupTemperature;
}
else
{
// 没出问题清除错误日志
g_error_log.temp_error_SHT30 = 0;
}
// 计时重置
time_s_temp_humi = 0;
}
// 一天重启
if (time_s_1Day >= 86400)
{
__iar_builtin_set_FAULTMASK(1);
NVIC_SystemReset();
}
// 风速风向更新
my_update_mcs_param(weather_info.instantaneous_wind_speed, weather_info.instantaneous_wind_direction);
}
}