micro_climate/App/Src/anemometer_dev.c

#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();
    }
}