#include "ptz_header_file.h"
#include <math.h>
#include "app_cfg.h"
#include "gd32f4xx.h"
#include "device_dac_out.h"
#include "Timer.h"
#include "full_bridge.h"
#include "as5047d.h"

#ifdef PTZ_BLDC_MOTOR

//?????????????
void ptz_send_speed(char dev, char speed)
{
  unsigned char  Speed1[7] = {0xff,0x00,0xd0,0x00,0x00,0x00,0x00};
  unsigned char  Speed2[7] = {0xff,0x00,0xd0,0x00,0x00,0x00,0x00};
  unsigned short int HSpeed = 0;
  unsigned short int VSpeed = 0;
  Speed1[1] = g_ptz.address;
  Speed2[1] = g_ptz.address;
  
  HSpeed = (unsigned short int)(g_ptz.hori_speed_actual * 100 + 0.5);
  Speed1[3] = 0X03;//?????
  Speed1[4] = (unsigned char)(HSpeed >> 8);
  Speed1[5] = (unsigned char)(HSpeed & 0x00ff);
  Speed1[6] = MotorCalPelcoDSUM(Speed1,sizeof(Speed1));
  
  VSpeed = (unsigned short int)(g_ptz.vert_speed_actual * 100 + 0.5);
  Speed2[3] = 0X04;//??????
  Speed2[4] = (unsigned char)(VSpeed >> 8);
  Speed2[5] = (unsigned char)(VSpeed & 0x00ff);
  Speed2[6] = MotorCalPelcoDSUM(Speed2,sizeof(Speed2));
  
  switch(speed)
  {
    case 1://?????????
      ptz_send_data(dev,Speed1,sizeof(Speed1));
      
      break;
      
    case 2://??????????
      ptz_send_data(dev,Speed2,sizeof(Speed2));
      
      break;
      
    default://??????????????
      ptz_send_data(dev,Speed1,sizeof(Speed1));
      ptz_send_data(dev,Speed2,sizeof(Speed2));
      
      break;
  }
}


void ptz_pid_init()
{
  g_ptz.hori_pid.SumError  = 0;//??????
  g_ptz.hori_pid.LastError = 0;//????????????
  g_ptz.hori_pid.PrevError = 0;//????????????
  g_ptz.hori_pid.LastUT_float    = 0;//???PID????u(t)
  g_ptz.hori_pid.PidUT_float     = 0;//PID???????
  g_ptz.hori_pid.KP = PTZ_HORI_PID_HORI_KP;//????????
  g_ptz.hori_pid.TI = PTZ_HORI_PID_HORI_TI;//???????
  g_ptz.hori_pid.TD = PTZ_HORI_PID_HORI_TD;//??????
  g_ptz.hori_pid.T  = PTZ_HORI_PID_T / 1000.0;//????????
  //?????????A??B??C
  g_ptz.hori_pid.A = g_ptz.hori_pid.KP * (1 + g_ptz.hori_pid.T / g_ptz.hori_pid.TI + g_ptz.hori_pid.TD);
  g_ptz.hori_pid.B = g_ptz.hori_pid.KP * (1 + 2 * g_ptz.hori_pid.TD / g_ptz.hori_pid.T);
  g_ptz.hori_pid.C = g_ptz.hori_pid.KP * (g_ptz.hori_pid.TD / g_ptz.hori_pid.T);

  g_ptz.vert_pid.SumError  = 0;//??????
  g_ptz.vert_pid.LastError = 0;//????????????
  g_ptz.vert_pid.PrevError = 0;//????????????
  g_ptz.vert_pid.LastUT_float    = 0;//???PID????u(t)
  g_ptz.vert_pid.PidUT_float     = 0;//PID???????
  g_ptz.vert_pid.KP = PTZ_VERT_PID_VERT_KP;//????????
  g_ptz.vert_pid.TI = PTZ_VERT_PID_VERT_TI;//???????
  g_ptz.vert_pid.TD = PTZ_VERT_PID_VERT_TD;//??????
  g_ptz.vert_pid.T  = PTZ_VERT_PID_T / 1000.0;//????????
  //?????????A??B??C
  g_ptz.vert_pid.A = g_ptz.vert_pid.KP * (1 + g_ptz.vert_pid.T / g_ptz.vert_pid.TI + g_ptz.vert_pid.TD);
  g_ptz.vert_pid.B = g_ptz.vert_pid.KP * (1 + 2 * g_ptz.vert_pid.TD / g_ptz.vert_pid.T);
  g_ptz.vert_pid.C = g_ptz.vert_pid.KP * (g_ptz.vert_pid.TD / g_ptz.vert_pid.T);
}

void ptz_hori_pid_clear_zero()
{
  g_ptz.hori_pid.SumError  = 0;//??????
  g_ptz.hori_pid.LastError = 0;//????????????
  g_ptz.hori_pid.PrevError = 0;//????????????
  g_ptz.hori_pid.LastUT_float    = 0;//???PID????u(t)
  g_ptz.hori_pid.PidUT_float     = 0;//PID???????
  g_ptz.hori_pid.PidUT_uint = 0;
//  hori_dac0_data_out(0);
}

static float ptz_hori_pid_calculate(float H_SampSpeed)
{
  float H_IError,H_IIncPid;
  H_IError = g_ptz.hori_speed_set - H_SampSpeed;//??????PID_INPUT_LIMIT
  /*????PID???????*/
//  if(H_IError > PTZ_HORI_PID_INPUT_LIMIT)
//  {
//    H_IError = PTZ_HORI_PID_INPUT_LIMIT;
//  }
//  if(H_IError < (-1 * PTZ_HORI_PID_INPUT_LIMIT))
//  {
//    H_IError = PTZ_HORI_PID_INPUT_LIMIT * -1;
//  }
  //????????
  H_IIncPid = g_ptz.hori_pid.A * H_IError + g_ptz.hori_pid.B * g_ptz.hori_pid.LastError + g_ptz.hori_pid.C * g_ptz.hori_pid.PrevError;
  //?›¥?????????¦Ì???
  g_ptz.hori_pid.PrevError = g_ptz.hori_pid.LastError;
  g_ptz.hori_pid.LastError = H_IError;
  //?????????
  return H_IIncPid;
}



