#include "device_adc_collect.h"
#include  <includes.h>
#include "gd32f4xx.h"
#include "comm_types.h"
#include "ptz_struct.h"
#include "tmp75.h"
#include "w25q128.h"
#include "agent_hyt.h"
#include "ptz_type_select.h"
#include "pdebug.h"
#include "rotate_plan.h"
#include "rotate_bldc.h"

ADC_Phase_current H_ADC_Collect;
ADC_Phase_current V_ADC_Collect;

//发送云台工作电压
void ptz_send_voltage(char dev)
{
  unsigned short int vol = 0;
  unsigned char VData[7]= {0xff,0x00,0x00,0x00,0x00,0x00,0x00};
  vol = ( unsigned short int)(g_ptz.Voltage * 100 + 0.5);
  VData[1] = g_ptz.address;
  VData[2] = 0xcd;
  VData[3] = (unsigned char)(vol >> 8);
  VData[4] = (unsigned char)(vol & 0x00ff);
  VData[6] = MotorCalPelcoDSUM(VData,sizeof(VData));
  ptz_send_data(dev, VData, sizeof(VData));
  //发送调试数据
}

//发送云台工作电流
void ptz_send_current(char dev)
{
  unsigned short int current = 0;
  unsigned char IData[7]= {0xff,0x00,0x00,0x00,0x00,0x00,0x00};
  current = (unsigned short int)(g_ptz.electric_current * 100 + 0.5);
  IData[1] = g_ptz.address;
  IData[2] = 0xc8;
  IData[3] = (unsigned char)(current >> 8);
  IData[4] = (unsigned char)(current & 0x00ff);
  IData[6] = MotorCalPelcoDSUM(IData,sizeof(IData));
  
  ptz_send_data(dev, IData, sizeof(IData));
}  

//发送云台温度
void ptz_send_temperature(char dev)
{
  unsigned short int tem = 0;
  unsigned char TData[7]= {0xff,0x00,0x00,0x00,0x00,0x00,0x00};
  tem = (unsigned short int)(g_ptz.temperature * 100 + 0.5);
  TData[1] = g_ptz.address;
  TData[2] = 0xd6;
  TData[3] = (unsigned char)(tem >> 8);
  TData[4] = (unsigned char)(tem & 0x00ff);
  TData[6] = MotorCalPelcoDSUM(TData,sizeof(TData));
  
  ptz_send_data(dev,TData,sizeof(TData));
  
}


//发送全桥驱动电路温度
void ptz_send_bridge_temperature(char dev)
{
  unsigned short int tem = 0;
  unsigned char TData[7]= {0xff,0x00,0x00,0x00,0x00,0x00,0x00};
  //发送水平驱动侧
  tem = (unsigned short int)(g_ptz.H_boad_temp * 100 + 0.5);
  TData[1] = g_ptz.address;
  TData[2] = 0xd7;
  TData[3] = PTZ_HORI;
  TData[4] = (unsigned char)(tem >> 8);
  TData[5] = (unsigned char)(tem & 0x00ff);
  TData[6] = MotorCalPelcoDSUM(TData,sizeof(TData));
  
  ptz_send_data(dev,TData,sizeof(TData));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 5u);
  
  //发送垂直驱动侧
  tem = (unsigned short int)(g_ptz.V_boad_temp * 100 + 0.5);
  TData[1] = g_ptz.address;
  TData[2] = 0xd7;
  TData[3] = PTZ_VERT;
  TData[4] = (unsigned char)(tem >> 8);
  TData[5] = (unsigned char)(tem & 0x00ff);
  TData[6] = MotorCalPelcoDSUM(TData,sizeof(TData));
  
  ptz_send_data(dev,TData,sizeof(TData));
  
  
}




static float current_caculate(uint16_t I)
{
  float i;
  i = ((float)I/4096.0*3.3)/0.5;
  return i;
}



