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

952 lines
32 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"
#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;
// 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};
//
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)
{
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++ )
{
x[i] = (int16_t)(buf2[i]*2.5f);
}
/*****************滤波器**********/
// 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 < ((float)g_stConfigInfo.RSSI_range/100.0))
// if(RSSI<0.2)
return -1;
// 可变参数
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);
tofy = cal_tof(adc_val1,ADC_VAL_LEN);
// 接受信号很小
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 = 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 = (g_stConfigInfo.transducer_distace * 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 * 2.0f)*dtof/1.41422f/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);
tofy = cal_tof(adc_val1,ADC_VAL_LEN);
// 如果测量的信号幅值过小。
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 = 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 = (g_stConfigInfo.transducer_distace * 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 * 2.0f)*dtof/1.41422f/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;
// 速度太小视为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);
}
//瞬时风速风向
g_stMcs_Para.instantaneous_wind_direction = av_angle;
g_stMcs_Para.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};
//求和函数
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_10min.speed_data[win_10min.index] = new_wind_speed; //添加新数据
win_10min.direction_data[win_10min.index] = new_wind_dirction;
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;
}
/** 线性插值 **/
/** win_10min.ave_speed_data[win_10min.index]为风速 **/
/** win_10min.ave_direction_data[win_10min.index]为风向 **/
/** 风向不要插值 **/
win_10min.ave_speed_data[win_10min.index] = linear_interpolation(win_10min.ave_speed_data[win_10min.index]);
/** 线性插值 **/
//默认第一个数据为最大或者最小
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.ave_speed_data[0];
float temp_peak_max_speed = win_10min.ave_speed_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]; // 更新风速极大值
}
}
// 更新最大最小极大极小风速风向
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;
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(g_stMcs_Para.instantaneous_wind_speed, g_stMcs_Para.instantaneous_wind_direction);
}
}
Reference in New Issue View Git Blame Copy Permalink