//???????
static void ptz_hori_pid_task()
{
  unsigned int time = 0;
  char i = 0;
  while(1)
  {
    if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
    {
#ifdef PTZ_HORI_PID_as5047d_ANGLE_ASY_SPEED
      //??????????????????0
      g_ptz.hori_as5047d.as5047d_speed_angle_a = 0;
      g_ptz.hori_as5047d.as5047d_speed_angle_b = 0;
      g_ptz.hori_as5047d.as5047d_speed_angle_c = 0;
      
      //??????????????????????
      g_ptz.hori_as5047d.as5047d_speed_angle_a = (float)as5047d_vert_read_data_a();//g_ptz.hori_as5047d.as5047d_angle_actual;
      
      //PID???????
      OSTimeDlyHMSM(0u, 0u, 0u, PTZ_HORI_PID_T);
      
      
      //??????????????????????
      g_ptz.hori_as5047d.as5047d_speed_angle_b = (float)as5047d_vert_read_data_a();//g_ptz.hori_as5047d.as5047d_angle_actual;
      //??????????????????
      //?????????????????????????????????????????????????
      //????????????§³?????????????
      if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
      {
        if(g_ptz.hori_as5047d.as5047d_speed_angle_a <= g_ptz.hori_as5047d.as5047d_speed_angle_b)
        {
          g_ptz.hori_as5047d.as5047d_speed_angle_c = 
            g_ptz.hori_as5047d.as5047d_speed_angle_b - g_ptz.hori_as5047d.as5047d_speed_angle_a;
        }
        else
        {
          g_ptz.hori_as5047d.as5047d_speed_angle_c = 
            (360.0 - g_ptz.hori_as5047d.as5047d_speed_angle_a) + 
              (g_ptz.hori_as5047d.as5047d_speed_angle_b - 0);
        }
      }
      //??????????????????????§³
      if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
      {
        if(g_ptz.hori_as5047d.as5047d_speed_angle_a >= g_ptz.hori_as5047d.as5047d_speed_angle_b)
        {
          g_ptz.hori_as5047d.as5047d_speed_angle_c = 
            g_ptz.hori_as5047d.as5047d_speed_angle_a - g_ptz.hori_as5047d.as5047d_speed_angle_b;
        }
        else
        {
          g_ptz.hori_as5047d.as5047d_speed_angle_c = 
            (360.0 - g_ptz.hori_as5047d.as5047d_speed_angle_b) + 
              (g_ptz.hori_as5047d.as5047d_speed_angle_a - 0);
        }
      }
      
      //???????????
      if(g_ptz.hori_as5047d.as5047d_speed_angle_c >= 50.0 || 
         g_ptz.hori_as5047d.as5047d_speed_angle_c < 0)
      {
        g_ptz.hori_as5047d.as5047d_speed_angle_c = g_ptz.hori_as5047d.as5047d_speed_angle_c_last;
      }
      
      g_ptz.hori_as5047d.as5047d_speed_angle_c_last = g_ptz.hori_as5047d.as5047d_speed_angle_c;
      
      //?????????????
      g_ptz.hori_as5047d.as5047d_speed_actual = (g_ptz.hori_as5047d.as5047d_speed_angle_c * 1000.0) / 
        (360.0 * PTZ_HORI_PID_T) * 60.0;
      
       //???????????????????
      g_ptz.hori_motor_speed_as5047d_actual = 
        g_ptz.hori_as5047d.as5047d_speed_actual * PTZ_HORI_MOTOR_RATIO;
      g_ptz.hori_motor_speed_actual = g_ptz.hori_motor_speed_as5047d_actual;
      //????????????????????
      g_ptz.hori_speed_as5047d_actual = 
        g_ptz.hori_as5047d.as5047d_speed_actual / PTZ_HORI_BIG_GEAR_RATIO;
      
      g_ptz.hori_speed_actual = g_ptz.hori_speed_as5047d_actual;
      
#endif     
     
#ifdef PTZ_HORI_PID_as5047d_ANGLE_SYN_SPEED
      //??????????????????0
      g_ptz.hori_as5047d.as5047d_speed_angle_a = 0;
      g_ptz.hori_as5047d.as5047d_speed_angle_b = 0;
      g_ptz.hori_as5047d.as5047d_speed_angle_c = 0;
      
      //??????????????????????
      g_ptz.hori_as5047d.as5047d_speed_angle_a = (float)as5047d_hori_read_data_a();//g_ptz.hori_as5047d.as5047d_angle_actual;
      
      //PID???????
      OSTimeDlyHMSM(0u, 0u, 0u, PTZ_HORI_PID_T);
      
      
      //??????????????????????
      g_ptz.hori_as5047d.as5047d_speed_angle_b = (float)as5047d_hori_read_data_a();//g_ptz.hori_as5047d.as5047d_angle_actual;
      //??????????????????
      //?????????????????????????????????????????????????
      //?????????????????????????
      if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
      {
        if(g_ptz.hori_as5047d.as5047d_speed_angle_a <= g_ptz.hori_as5047d.as5047d_speed_angle_b)
        {
          g_ptz.hori_as5047d.as5047d_speed_angle_c = 
            g_ptz.hori_as5047d.as5047d_speed_angle_b - g_ptz.hori_as5047d.as5047d_speed_angle_a;
        }
        else
        {
          g_ptz.hori_as5047d.as5047d_speed_angle_c = 
            (16384.0 - g_ptz.hori_as5047d.as5047d_speed_angle_a) + 
              (g_ptz.hori_as5047d.as5047d_speed_angle_b - 0);
        }
      }
      //????????????§³??????????§³
      if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
      {
        if(g_ptz.hori_as5047d.as5047d_speed_angle_a >= g_ptz.hori_as5047d.as5047d_speed_angle_b)
        {
          g_ptz.hori_as5047d.as5047d_speed_angle_c = 
            g_ptz.hori_as5047d.as5047d_speed_angle_a - g_ptz.hori_as5047d.as5047d_speed_angle_b;
        }
        else
        {
          g_ptz.hori_as5047d.as5047d_speed_angle_c = 
            (16384.0 - g_ptz.hori_as5047d.as5047d_speed_angle_b) + 
              (g_ptz.hori_as5047d.as5047d_speed_angle_a - 0);
        }
      }
      
      //???????????
      if(g_ptz.hori_as5047d.as5047d_speed_angle_c >= 3000.0 || 
         g_ptz.hori_as5047d.as5047d_speed_angle_c < 0)
      {
        g_ptz.hori_as5047d.as5047d_speed_angle_c = g_ptz.hori_as5047d.as5047d_speed_angle_c_last;
      }
      
      g_ptz.hori_as5047d.as5047d_speed_angle_c_last = g_ptz.hori_as5047d.as5047d_speed_angle_c;
      
      //?????????????
      g_ptz.hori_as5047d.as5047d_speed_actual = ((g_ptz.hori_as5047d.as5047d_speed_angle_c / 16384.0) * 1000.0) / 
        (PTZ_HORI_PID_T) * 60.0;
      
//            g_ptz.hori_as5047d.as5047d_speed_actual = (g_ptz.hori_as5047d.as5047d_speed_angle_c * 1000.0) / 
//        (360.0 * PTZ_HORI_PID_T) * 60.0;
      
       //???????????????????
      g_ptz.hori_motor_speed_as5047d_actual = 
        g_ptz.hori_as5047d.as5047d_speed_actual * PTZ_HORI_MOTOR_RATIO;
      g_ptz.hori_motor_speed_actual = g_ptz.hori_motor_speed_as5047d_actual;
      //????????????????????
      g_ptz.hori_speed_as5047d_actual = 
        g_ptz.hori_as5047d.as5047d_speed_actual / PTZ_HORI_BIG_GEAR_RATIO;
      
      g_ptz.hori_speed_actual = g_ptz.hori_speed_as5047d_actual;
#endif
      

#ifdef  PTZ_HORI_PID_JY02A_SPEED
      //???????§à?????,??????0
      g_ptz.hori_jy02a.jy02a_fg_num_last = g_ptz.hori_jy02a.jy02a_fg_num_actual;
      g_ptz.hori_jy02a.jy02a_fg_num_actual = 0;
      g_ptz.hori_motor_speed_actual = 0;
      
      //PID???????
      OSTimeDlyHMSM(0u, 0u, 0u, PTZ_HORI_PID_T);
      
       //??????????§à?????????????
      g_ptz.hori_motor_speed_jy02a_actual = 
        g_ptz.hori_jy02a.jy02a_fg_num_actual / 
          PTZ_HORI_FG_PWM_NUM_CYCLE * 
            1000.0 * 60 / PTZ_HORI_PID_T;
      g_ptz.hori_motor_speed_actual = g_ptz.hori_motor_speed_jy02a_actual;
      //??????????§à???????????
      g_ptz.hori_speed_jy02a_actual = g_ptz.hori_motor_speed_jy02a_actual / PTZ_HORI_RATIO;
      
      g_ptz.hori_speed_actual = g_ptz.hori_speed_jy02a_actual;
#endif

      
#ifdef PTZ_PID_HALL_SPEED //????????????
      g_ptz.hori_pid.hall_h1_count   = 0;
      g_ptz.hori_pid.hall_h2_count   = 0;
      g_ptz.hori_pid.hall_h3_count   = 0;
      g_ptz.hori_pid.hall_h123_count = 0;
      
      g_ptz.hori_pid.hall_h1_count_last = 0;
      g_ptz.hori_pid.hall_h1_count_time = 0;
      g_ptz.hori_pid.hall_h1_count_time_s = 0;
      g_ptz.hori_pid.hall_h1_count_time_e = 0;
      
      g_ptz.hori_pid.hall_h2_count_last = 0;
      g_ptz.hori_pid.hall_h2_count_time = 0;
      g_ptz.hori_pid.hall_h2_count_time_s = 0;
      g_ptz.hori_pid.hall_h2_count_time_e = 0;
      
      g_ptz.hori_pid.hall_h3_count_last = 0;
      g_ptz.hori_pid.hall_h3_count_time = 0;
      g_ptz.hori_pid.hall_h3_count_time_s = 0;
      g_ptz.hori_pid.hall_h3_count_time_e = 0;
      
      
      g_ptz.hori_pid.hall_h123_count_last = 0;
      g_ptz.hori_pid.hall_h123_count_time = 0;
      g_ptz.hori_pid.hall_h123_count_time_s = 0;
      g_ptz.hori_pid.hall_h123_count_time_e = 0;
      
      g_ptz.hori_pid.hall_h1_start_flag = 0;
      g_ptz.hori_pid.hall_h2_start_flag = 0;
      g_ptz.hori_pid.hall_h3_start_flag = 0;
      g_ptz.hori_pid.hall_h123_start_flag = 0;
      
      /*
      
      for(time = 0; time < PTZ_HORI_PID_T_MAX; time ++)
      {
        if(g_ptz.hori_pid.hall_h1_count != g_ptz.hori_pid.hall_h1_count_last)
        {
          if(g_ptz.hori_pid.hall_h1_start_flag == 0)
          {
            g_ptz.hori_pid.hall_h1_count_last = g_ptz.hori_pid.hall_h1_count;
            g_ptz.hori_pid.hall_h1_count_time_s = time;
            g_ptz.hori_pid.hall_h1_start_flag = 1;
          }
          else
          {
            g_ptz.hori_pid.hall_h1_count_last = g_ptz.hori_pid.hall_h1_count;
            g_ptz.hori_pid.hall_h1_count_time_e = time;
          }
          //??????????§Ý???????????
          if(g_ptz.hori_pid.hall_h123_start_flag == 0)
          {
            g_ptz.hori_pid.hall_h123_count_time_s = time;
            g_ptz.hori_pid.hall_h123_start_flag = 1;
          }
          else
          {
             g_ptz.hori_pid.hall_h123_count_time_e = time;
          }
        }
        
        if(g_ptz.hori_pid.hall_h2_count != g_ptz.hori_pid.hall_h2_count_last)
        {
          if(g_ptz.hori_pid.hall_h2_start_flag == 0)
          {
            g_ptz.hori_pid.hall_h2_count_last = g_ptz.hori_pid.hall_h2_count;
            g_ptz.hori_pid.hall_h2_count_time_s = time;
            g_ptz.hori_pid.hall_h2_start_flag = 1;
          }
          else
          {
            g_ptz.hori_pid.hall_h2_count_last = g_ptz.hori_pid.hall_h2_count;
            g_ptz.hori_pid.hall_h2_count_time_e = time;
          }
          //??????????§Ý???????????
          if(g_ptz.hori_pid.hall_h123_start_flag == 0)
          {
            g_ptz.hori_pid.hall_h123_count_time_s = time;
            g_ptz.hori_pid.hall_h123_start_flag = 1;
          }
          else
          {
             g_ptz.hori_pid.hall_h123_count_time_e = time;
          }
        }
        
        if(g_ptz.hori_pid.hall_h3_count != g_ptz.hori_pid.hall_h3_count_last)
        {
          if(g_ptz.hori_pid.hall_h3_start_flag == 0)
          {
            g_ptz.hori_pid.hall_h3_count_last = g_ptz.hori_pid.hall_h3_count;
            g_ptz.hori_pid.hall_h3_count_time_s = time;
            g_ptz.hori_pid.hall_h3_start_flag = 1;
          }
          else
          {
            g_ptz.hori_pid.hall_h3_count_last = g_ptz.hori_pid.hall_h3_count;
            g_ptz.hori_pid.hall_h3_count_time_e = time;
          }
          //??????????§Ý???????????
          if(g_ptz.hori_pid.hall_h123_start_flag == 0)
          {
            g_ptz.hori_pid.hall_h123_count_time_s = time;
            g_ptz.hori_pid.hall_h123_start_flag = 1;
          }
          else
          {
             g_ptz.hori_pid.hall_h123_count_time_e = time;
          }
        }
        
        if(time >= PTZ_HORI_PID_T)//?????????????
        {
          if(g_ptz.hori_pid.hall_h1_count >= 2 &&
             g_ptz.hori_pid.hall_h2_count >= 2 &&
             g_ptz.hori_pid.hall_h3_count >= 2)
          {
            break;
          }
        }
        OSTimeDlyHMSM(0u, 0u, 0u, 1u);
      }
      
      
      
      
      //?????????
      if(g_ptz.hori_pid.hall_h1_count_time_e > g_ptz.hori_pid.hall_h1_count_time_s)
      {
        g_ptz.hori_pid.hall_h1_count_time = 
          g_ptz.hori_pid.hall_h1_count_time_e - g_ptz.hori_pid.hall_h1_count_time_s;
      }
      else
      {
        g_ptz.hori_pid.hall_h1_count_time = time;
      }
      
      if(g_ptz.hori_pid.hall_h2_count_time_e > g_ptz.hori_pid.hall_h2_count_time_s)
      {
        g_ptz.hori_pid.hall_h2_count_time = 
          g_ptz.hori_pid.hall_h2_count_time_e - g_ptz.hori_pid.hall_h2_count_time_s;
      }
      else
      {
        g_ptz.hori_pid.hall_h2_count_time = time;
      }
      
      if(g_ptz.hori_pid.hall_h3_count_time_e > g_ptz.hori_pid.hall_h3_count_time_s)
      {
        g_ptz.hori_pid.hall_h3_count_time = 
          g_ptz.hori_pid.hall_h3_count_time_e - g_ptz.hori_pid.hall_h3_count_time_s;
      }
      else
      {
        g_ptz.hori_pid.hall_h3_count_time = time;
      }
      
      //??????????§Ý???????????
      if(g_ptz.hori_pid.hall_h123_count_time_e > g_ptz.hori_pid.hall_h123_count_time_s)
      {
        g_ptz.hori_pid.hall_h123_count_time = 
          g_ptz.hori_pid.hall_h123_count_time_e - g_ptz.hori_pid.hall_h123_count_time_s;
      }
      else
      {
        g_ptz.hori_pid.hall_h123_count_time = time;
      }

      //?????????????????????????????
      i = 0;
      if(g_ptz.hori_pid.hall_h1_count >= 2 && g_ptz.hori_pid.hall_h1_count_time > 0)
      {
        g_ptz.hori_pid.hall_h1_motor_speed = 
          60000.0 * (float)(g_ptz.hori_pid.hall_h1_count - 1) / 
            (float)g_ptz.hori_pid.hall_h1_count_time  /
              PTZ_HORI_MOTOR_POLE_PAIRS;
        i++;
      }
      else
      {
        g_ptz.hori_pid.hall_h1_motor_speed = 0;
      }
      
      
      if(g_ptz.hori_pid.hall_h2_count >= 2 && g_ptz.hori_pid.hall_h2_count_time > 0)
      {
        g_ptz.hori_pid.hall_h2_motor_speed = 
          60000.0 * (float)(g_ptz.hori_pid.hall_h2_count - 1) / 
            (float)g_ptz.hori_pid.hall_h2_count_time  /
              PTZ_HORI_MOTOR_POLE_PAIRS;
        i++;
      }
      else
      {
        g_ptz.hori_pid.hall_h2_motor_speed = 0;
      }      
         
         
      if(g_ptz.hori_pid.hall_h3_count >= 2 && g_ptz.hori_pid.hall_h3_count_time > 0)
      {
        g_ptz.hori_pid.hall_h3_motor_speed = 
          60000.0 * (float)(g_ptz.hori_pid.hall_h3_count - 1) / 
            (float)g_ptz.hori_pid.hall_h3_count_time  /
              PTZ_HORI_MOTOR_POLE_PAIRS;
        i++;
      }
      else
      {
        g_ptz.hori_pid.hall_h3_motor_speed = 0;
      }   
      
      g_ptz.hori_pid.hall_h123_count = g_ptz.hori_pid.hall_h1_count +
                                       g_ptz.hori_pid.hall_h2_count +
                                       g_ptz.hori_pid.hall_h3_count;
      //?????????????????????
      if(g_ptz.hori_pid.hall_h123_count >= 2 && g_ptz.hori_pid.hall_h123_count_time > 0)
      {
        g_ptz.hori_pid.hall_h123_motor_speed = 
          60000.0 * (float)(g_ptz.hori_pid.hall_h123_count - 1) / 
            (float)g_ptz.hori_pid.hall_h123_count_time  /
              PTZ_HORI_ONE_CYCLE_HALL_NUM;
          
        i++;
      }
      else
      {
        g_ptz.hori_pid.hall_h123_motor_speed = 0;
      }
      
       
      //????????????
      if(i > 0)
      {
        g_ptz.hori_motor_speed_hall_actual = (g_ptz.hori_pid.hall_h1_motor_speed +
                                              g_ptz.hori_pid.hall_h2_motor_speed +
                                              g_ptz.hori_pid.hall_h3_motor_speed +
                                              g_ptz.hori_pid.hall_h123_motor_speed) / i;
      }
      else
      {
        g_ptz.hori_motor_speed_hall_actual = 0;
      }

      */
      
      
      /* ********************************** */
      OSTimeDlyHMSM(0u, 0u, 0u, PTZ_HORI_PID_T);
      g_ptz.hori_pid.hall_h123_count = g_ptz.hori_pid.hall_h1_count +
                                       g_ptz.hori_pid.hall_h2_count +
                                       g_ptz.hori_pid.hall_h3_count;
      
      static uint32_t time_hori, lastTime_hori, horiTime;
      time_hori = OSTimeGet();
      horiTime = time_hori - lastTime_hori;
      if ( horiTime < 0 )
      {
        continue;
      }
      lastTime_hori = time_hori;
      
      if(g_ptz.hori_pid.hall_h123_count >= 2)
      {
        // g_ptz.hori_pid.hall_h123_motor_speed = 
        //   60000.0 * (float)(g_ptz.hori_pid.hall_h123_count/* - 1*/) / 
        //     (float)PTZ_HORI_PID_T / PTZ_HORI_ONE_CYCLE_HALL_NUM;
        g_ptz.hori_pid.hall_h123_motor_speed = 
          60000.0 * (float)(g_ptz.hori_pid.hall_h123_count/* - 1*/) / 
            (float)horiTime / PTZ_HORI_ONE_CYCLE_HALL_NUM;          
      }
      
      g_ptz.hori_motor_speed_hall_actual = g_ptz.hori_pid.hall_h123_motor_speed;
      
      /* ********************************** */
      
      
      
      //?§Ø??????????
      if(g_ptz.hori_pid.hall_h1_count == 0 ||
         g_ptz.hori_pid.hall_h2_count == 0 ||
         g_ptz.hori_pid.hall_h3_count == 0)
      {//??????????????????0
        g_ptz.hori_motor_speed_hall_actual = 0;
      }
      
      
      g_ptz.hori_motor_speed_actual = g_ptz.hori_motor_speed_hall_actual;
      
      //??????????????????
      g_ptz.hori_speed_hall_actual = g_ptz.hori_motor_speed_hall_actual / PTZ_HORI_RATIO;
      g_ptz.hori_speed_actual = g_ptz.hori_speed_hall_actual;
#endif      
     
      
      
      /*???????????????????????????????????
      1.???????????????????????????
      2.?????Ú…????60????????PID?????*/
      
      
//      if(g_ptz.hori_speed_actual < g_ptz.hori_speed_set * PTZ_HORI_PRO_ADD_RANGE_LESS)
//      {
////        if(time > PTZ_HORI_PID_ADD)
////        {
////          time = PTZ_HORI_PID_ADD;
////        }
////        g_ptz.hori_pid.PidUT_float = g_ptz.hori_pid.PidUT_float + time/*PTZ_HORI_PID_ADD*/;
////        //?›¥???????PID????
////        g_ptz.hori_pid.PrevError = g_ptz.hori_pid.LastError;
////        g_ptz.hori_pid.LastError = g_ptz.hori_speed_set - g_ptz.hori_speed_actual;
//
//        g_ptz.hori_pid.PidUT_float = g_ptz.hori_pid.PidUT_float * PTZ_HORI_PRO_ADD_MUL_LESS;//* 1.3/*PTZ_HORI_PID_ADD*/;
//        if(g_ptz.hori_pid.PidUT_float < PTZ_HORI_PID_ADD)
//        {
//          g_ptz.hori_pid.PidUT_float = PTZ_HORI_PID_ADD;
//        }
//        //?›¥???????PID????
//        g_ptz.hori_pid.PrevError = g_ptz.hori_pid.LastError;
//        g_ptz.hori_pid.LastError = g_ptz.hori_speed_set - g_ptz.hori_speed_actual;
//        /**??????????????????????????????????????§Þ???**/
//      }
//      else 
        
        if(g_ptz.hori_speed_actual > g_ptz.hori_speed_set * 2)
      {//???????Ú…????3??
        g_ptz.hori_pid.PidUT_float = g_ptz.hori_pid.PidUT_float * 0.7;////??????????
       
        g_ptz.hori_pid.PrevError = g_ptz.hori_pid.LastError;
        g_ptz.hori_pid.LastError = g_ptz.hori_speed_set - g_ptz.hori_speed_actual;
      }
      else
      {
        //????PID??????????
        g_ptz.hori_pid.PidUT_float = ptz_hori_pid_calculate(g_ptz.hori_speed_actual) + g_ptz.hori_pid.LastUT_float;      
        
        //????PID?????????????????????????????????????????????¦¶??
        //?????PID????????
        if(fabs(g_ptz.hori_pid.PidUT_float - g_ptz.hori_pid.LastUT_float) > PTZ_HORI_PID_OUTPUT_LIMIT)//???PID??????
        {
          if(g_ptz.hori_pid.PidUT_float > g_ptz.hori_pid.LastUT_float)
          {
            g_ptz.hori_pid.PidUT_float = g_ptz.hori_pid.LastUT_float + PTZ_HORI_PID_OUTPUT_LIMIT;
          }
          else
          {
            g_ptz.hori_pid.PidUT_float = g_ptz.hori_pid.LastUT_float - PTZ_HORI_PID_OUTPUT_LIMIT;
          }
        }
      }

      if(g_ptz.hori_pid.PidUT_float < PTZ_HORI_VR_MIN)//???????????????§³?
      {
        g_ptz.hori_pid.PidUT_float = PTZ_HORI_VR_MIN;
      }
      if(g_ptz.hori_pid.PidUT_float > PTZ_HORI_VR_MAX)//?????????????????
      {
        g_ptz.hori_pid.PidUT_float = PTZ_HORI_VR_MAX;
      }
      
      g_ptz.hori_pid.PidUT_uint = (unsigned int)(g_ptz.hori_pid.PidUT_float + 0.5);
      
      //????PID?????????????????¦¶
      if(g_ptz.hori_pid.PidUT_uint <= PTZ_HORI_VR_MIN)//???????????????§³?
      {
        g_ptz.hori_pid.PidUT_uint = PTZ_HORI_VR_MIN;
      }
      if(g_ptz.hori_pid.PidUT_uint > PTZ_HORI_VR_MAX)//?????????????????
      {
        g_ptz.hori_pid.PidUT_uint = PTZ_HORI_VR_MAX;
      }

      //??PID??????????????????????????????§à?
//      hori_dac0_data_out(g_ptz.hori_pid.PidUT_uint);
      
      h_pwm_duty_change(g_ptz.hori_pid.PidUT_uint);
      
      //?????PID????????
      g_ptz.hori_pid.LastUT_float = g_ptz.hori_pid.PidUT_float;
    }
    
    if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
    {
      g_ptz.hori_speed_actual = 0;
      OSTimeDlyHMSM(0u, 0u, 0u, 10u);
    }
    
#ifdef PTZ_PID_HALL_SPEED //????????????
      //????????????? 
      if(g_ptz.hori_pid.hall_h1_count >= UNSIGNED_INT_MAX_VALUE)
      {
        g_ptz.hori_pid.hall_h1_count = 0;
      }
      if(g_ptz.hori_pid.hall_h2_count >= UNSIGNED_INT_MAX_VALUE)
      {
        g_ptz.hori_pid.hall_h2_count = 0;
      }
      if(g_ptz.hori_pid.hall_h3_count >= UNSIGNED_INT_MAX_VALUE)
      {
        g_ptz.hori_pid.hall_h3_count = 0;
      }
#endif
    
  }
}




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

}