/*!
    \brief      ADC2初始化,水平电机相电流采集
    \param[in]  none
    \param[out] none
    \retval     none
    \note       LH  @2022.07.21
*/
static void ADC2_Init()
{
  /* 水平电机相电流：PC0-U，PA0-V，PA3-W ---ADC2*/
  //配置引脚时钟
  rcu_periph_clock_enable(RCU_GPIOA);
  rcu_periph_clock_enable(RCU_GPIOC);
  gpio_mode_set(GPIOC,GPIO_MODE_ANALOG,GPIO_PUPD_NONE,GPIO_PIN_0);
  gpio_mode_set(GPIOA,GPIO_MODE_ANALOG,GPIO_PUPD_NONE,GPIO_PIN_0);
  gpio_mode_set(GPIOA,GPIO_MODE_ANALOG,GPIO_PUPD_NONE,GPIO_PIN_3);
  
  
  rcu_periph_clock_enable(RCU_ADC2);
  //总线时钟20分频=6MHZ
  adc_clock_config(ADC_ADCCK_PCLK2_DIV4);
  /***********************************************/
  
  //对齐方式      ADC_INSERTED_CHANNEL
  adc_data_alignment_config(ADC2,ADC_DATAALIGN_RIGHT);
  //扫描模式
  adc_special_function_config(ADC2,ADC_SCAN_MODE,ENABLE);
  //外部触发关闭
  adc_external_trigger_config(ADC2,ADC_INSERTED_CHANNEL,DISABLE);
  //采集通道数
  adc_channel_length_config(ADC2,ADC_INSERTED_CHANNEL,3);
  
  //通道注入顺序
  adc_inserted_channel_config(ADC2,0,ADC_CHANNEL_10,ADC_SAMPLETIME_480);//u
  adc_inserted_channel_config(ADC2,1,ADC_CHANNEL_0,ADC_SAMPLETIME_480);//v
  adc_inserted_channel_config(ADC2,2,ADC_CHANNEL_3,ADC_SAMPLETIME_480);//w
  
  adc_enable(ADC2);
  
  adc_calibration_enable(ADC2);
}


/*!
    \brief      ADC0初始化,垂直电机相电流采集
    \param[in]  none
    \param[out] none
    \retval     none
    \note       LH  @2022.07.21
*/
static void ADC0_Init()
{
  /* 垂直电机相电流：PA4-U，PA5-V，PA6-W ---ADC0*/
  //配置引脚时钟
  rcu_periph_clock_enable(RCU_GPIOA);
  gpio_mode_set(GPIOA,GPIO_MODE_ANALOG,GPIO_PUPD_NONE,GPIO_PIN_4);
  gpio_mode_set(GPIOA,GPIO_MODE_ANALOG,GPIO_PUPD_NONE,GPIO_PIN_5);
  gpio_mode_set(GPIOA,GPIO_MODE_ANALOG,GPIO_PUPD_NONE,GPIO_PIN_6);
  
  
  rcu_periph_clock_enable(RCU_ADC0);
  //总线时钟20分频=6MHZ
  adc_clock_config(ADC_ADCCK_PCLK2_DIV4);
  /***********************************************/
  
  //对齐方式      ADC_INSERTED_CHANNEL
  adc_data_alignment_config(ADC0,ADC_DATAALIGN_RIGHT);
  //扫描模式
  adc_special_function_config(ADC0,ADC_SCAN_MODE,ENABLE);
  //外部触发关闭
  adc_external_trigger_config(ADC0,ADC_INSERTED_CHANNEL,DISABLE);
  //采集通道数
  adc_channel_length_config(ADC0,ADC_INSERTED_CHANNEL,3);
  
  //通道注入顺序
  adc_inserted_channel_config(ADC0,0,ADC_CHANNEL_4,ADC_SAMPLETIME_480);//u
  adc_inserted_channel_config(ADC0,1,ADC_CHANNEL_5,ADC_SAMPLETIME_480);//v
  adc_inserted_channel_config(ADC0,2,ADC_CHANNEL_6,ADC_SAMPLETIME_480);//w
  
  adc_enable(ADC0);
  
  adc_calibration_enable(ADC0);
}


