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"
#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);
    }
    //瞬时风速风向
    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;
    
        
///    十分钟滑动平均值
        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);
    }
}