/****************************************************************************/

void ptz_vert_pid_clear_zero()
{
  g_ptz.vert_pid.SumError  = 0;//??????
  g_ptz.vert_pid.LastError = 0;//????????????
  g_ptz.vert_pid.PrevError = 0;//????????????
  g_ptz.vert_pid.LastUT_float    = 0;//???PID????u(t)
  g_ptz.vert_pid.PidUT_float     = 0;//PID???????
  g_ptz.vert_pid.PidUT_uint = 0;
  g_ptz.vert_pid.mode      = PID_ADJUST_SPEED;//???????PID????
  g_ptz.vert_pid.director_speed_state = 0;
//  vert_dac1_data_out(0);
}

static float ptz_vert_pid_calculate(float SampSpeed)
{
  float IError,IIncPid;
  IError = g_ptz.vert_speed_set - SampSpeed;//??????PID_INPUT_LIMIT

  /*????PID???????*/
//  if(IError > PTZ_VERT_PID_INPUT_LIMIT)
//  {
//    IError = PTZ_VERT_PID_INPUT_LIMIT;
//  }
//  if(IError < (-1 * PTZ_VERT_PID_INPUT_LIMIT))
//  {
//    IError = PTZ_VERT_PID_INPUT_LIMIT * -1;
//  }
  //????????
  IIncPid = g_ptz.vert_pid.A * IError + g_ptz.vert_pid.B * g_ptz.vert_pid.LastError + g_ptz.vert_pid.C * g_ptz.vert_pid.PrevError;
  //?›¥?????????¦Ì???
  g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError;
  g_ptz.vert_pid.LastError = IError;
  //?????????
  return IIncPid;
}