/* H_NTC-PB0 ,V_NTC-PB1 ----ADC1*/
/*!
    \brief      ADC1初始化,电压,电流,NTC温度采集
    \param[in]  none
    \param[out] none
    \retval     none
    \note       LH  @2022.07.21
*/
static void ADC1_Init()
{
  //配置引脚时钟
  rcu_periph_clock_enable(RCU_GPIOC);
  rcu_periph_clock_enable(RCU_GPIOB);
  //引脚复用，PC2——电压ADC1-CH12，PC3——电流ADC1-CH13
  gpio_mode_set(GPIOC,GPIO_MODE_ANALOG,GPIO_PUPD_NONE,GPIO_PIN_2);
  gpio_mode_set(GPIOC,GPIO_MODE_ANALOG,GPIO_PUPD_NONE,GPIO_PIN_3);
  /* H_NTC-PB0 ,V_NTC-PB1 */
  gpio_mode_set(GPIOB,GPIO_MODE_ANALOG,GPIO_PUPD_NONE,GPIO_PIN_0);
  gpio_mode_set(GPIOB,GPIO_MODE_ANALOG,GPIO_PUPD_NONE,GPIO_PIN_1);
  
  
  rcu_periph_clock_enable(RCU_ADC1);
  //总线时钟20分频=6MHZ
  adc_clock_config(ADC_ADCCK_PCLK2_DIV4);
  /***********************************************/
  
  //对齐方式      ADC_INSERTED_CHANNEL
  adc_data_alignment_config(ADC1,ADC_DATAALIGN_RIGHT);
  //扫描模式
  adc_special_function_config(ADC1,ADC_SCAN_MODE,ENABLE);
  //外部触发关闭
  adc_external_trigger_config(ADC1,ADC_INSERTED_CHANNEL,DISABLE);
  //采集通道数
  adc_channel_length_config(ADC1,ADC_INSERTED_CHANNEL,4);
  
  //通道注入顺序
  adc_inserted_channel_config(ADC1,0,ADC_CHANNEL_13,ADC_SAMPLETIME_480);//电流
  adc_inserted_channel_config(ADC1,1,ADC_CHANNEL_12,ADC_SAMPLETIME_480);//电压
  adc_inserted_channel_config(ADC1,2,ADC_CHANNEL_8,ADC_SAMPLETIME_480);//H_NTC-PB0
  adc_inserted_channel_config(ADC1,3,ADC_CHANNEL_9,ADC_SAMPLETIME_480);//V_NTC-PB1
  
  
  adc_enable(ADC1);
  
  adc_calibration_enable(ADC1);
}



/*!
    \brief       水平电机相电流采集
    \param[in]  none
    \param[out] none
    \retval     none
    \note       LH  @2022.07.19
*/
void H_ADC2_Phase_current()
{  
  adc_software_trigger_enable(ADC2,ADC_INSERTED_CHANNEL);
  asm ("nop");
  asm ("nop");
  H_ADC_Collect.Phase_curr_U = current_caculate(ADC_IDATA0(ADC2));//U
  asm ("nop");
  H_ADC_Collect.Phase_curr_V = current_caculate(ADC_IDATA1(ADC2));//V
  asm ("nop");
  H_ADC_Collect.Phase_curr_W = current_caculate(ADC_IDATA2(ADC2));//W
  
}


/*!
    \brief       垂直电机相电流采集
    \param[in]  none
    \param[out] none
    \retval     none
    \note       LH  @2022.07.19
*/
void V_ADC0_Phase_current()
{  
//  int Ia,Ib,Ic;
  adc_software_trigger_enable(ADC0,ADC_INSERTED_CHANNEL);
  asm ("nop");
  asm ("nop");
//  Ia = ADC_IDATA0(ADC0);//U
  V_ADC_Collect.Phase_curr_U = current_caculate(ADC_IDATA0(ADC0));//U
  asm ("nop");
//  Ib = ADC_IDATA1(ADC0);//V
  V_ADC_Collect.Phase_curr_V = current_caculate(ADC_IDATA1(ADC0));//V
  asm ("nop");
//  Ic = ADC_IDATA2(ADC0);//W
  V_ADC_Collect.Phase_curr_W = current_caculate(ADC_IDATA2(ADC0));//W
  
}




#define R_constant  10.0  //千欧姆——分压电阻值

#define R_25     10000.0//热敏电阻常温下的电阻值

#define Ntc_B  3435.0//常量B

#define Kelvin_temp  273.15+25//常温——开尔文温度
//NTC电流
static float Ntc_current(uint16_t data)
{
  static float curr;
  curr = ((float)data / 4096.0 * 3.3)/R_constant;
  return curr;
  
}
//NTC电压
static float Ntc_voltage(uint16_t data)
{
  static float voltage;
  voltage = 3.3 - ((float)data / 4096.0 * 3.3);
  return voltage;
}
//NTC电阻
static float Ntc_resis(uint16_t data)
{
  static float resis,curr,voltage;
  curr = Ntc_current(data);
  voltage = Ntc_voltage(data);
  resis = voltage / curr * 1000.0;//因为电流值放大了1000倍
  return resis;
}