//???????
static void ptz_vert_pid_task()
{
  unsigned int time = 0;
  char i = 0;
  while(1)
  {
    //???????
    if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
    {
#ifdef PTZ_VERT_PID_as5047d_ANGLE_ASY_SPEED
      //??????????????????0
      g_ptz.vert_as5047d.as5047d_speed_angle_a = 0;
      g_ptz.vert_as5047d.as5047d_speed_angle_b = 0;
      g_ptz.vert_as5047d.as5047d_speed_angle_c = 0;
      
      //??????????????????????
      g_ptz.vert_as5047d.as5047d_speed_angle_a = (float)as5047d_vert_read_data_a();//as5047d_vert_get_angle_a();//g_ptz.vert_as5047d.as5047d_angle_actual;
      
      //PID???????
      OSTimeDlyHMSM(0u, 0u, 0u, PTZ_VERT_PID_T);
      
      //??????????????????????
      g_ptz.vert_as5047d.as5047d_speed_angle_b = (float)as5047d_vert_read_data_a();//as5047d_vert_get_angle_a();//g_ptz.vert_as5047d.as5047d_angle_actual;

      //??????????????????
      //?????????????????????????????????????????????????
      //????????????§³?????????????
      if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
      {
        if(g_ptz.vert_as5047d.as5047d_speed_angle_a <= g_ptz.vert_as5047d.as5047d_speed_angle_b)
        {
          g_ptz.vert_as5047d.as5047d_speed_angle_c = 
            g_ptz.vert_as5047d.as5047d_speed_angle_b - g_ptz.vert_as5047d.as5047d_speed_angle_a;
        }
        else
        {
          g_ptz.vert_as5047d.as5047d_speed_angle_c = 
            (16384.0 - g_ptz.vert_as5047d.as5047d_speed_angle_a) + //360.0
              (g_ptz.vert_as5047d.as5047d_speed_angle_b - 0);
        }
      }
      //??????????????????????§³
      if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
      {
        if(g_ptz.vert_as5047d.as5047d_speed_angle_a >= g_ptz.vert_as5047d.as5047d_speed_angle_b)
        {
          g_ptz.vert_as5047d.as5047d_speed_angle_c = 
            g_ptz.vert_as5047d.as5047d_speed_angle_a - g_ptz.vert_as5047d.as5047d_speed_angle_b;
        }
        else
        {
          g_ptz.vert_as5047d.as5047d_speed_angle_c = 
            (16384.0 - g_ptz.vert_as5047d.as5047d_speed_angle_b) + //360.0
              (g_ptz.vert_as5047d.as5047d_speed_angle_a - 0);
        }
      }
      
      //???????????
      if(g_ptz.vert_as5047d.as5047d_speed_angle_c < 0 ||
         g_ptz.vert_as5047d.as5047d_speed_angle_c >= 3000.0)//50.0
      {
        g_ptz.vert_as5047d.as5047d_speed_angle_c = g_ptz.vert_as5047d.as5047d_speed_angle_c_last;
      }
      
      g_ptz.vert_as5047d.as5047d_speed_angle_c_last = g_ptz.vert_as5047d.as5047d_speed_angle_c;
        
      //?????????????
      g_ptz.vert_as5047d.as5047d_speed_actual = ((g_ptz.vert_as5047d.as5047d_speed_angle_c / 16384.0) * 1000) / 
        PTZ_VERT_PID_T * 60.0;
      
//            g_ptz.vert_as5047d.as5047d_speed_actual = (g_ptz.vert_as5047d.as5047d_speed_angle_c * 1000) / 
//        (360.0 * PTZ_VERT_PID_T) * 60.0;
      //???????????????????
      g_ptz.vert_motor_speed_as5047d_actual = 
        g_ptz.vert_as5047d.as5047d_speed_actual * PTZ_VERT_MOTOR_RATIO;
      g_ptz.vert_motor_speed_actual =  g_ptz.vert_motor_speed_as5047d_actual;
      //????????????????????
      g_ptz.vert_speed_as5047d_actual = 
        g_ptz.vert_as5047d.as5047d_speed_actual / PTZ_VERT_BIG_GEAR_RATIO;
      
      g_ptz.vert_speed_actual = g_ptz.vert_speed_as5047d_actual;
#endif
      
#ifdef PTZ_VERT_PID_as5047d_ANGLE_SYN_SPEED
      //??????????????????0
      g_ptz.vert_as5047d.as5047d_speed_angle_a = 0;
      g_ptz.vert_as5047d.as5047d_speed_angle_b = 0;
      g_ptz.vert_as5047d.as5047d_speed_angle_c = 0;
      
      //??????????????????????
      g_ptz.vert_as5047d.as5047d_speed_angle_a = g_ptz.vert_as5047d.as5047d_angle_actual;
      
      //PID???????
      OSTimeDlyHMSM(0u, 0u, 0u, PTZ_VERT_PID_T);
      
      //??????????????????????
      g_ptz.vert_as5047d.as5047d_speed_angle_b = g_ptz.vert_as5047d.as5047d_angle_actual;

      //??????????????????
      //?????????????????????????????????????????????????
      //?????????????????????????
      if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
      {
        if(g_ptz.vert_as5047d.as5047d_speed_angle_a <= g_ptz.vert_as5047d.as5047d_speed_angle_b)
        {
          g_ptz.vert_as5047d.as5047d_speed_angle_c = 
            g_ptz.vert_as5047d.as5047d_speed_angle_b - g_ptz.vert_as5047d.as5047d_speed_angle_a;
        }
        else
        {
          g_ptz.vert_as5047d.as5047d_speed_angle_c = 
            (360.0 - g_ptz.vert_as5047d.as5047d_speed_angle_a) + 
              (g_ptz.vert_as5047d.as5047d_speed_angle_b - 0);
        }
      }
      //????????????§³??????????§³
      if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
      {
        if(g_ptz.vert_as5047d.as5047d_speed_angle_a >= g_ptz.vert_as5047d.as5047d_speed_angle_b)
        {
          g_ptz.vert_as5047d.as5047d_speed_angle_c = 
            g_ptz.vert_as5047d.as5047d_speed_angle_a - g_ptz.vert_as5047d.as5047d_speed_angle_b;
        }
        else
        {
          g_ptz.vert_as5047d.as5047d_speed_angle_c = 
            (360.0 - g_ptz.vert_as5047d.as5047d_speed_angle_b) + 
              (g_ptz.vert_as5047d.as5047d_speed_angle_a - 0);
        }
      } 
      
      //???????????
      if(g_ptz.vert_as5047d.as5047d_speed_angle_c < 0 ||
         g_ptz.vert_as5047d.as5047d_speed_angle_c >= 50.0)
      {
        g_ptz.vert_as5047d.as5047d_speed_angle_c = g_ptz.vert_as5047d.as5047d_speed_angle_c_last;
      }
      
      g_ptz.vert_as5047d.as5047d_speed_angle_c_last = g_ptz.vert_as5047d.as5047d_speed_angle_c;
      
      //?????????????
      g_ptz.vert_as5047d.as5047d_speed_actual = (g_ptz.vert_as5047d.as5047d_speed_angle_c * 1000) / 
        (360.0 * PTZ_VERT_PID_T) * 60.0;
      //???????????????????
      g_ptz.vert_motor_speed_as5047d_actual = 
        g_ptz.vert_as5047d.as5047d_speed_actual * PTZ_VERT_MOTOR_RATIO;
      g_ptz.vert_motor_speed_actual =  g_ptz.vert_motor_speed_as5047d_actual;
      //????????????????????
      g_ptz.vert_speed_as5047d_actual = 
        g_ptz.vert_as5047d.as5047d_speed_actual / PTZ_VERT_BIG_GEAR_RATIO;
      
      g_ptz.vert_speed_actual = g_ptz.vert_speed_as5047d_actual;
#endif      

#ifdef  PTZ_VERT_PID_JY02A_SPEED
      //???????§à????????????0
      g_ptz.vert_jy02a.jy02a_fg_num_last = g_ptz.vert_jy02a.jy02a_fg_num_actual;
      g_ptz.vert_jy02a.jy02a_fg_num_actual = 0;
      g_ptz.vert_motor_speed_actual = 0;
      
      //PID???????
      OSTimeDlyHMSM(0u, 0u, 0u, PTZ_VERT_PID_T);
      
       //??????????§à?????????????
      g_ptz.vert_motor_speed_jy02a_actual = 
        g_ptz.vert_jy02a.jy02a_fg_num_actual / 
          PTZ_VERT_FG_PWM_NUM_CYCLE * 
            1000.0 * 60 / PTZ_VERT_PID_T;
      g_ptz.vert_motor_speed_actual =  g_ptz.vert_motor_speed_jy02a_actual;
      //??????????§à???????????
      g_ptz.vert_speed_jy02a_actual = g_ptz.vert_motor_speed_jy02a_actual / PTZ_VERT_RATIO;
     
      g_ptz.vert_speed_actual = g_ptz.vert_speed_jy02a_actual; 
#endif
      
#ifdef PTZ_PID_HALL_SPEED //????????????
      
      g_ptz.vert_pid.hall_h1_count   = 0;
      g_ptz.vert_pid.hall_h2_count   = 0;
      g_ptz.vert_pid.hall_h3_count   = 0;
      g_ptz.vert_pid.hall_h123_count = 0;
      
      g_ptz.vert_pid.hall_h1_count_last = 0;
      g_ptz.vert_pid.hall_h1_count_time = 0;
      g_ptz.vert_pid.hall_h1_count_time_s = 0;
      g_ptz.vert_pid.hall_h1_count_time_e = 0;
      
      g_ptz.vert_pid.hall_h2_count_last = 0;
      g_ptz.vert_pid.hall_h2_count_time = 0;
      g_ptz.vert_pid.hall_h2_count_time_s = 0;
      g_ptz.vert_pid.hall_h2_count_time_e = 0;
      
      g_ptz.vert_pid.hall_h3_count_last = 0;
      g_ptz.vert_pid.hall_h3_count_time = 0;
      g_ptz.vert_pid.hall_h3_count_time_s = 0;
      g_ptz.vert_pid.hall_h3_count_time_e = 0;
      
      g_ptz.vert_pid.hall_h123_count_last = 0;
      g_ptz.vert_pid.hall_h123_count_time = 0;
      g_ptz.vert_pid.hall_h123_count_time_s = 0;
      g_ptz.vert_pid.hall_h123_count_time_e = 0;
      
      g_ptz.vert_pid.hall_h1_start_flag = 0;
      g_ptz.vert_pid.hall_h2_start_flag = 0;
      g_ptz.vert_pid.hall_h3_start_flag = 0;
      g_ptz.vert_pid.hall_h123_start_flag = 0;
      
      /*
      
      for(time = 0; time < PTZ_VERT_PID_T_MAX; time ++)
      {
        if(g_ptz.vert_pid.hall_h1_count != g_ptz.vert_pid.hall_h1_count_last)
        {
          if(g_ptz.vert_pid.hall_h1_start_flag == 0)
          {
            g_ptz.vert_pid.hall_h1_count_last = g_ptz.vert_pid.hall_h1_count;
            g_ptz.vert_pid.hall_h1_count_time_s = time;
            g_ptz.vert_pid.hall_h1_start_flag = 1;
          }
          else
          {
            g_ptz.vert_pid.hall_h1_count_last = g_ptz.vert_pid.hall_h1_count;
            g_ptz.vert_pid.hall_h1_count_time_e = time;
          }
          
          //??????????§Ý???????????
          if(g_ptz.vert_pid.hall_h123_start_flag == 0)
          {
            g_ptz.vert_pid.hall_h123_count_time_s = time;
            g_ptz.vert_pid.hall_h123_start_flag = 1;
          }
          else
          {
             g_ptz.vert_pid.hall_h123_count_time_e = time;
          }
        }
        
        if(g_ptz.vert_pid.hall_h2_count != g_ptz.vert_pid.hall_h2_count_last)
        {
          if(g_ptz.vert_pid.hall_h2_start_flag == 0)
          {
            g_ptz.vert_pid.hall_h2_count_last = g_ptz.vert_pid.hall_h2_count;
            g_ptz.vert_pid.hall_h2_count_time_s = time;
            g_ptz.vert_pid.hall_h2_start_flag = 1;
          }
          else
          {
            g_ptz.vert_pid.hall_h2_count_last = g_ptz.vert_pid.hall_h2_count;
            g_ptz.vert_pid.hall_h2_count_time_e = time;
          }
          
          //??????????§Ý???????????
          if(g_ptz.vert_pid.hall_h123_start_flag == 0)
          {
            g_ptz.vert_pid.hall_h123_count_time_s = time;
            g_ptz.vert_pid.hall_h123_start_flag = 1;
          }
          else
          {
             g_ptz.vert_pid.hall_h123_count_time_e = time;
          }
        }
        
        if(g_ptz.vert_pid.hall_h3_count != g_ptz.vert_pid.hall_h3_count_last)
        {
          if(g_ptz.vert_pid.hall_h3_start_flag == 0)
          {
            g_ptz.vert_pid.hall_h3_count_last = g_ptz.vert_pid.hall_h3_count;
            g_ptz.vert_pid.hall_h3_count_time_s = time;
            g_ptz.vert_pid.hall_h3_start_flag = 1;
          }
          else
          {
            g_ptz.vert_pid.hall_h3_count_last = g_ptz.vert_pid.hall_h3_count;
            g_ptz.vert_pid.hall_h3_count_time_e = time;
          }
          
          //??????????§Ý???????????
          if(g_ptz.vert_pid.hall_h123_start_flag == 0)
          {
            g_ptz.vert_pid.hall_h123_count_time_s = time;
            g_ptz.vert_pid.hall_h123_start_flag = 1;
          }
          else
          {
             g_ptz.vert_pid.hall_h123_count_time_e = time;
          }
        }
        
        if(time >= PTZ_VERT_PID_T)//?????????????
        {
          if(g_ptz.vert_pid.hall_h1_count >= 2 &&
             g_ptz.vert_pid.hall_h2_count >= 2 &&
             g_ptz.vert_pid.hall_h3_count >= 2)
          {
            break;
          }
        }
        OSTimeDlyHMSM(0u, 0u, 0u, 1u);
      }
      
      //???????
      if(g_ptz.vert_pid.hall_h1_count_time_e > g_ptz.vert_pid.hall_h1_count_time_s)
      {
        g_ptz.vert_pid.hall_h1_count_time = 
          g_ptz.vert_pid.hall_h1_count_time_e - g_ptz.vert_pid.hall_h1_count_time_s;
      }
      else
      {
        g_ptz.vert_pid.hall_h1_count_time = time;
      }
      
      if(g_ptz.vert_pid.hall_h2_count_time_e > g_ptz.vert_pid.hall_h2_count_time_s)
      {
        g_ptz.vert_pid.hall_h2_count_time = 
          g_ptz.vert_pid.hall_h2_count_time_e - g_ptz.vert_pid.hall_h2_count_time_s;
      }
      else
      {
        g_ptz.vert_pid.hall_h2_count_time = time;
      }
      
      if(g_ptz.vert_pid.hall_h3_count_time_e > g_ptz.vert_pid.hall_h3_count_time_s)
      {
        g_ptz.vert_pid.hall_h3_count_time = 
          g_ptz.vert_pid.hall_h3_count_time_e - g_ptz.vert_pid.hall_h3_count_time_s;
      }
      else
      {
        g_ptz.vert_pid.hall_h3_count_time = time;
      }
      
      //??????????§Ý???????????
      if(g_ptz.vert_pid.hall_h123_count_time_e > g_ptz.vert_pid.hall_h123_count_time_s)
      {
        g_ptz.vert_pid.hall_h123_count_time = 
          g_ptz.vert_pid.hall_h123_count_time_e - g_ptz.vert_pid.hall_h123_count_time_s;
      }
      else
      {
        g_ptz.vert_pid.hall_h123_count_time = time;
      }
      
      //?????????????????????????????
      i = 0;
      if(g_ptz.vert_pid.hall_h1_count >= 2 && g_ptz.vert_pid.hall_h1_count_time > 0)
      {
        g_ptz.vert_pid.hall_h1_motor_speed = 
          60000.0 * (float)(g_ptz.vert_pid.hall_h1_count - 1) / 
            (float)g_ptz.vert_pid.hall_h1_count_time  /
              PTZ_VERT_MOTOR_POLE_PAIRS;
        i++;
      }
      else
      {
        g_ptz.vert_pid.hall_h1_motor_speed = 0;
      }
      
      
      if(g_ptz.vert_pid.hall_h2_count >= 2 && g_ptz.vert_pid.hall_h2_count_time > 0)
      {
        g_ptz.vert_pid.hall_h2_motor_speed = 
          60000.0 * (float)(g_ptz.vert_pid.hall_h2_count - 1) / 
            (float)g_ptz.vert_pid.hall_h2_count_time  /
              PTZ_VERT_MOTOR_POLE_PAIRS;
        i++;
      }
      else
      {
        g_ptz.vert_pid.hall_h2_motor_speed = 0;
      }      
         
         
      if(g_ptz.vert_pid.hall_h3_count >= 2 && g_ptz.vert_pid.hall_h3_count_time > 0)
      {
        g_ptz.vert_pid.hall_h3_motor_speed = 
          60000.0 * (float)(g_ptz.vert_pid.hall_h3_count - 1) / 
            (float)g_ptz.vert_pid.hall_h3_count_time  /
              PTZ_VERT_MOTOR_POLE_PAIRS;
        i++;
      }
      else
      {
        g_ptz.vert_pid.hall_h3_motor_speed = 0;
      }   
      
      g_ptz.vert_pid.hall_h123_count = g_ptz.vert_pid.hall_h1_count +
                                       g_ptz.vert_pid.hall_h2_count +
                                       g_ptz.vert_pid.hall_h3_count;
      //?????????????????????
      if(g_ptz.vert_pid.hall_h123_count >= 2 && g_ptz.vert_pid.hall_h123_count_time > 0)
      {
        g_ptz.vert_pid.hall_h123_motor_speed = 
          60000.0 * (float)(g_ptz.vert_pid.hall_h123_count - 1) / 
            (float)g_ptz.vert_pid.hall_h123_count_time  /
              PTZ_VERT_ONE_CYCLE_HALL_NUM;
          
        i++;
      }
      else
      {
        g_ptz.vert_pid.hall_h123_motor_speed = 0;
      }
      
       
      //????????????
      if(i > 0)
      {
        g_ptz.vert_motor_speed_hall_actual = (g_ptz.vert_pid.hall_h1_motor_speed +
                                              g_ptz.vert_pid.hall_h2_motor_speed +
                                              g_ptz.vert_pid.hall_h3_motor_speed +
                                              g_ptz.vert_pid.hall_h123_motor_speed) / i;
      }
      else
      {
        g_ptz.vert_motor_speed_hall_actual = 0;
      }
      
      */
      
      /* ********************************** */
      OSTimeDlyHMSM(0u, 0u, 0u, PTZ_VERT_PID_T);
      g_ptz.vert_pid.hall_h123_count = g_ptz.vert_pid.hall_h1_count +
                                       g_ptz.vert_pid.hall_h2_count +
                                       g_ptz.vert_pid.hall_h3_count;
      
      static uint32_t time_vert, lastTime_vert, vertTime;
      time_vert = OSTimeGet();
      vertTime = time_vert - lastTime_vert;
      if ( vertTime < 0 )
      {
        continue;
      }
      lastTime_vert = time_vert;
      
      if(g_ptz.vert_pid.hall_h123_count >= 2)
      {
//        g_ptz.vert_pid.hall_h123_motor_speed = 
//          60000.0 * (float)(g_ptz.vert_pid.hall_h123_count /*-1*/) / 
//            (float)PTZ_VERT_PID_T / PTZ_VERT_ONE_CYCLE_HALL_NUM;
        g_ptz.vert_pid.hall_h123_motor_speed = 
          60000.0 * (float)(g_ptz.vert_pid.hall_h123_count/* - 1*/) / 
            (float)vertTime / PTZ_VERT_ONE_CYCLE_HALL_NUM;          
      }
      
      g_ptz.vert_motor_speed_hall_actual = g_ptz.vert_pid.hall_h123_motor_speed;
      /* ********************************** */
      
      //?§Ø??????????
      if(g_ptz.vert_pid.hall_h1_count == 0 ||
         g_ptz.vert_pid.hall_h2_count == 0 ||
         g_ptz.vert_pid.hall_h3_count == 0)
      {//??????????????????0
        g_ptz.vert_motor_speed_hall_actual = 0;
      }
      
      g_ptz.vert_motor_speed_actual = g_ptz.vert_motor_speed_hall_actual;
      
      //??????????????????
      g_ptz.vert_speed_hall_actual = g_ptz.vert_motor_speed_hall_actual / PTZ_VERT_RATIO;
      g_ptz.vert_speed_actual = g_ptz.vert_speed_hall_actual;
      
#endif 
      
   
     
    }
    //PID????
    if(g_ptz.vert_start_stop_set == PTZ_VERT_START &&
       g_ptz.vert_pid.mode       == PID_ADJUST_SPEED)
    {
      
      #ifdef PTZ_CONTROL_SPEED_L6235D
      //?§Ø??????????????????
      if(g_ptz.vert_pid.PidUT_float <= 0 && g_ptz.vert_speed_actual > g_ptz.vert_speed_set * 2)
      {
        v_pwm_duty_change(0);
        g_ptz.vert_pid.director_speed_state = 0;
        g_ptz.vert_pid.mode = DIRECTOR_ADJUST_SPEED;
      }
      #endif
      
  
      
      /*???????????????????????????????????
      1.???????????????????????????
      2.?????Ú…????70????????PID?????*/
      
      
//      if(g_ptz.vert_speed_actual < g_ptz.vert_speed_set * PTZ_VERT_PRO_ADD_RANGE_LESS)
//      {
////        if(time > PTZ_VERT_PID_ADD)
////        {
////          time = PTZ_VERT_PID_ADD;
////        }
////        g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.PidUT_float + time/*PTZ_VERT_PID_ADD*/;
////        //?›¥???????PID????
////        g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError;
////        g_ptz.vert_pid.LastError = g_ptz.vert_speed_set - g_ptz.vert_speed_actual;
//        
//        g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.PidUT_float * PTZ_VERT_PRO_ADD_MUL_LESS;
//        if(g_ptz.vert_pid.PidUT_float < PTZ_VERT_PID_ADD)
//        {
//          g_ptz.vert_pid.PidUT_float = PTZ_VERT_PID_ADD;
//        }
//        //?›¥???????PID????
//        g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError;
//        g_ptz.vert_pid.LastError = g_ptz.vert_speed_set - g_ptz.vert_speed_actual;
//      }
//      else 
        if(g_ptz.vert_speed_actual > g_ptz.vert_speed_set * 2)
      {//???????Ú…????3??
        g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.PidUT_float - 100.0;//* 1.3/*PTZ_HORI_PID_ADD*/;
       
        g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError;
        g_ptz.vert_pid.LastError = g_ptz.vert_speed_set - g_ptz.vert_speed_actual;
      }
      else
      {
        //????PID??????????
        g_ptz.vert_pid.PidUT_float = ptz_vert_pid_calculate(g_ptz.vert_speed_actual) + g_ptz.vert_pid.LastUT_float;      
        
        //????PID?????????????????????????????????????????????¦¶??
        //?????PID????????
        if(fabs(g_ptz.vert_pid.PidUT_float - g_ptz.vert_pid.LastUT_float) > PTZ_VERT_PID_OUTPUT_LIMIT)//???PID??????
        {
          if(g_ptz.vert_pid.PidUT_float > g_ptz.vert_pid.LastUT_float)
          {
            g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.LastUT_float + PTZ_VERT_PID_OUTPUT_LIMIT;
          }
          else
          {
            g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.LastUT_float - PTZ_VERT_PID_OUTPUT_LIMIT;
          }
        }
      }
 
      
      if(g_ptz.vert_pid.PidUT_float < PTZ_VERT_VR_MIN)//???????????????§³?
      {
        g_ptz.vert_pid.PidUT_float = PTZ_VERT_VR_MIN;
      }
      if(g_ptz.vert_pid.PidUT_float > PTZ_VERT_VR_MAX)//?????????????????
      {
        g_ptz.vert_pid.PidUT_float = PTZ_VERT_VR_MAX;
      }
      
      g_ptz.vert_pid.PidUT_uint = (unsigned int)(g_ptz.vert_pid.PidUT_float + 0.5);
      
      //????PID?????????????????¦¶
      if(g_ptz.vert_pid.PidUT_uint <= PTZ_VERT_VR_MIN)//???????????????§³?
      {
        g_ptz.vert_pid.PidUT_uint = PTZ_VERT_VR_MIN;
      }
      if(g_ptz.vert_pid.PidUT_uint > PTZ_VERT_VR_MAX)//?????????????????
      {
        g_ptz.vert_pid.PidUT_uint = PTZ_VERT_VR_MAX;
      }
      
      
      //??PID??????????????????????????????§à?
//      vert_dac1_data_out( g_ptz.vert_pid.PidUT_uint);
      
      v_pwm_duty_change(g_ptz.vert_pid.PidUT_uint);
      //?????PID????????
      g_ptz.vert_pid.LastUT_float = g_ptz.vert_pid.PidUT_float;
    }
    
    
    
#ifdef PTZ_CONTROL_SPEED_L6235D    
    //????pwm?????????????
    if(g_ptz.vert_start_stop_set == PTZ_VERT_START &&
       g_ptz.vert_pid.mode       == DIRECTOR_ADJUST_SPEED)
    {
      /******/
      //?§Ø??????????PID????
      if(g_ptz.vert_pid.director_speed_start_t <= 0 &&
         g_ptz.vert_speed_actual < (g_ptz.vert_speed_set / 2.0))
      {
        vert_dir_speed_stop();
//        adc7311_vert_vr(0);
        g_ptz.vert_pid.director_speed_state = 0;
        g_ptz.vert_pid.SumError  = 0;//??????
        g_ptz.vert_pid.LastError = 0;//????????????
        g_ptz.vert_pid.PrevError = 0;//????????????
        g_ptz.vert_pid.LastUT_float    = 0;//???PID????u(t)
        g_ptz.vert_pid.PidUT_float     = 0;//PID???????
//        l6235d_vert_set_direction(g_ptz.vert_direction_set);
        g_ptz.vert_pid.mode = PID_ADJUST_SPEED;
        continue;
      }
      /*****/
      switch(g_ptz.vert_pid.director_speed_state)
      {
        case 0://???????????
          if(g_ptz.vert_direction_set == PTZ_VERT_DIR_DOWN)
          {
            g_ptz.vert_pid.director_speed_direction = PTZ_VERT_DIR_UP;
          }
          if(g_ptz.vert_direction_set == PTZ_VERT_DIR_UP)
          {
            g_ptz.vert_pid.director_speed_direction = PTZ_VERT_DIR_DOWN;
          }
          g_ptz.vert_pid.director_speed_t = PTZ_DIRECTOR_SPEED_T;//20ms
//          g_ptz.vert_pid.director_speed_vref = 0;
//          adc7311_vert_vr(g_ptz.vert_pid.director_speed_vref);
          g_ptz.vert_pid.director_speed_start_t = 0;
          g_ptz.vert_pid.director_speed_stop_t = PTZ_DIRECTOR_SPEED_T - g_ptz.vert_pid.director_speed_start_t;
          g_ptz.vert_pid.director_speed_state ++;
          break;
          
        case 1://???????????
          if(g_ptz.vert_speed_actual > g_ptz.vert_speed_set &&
             g_ptz.vert_pid.director_speed_start_t < g_ptz.vert_pid.director_speed_t)
          {
            g_ptz.vert_pid.director_speed_start_t ++;
          }
          if(g_ptz.vert_speed_actual < g_ptz.vert_speed_set &&
             g_ptz.vert_pid.director_speed_start_t > 0)
          {
            g_ptz.vert_pid.director_speed_start_t --;
          }
          g_ptz.vert_pid.director_speed_stop_t = 
            g_ptz.vert_pid.director_speed_t - g_ptz.vert_pid.director_speed_start_t;
          break;
      }
    }
#endif
    
    if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
    {
      g_ptz.vert_pid.roll_start = 0;
      g_ptz.vert_speed_actual = 0;
      OSTimeDlyHMSM(0u, 0u, 0u, 10u);
    }
    
#ifdef PTZ_PID_HALL_SPEED //????????????
      //????????????? 
      if(g_ptz.vert_pid.hall_h1_count >= UNSIGNED_INT_MAX_VALUE)
      {
        g_ptz.vert_pid.hall_h1_count = 0;
      }
      if(g_ptz.vert_pid.hall_h2_count >= UNSIGNED_INT_MAX_VALUE)
      {
        g_ptz.vert_pid.hall_h2_count = 0;
      }
      if(g_ptz.vert_pid.hall_h3_count >= UNSIGNED_INT_MAX_VALUE)
      {
        g_ptz.vert_pid.hall_h3_count = 0;
      }
#endif
    
  }
}






//???????????,????PWM??????????,,,??????????????????????????????????????
static void ptz_vert_director_speed_pwm_task()
{
  while(1)
  {//????pwm??
    if(g_ptz.vert_start_stop_set == PTZ_VERT_START &&
       g_ptz.vert_pid.mode       == DIRECTOR_ADJUST_SPEED)
    {
      switch(g_ptz.vert_pid.director_speed_state)
      {  
        case 1:
          if(g_ptz.vert_start_stop_set == PTZ_VERT_START &&
             g_ptz.vert_pid.director_speed_start_t > 0)
          {//????
//            l6235d_vert_set_direction(g_ptz.vert_pid.director_speed_direction);
            vert_dir_speed_start(80);
            OSTimeDlyHMSM(0u, 0u, 0u, g_ptz.vert_pid.director_speed_start_t);
          }
          if(g_ptz.vert_start_stop_set == PTZ_VERT_START &&
             g_ptz.vert_pid.director_speed_stop_t > 0)
          {
//            l6235d_vert_set_direction(g_ptz.vert_direction_actual);
            vert_dir_speed_stop();
            OSTimeDlyHMSM(0u, 0u, 0u, g_ptz.vert_pid.director_speed_stop_t);
          }
          break;
          
        default:
          OSTimeDlyHMSM(0u, 0u, 0u, 10u);
          break;
      }
    }
    else
    {
      OSTimeDlyHMSM(0u, 0u, 0u, 10u);
    }
  }
}

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



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

}
/******************************************************************************/


void init_speed_module(void)
{
  bldc_pwm_init();//??????????????
  ptz_pid_init();
  creat_task_hori_pid();
  creat_task_vert_pid();
  
#ifdef PTZ_CONTROL_SPEED_L6235D
  creat_task_director_speed_pwm();
#endif
  
}
#endif