//NTC温度计算,开尔文温度
static float Ntc_temp(uint16_t adc_data)
{
  float R_ntc;//实际电阻
  double temp,K,j,l,m;//实际温度
  R_ntc = Ntc_resis(adc_data);
  K = R_ntc/10000.0;
  j = log(K);//log e为底数
  l = j/3435.0;
  m = 1.0 /298.15; 
  temp = 1/(l+m);
//  temp = 1/(j / Ntc_B + 1/Kelvin_temp);
  return (float)temp;
}


/*!
    \brief       热敏电阻计算主板温度
    \param[in]  none
    \param[out] none
    \retval     none
    \note       LH  @2022.07.19
*/
static void Ntc_value()//ADC1
{
  float H_temp_f,V_temp_f;//初始温度,开尔文温度
  short int H_temp,V_temp;//矫正温度,摄氏温度
  
  adc_software_trigger_enable(ADC1,ADC_INSERTED_CHANNEL);
  asm ("nop");
  asm ("nop");
  H_temp_f = Ntc_temp(ADC_IDATA2(ADC1));
  asm ("nop");
  asm ("nop");
  V_temp_f = Ntc_temp(ADC_IDATA3(ADC1));
  
  H_temp = (short int)(H_temp_f - 273.15 + 0.5);
  
  V_temp = (short int)(V_temp_f - 273.15 + 0.5);
  
  g_ptz.H_boad_temp = (float)H_temp;//该温度参数需要增加回传与查询
  
  g_ptz.V_boad_temp = (float)V_temp;
  
}


//电压采集
static void ptz_Voltage_collect_adc1_task()
{
  static float adc1_v[5];
  static unsigned char adc1_num;
  int j,k;
  float tem;
  float curadc1;
  uint16_t value_V;
  //软件触发使能
  adc_software_trigger_enable(ADC1,ADC_INSERTED_CHANNEL);
  value_V = ADC_IDATA1(ADC1);
  
  adc1_v[adc1_num] = (float)value_V / 4096.0 * 11 *3.3;//(float)value_V;//
  adc1_num++;
  if(adc1_num >= 5)
  {
    adc1_num = 0;
     for(j = 0; j < 5-1; j++)//采样值由小到大排列
     {
         for(k = 0; k < 5-j-1; k++)
         {
             if(adc1_v[k] > adc1_v[k+1])
             {
                tem      = adc1_v[k];
                adc1_v[k]   = adc1_v[k+1];
                adc1_v[k+1] = tem;
             }
          }
       }
      for(uint8_t i = 1; i < 5 - 1; i++)
      {
        curadc1 = curadc1 + adc1_v[i];
      }
    curadc1 = curadc1 /((float)(5 - 2));//去掉一个最大值和一个最小值求平均值
    g_ptz.Voltage =  curadc1;
    memset(adc1_v,0,sizeof(adc1_v));
//    pdebug(DEBUG_LEVEL_INFO,"get ptz Voltage: %.3fV",g_ptz.Voltage);
  }
  
}

//电流采集
static void ptz_Current_collect_adc1_task()
{
  static float adc1_i[5];
  static unsigned char adc0_num;
  int j,k;
  float tem;
  float curadc0;
  uint16_t value_I;
  
  adc_software_trigger_enable(ADC1,ADC_INSERTED_CHANNEL);
  value_I = ADC_IDATA0(ADC1);
  
  adc1_i[adc0_num] = (((float)value_I / 4096.0  * 3.3)-3.3/2) / 0.132;//(float)value_I;//
  
  adc0_num ++;
  if(adc0_num >= 5)
  {
    adc0_num = 0;
     for(j = 0; j < 5-1; j++)//采样值由小到大排列
     {
         for(k = 0; k < 5-j-1; k++)
         {
             if(adc1_i[k] > adc1_i[k+1])
             {
                tem      = adc1_i[k];
                adc1_i[k]   = adc1_i[k+1];
                adc1_i[k+1] = tem;
             }
          }
       }
      for(uint8_t i = 1; i < 5 - 1; i++)
      {
        curadc0 = curadc0 + adc1_i[i];
      }
    curadc0 = curadc0 /((float)(5 - 2));//去掉一个最大值和一个最小值求平均值
    g_ptz.electric_current =  curadc0;
    memset(adc1_i,0,sizeof(adc1_i));
//    pdebug(DEBUG_LEVEL_INFO,"get ptz Electric_Current: %.3fA",g_ptz.electric_current);
  }
  
}

//温度采集
static void ptz_temperature_collect_tmp75_task()
{
  static float tmp75[5];
  static unsigned char tmp75_num;
  float curtmp75;
  float tem;
  int j,k;
  tmp75[tmp75_num] = tmp75_read_temp();
  tmp75_num ++;
  if(tmp75_num >= 5)
  {
    tmp75_num = 0;
    for(j = 0; j < 5-1; j++)//采样值由小到大排列
    {
       for(k = 0; k < 5-j-1; k++)
       {
           if(tmp75[k] > tmp75[k+1])
           {
              tem        = tmp75[k];
              tmp75[k]   = tmp75[k+1];
              tmp75[k+1] = tem;
           }
        }
     }
    for(uint8_t i = 1; i < 5 - 1; i++)
    {
      curtmp75 = curtmp75 + tmp75[i];//去掉一个最大值和一个最小值
    }
    curtmp75 = curtmp75 / ((float)(5 - 2));
    g_ptz.temperature = curtmp75;
    memset(tmp75,0,sizeof(tmp75));
//    pdebug(DEBUG_LEVEL_INFO,"get ptz Temperature: %.3f℃",g_ptz.temperature);
  }
  
}

//int counter;
//采集任务
static char ptz_data_collect_task()
{
  int i=0,j=0;
  while(1)
  {
    
    
    
    if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
    {//电机处于启动状态
      H_ADC2_Phase_current();
    }
    if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
    {//电机处于启动状态
      V_ADC0_Phase_current();
    }
    
    if(j >= 100)
    {
      j=0;
      //云台不自检关闭，打开采集任务
#ifndef PTZ_NO_SELF_CHECK 
      ptz_Voltage_collect_adc1_task();
#endif
      OSTimeDlyHMSM(0u, 0u, 0u, 2u); 
      ptz_Current_collect_adc1_task();
//      OSTimeDlyHMSM(0u, 0u, 0u, 50u); 
    }
    
    if(i >= 3000)
    {//降低温度采集频率
      ptz_temperature_collect_tmp75_task();
      //驱动电路温度采集
      Ntc_value();
      i=0;
    }
    
    OSTimeDlyHMSM(0u, 0u, 0u, 1u); 
    
    i++;
    j++;
  }
}




static OS_STK  task_data_collect_stk[TASK_PTZ_DATA_COLLECT_STK_SIZE];
static  void  creat_task_data_collect(void)
{ 
   CPU_INT08U  task_err;   
   CPU_INT08U  name_err;
   
   task_err = OSTaskCreateExt((void (*)(void *)) ptz_data_collect_task,           
		   (void           *) 0,
		   (OS_STK         *)&task_data_collect_stk[TASK_PTZ_DATA_COLLECT_STK_SIZE - 1],
		   (INT8U           ) TASK_PTZ_DATA_COLLECT_PRIO,                   
		   (INT16U          ) TASK_PTZ_DATA_COLLECT_PRIO,
		   (OS_STK         *)&task_data_collect_stk[0],
		   (INT32U          ) TASK_PTZ_DATA_COLLECT_STK_SIZE,
		   (void           *) 0,
		   (INT16U          )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));     
    #if (OS_TASK_NAME_EN > 0)
    OSTaskNameSet(TASK_PTZ_DATA_COLLECT_PRIO, "ptz_data_collect_task", &name_err);
    #endif
       if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
		pdebug(DEBUG_LEVEL_INFO,"create ptz_data_collect_task success\r\n");
	} else {
		pdebug(DEBUG_LEVEL_FATAL,"create ptz_data_collect_task failed\r\n");
	}
}


void init_data_collect_module(void)
{
//#ifndef PTZ_NO_SELF_CHECK
  ADC1_Init();//AD采集引脚初始化
//#endif
  ADC2_Init();//AD采集引脚初始化
  ADC0_Init();//AD采集引脚初始化
  
  temp75_gpio_init();//温度传感器初始化
  creat_task_data_collect();
}

