#include "ptz_header_file.h"
//#include "rotate_plan.h"


///云台定位回传到点回复
void ptz_location_return_return(char hori_vert)
{
  if(g_ptz.location_return.net_udp_location_switch == LOCATION_RETURN_ON)
  {//如果是到了指定位置需要回传数据
    if(hori_vert == LOCATION_HORI && g_ptz.location_return.hori_cmd == LOCATION_UDP_CMD_ON)
    {//转动到水平位置回复

       g_ptz.location_return.hori_angle[2] = 0xc5;//修改命令
       g_ptz.location_return.hori_angle[3] = LOCATION_HORI;//回传水平位置到达
       g_ptz.location_return.hori_angle[6] = 
       MotorCalPelcoDSUM(g_ptz.location_return.hori_angle,sizeof(g_ptz.location_return.hori_angle));
       
       //发送数据
       send_udp_data_aim(g_ptz.location_return.hori_angle, sizeof(g_ptz.location_return.hori_angle), 
                         (struct sockaddr*)&g_ptz.location_return.hori_cmd_from, g_ptz.location_return.hori_cmd_fromlen);
       g_ptz.location_return.hori_cmd = LOCATION_CMD_OFF;
    }
    if(hori_vert == LOCATION_VERT && g_ptz.location_return.vert_cmd == LOCATION_UDP_CMD_ON)
    {//转动到垂直位置回复
       g_ptz.location_return.vert_angle[2] = 0xc5;//修改命令
       g_ptz.location_return.vert_angle[3] = LOCATION_VERT;//回传垂直位置到达
       g_ptz.location_return.vert_angle[6] = 
       MotorCalPelcoDSUM(g_ptz.location_return.vert_angle,sizeof(g_ptz.location_return.vert_angle));
       
       //发送数据
       send_udp_data_aim(g_ptz.location_return.vert_angle, sizeof(g_ptz.location_return.vert_angle), 
                         (struct sockaddr*)&g_ptz.location_return.vert_cmd_from, g_ptz.location_return.vert_cmd_fromlen);
       g_ptz.location_return.vert_cmd = LOCATION_CMD_OFF;
    }
  }
  
  
  if(g_ptz.location_return.uart_422_location_switch == LOCATION_RETURN_ON)
  {//如果是到了指定位置需要回传数据
    if(hori_vert == LOCATION_HORI && g_ptz.location_return.hori_cmd == LOCATION_UART_422_CMD_ON)
    {//转动到水平位置回复

       g_ptz.location_return.hori_angle[2] = 0xc5;//修改命令
       g_ptz.location_return.hori_angle[3] = LOCATION_HORI;//回传水平位置到达
       g_ptz.location_return.hori_angle[6] = 
       MotorCalPelcoDSUM(g_ptz.location_return.hori_angle,sizeof(g_ptz.location_return.hori_angle));
       
       //发送数据
       ptz_send_data(PTZ_UART_422, g_ptz.location_return.hori_angle, 
                     sizeof(g_ptz.location_return.hori_angle));
       
       g_ptz.location_return.hori_cmd = LOCATION_CMD_OFF;
    }
    if(hori_vert == LOCATION_VERT && g_ptz.location_return.vert_cmd == LOCATION_UART_422_CMD_ON)
    {//转动到垂直位置回复
       g_ptz.location_return.vert_angle[2] = 0xc5;//修改命令
       g_ptz.location_return.vert_angle[3] = LOCATION_VERT;//回传垂直位置到达
       g_ptz.location_return.vert_angle[6] = 
         MotorCalPelcoDSUM(g_ptz.location_return.vert_angle,sizeof(g_ptz.location_return.vert_angle));
       
       //发送数据
       ptz_send_data(PTZ_UART_422, g_ptz.location_return.vert_angle, 
                     sizeof(g_ptz.location_return.vert_angle));
       
       g_ptz.location_return.vert_cmd = LOCATION_CMD_OFF;
    }
  }
  
  
  if(g_ptz.location_return.uart_485_location_switch == LOCATION_RETURN_ON)
  {//如果是到了指定位置需要回传数据
    if(hori_vert == LOCATION_HORI && g_ptz.location_return.hori_cmd == LOCATION_UART_485_CMD_ON)
    {//转动到水平位置回复

       g_ptz.location_return.hori_angle[2] = 0xc5;//修改命令
       g_ptz.location_return.hori_angle[3] = LOCATION_HORI;//回传水平位置到达
       g_ptz.location_return.hori_angle[6] = 
         MotorCalPelcoDSUM(g_ptz.location_return.hori_angle,sizeof(g_ptz.location_return.hori_angle));
       
       //发送数据
       ptz_send_data(PTZ_UART_485, g_ptz.location_return.hori_angle, 
                     sizeof(g_ptz.location_return.hori_angle));
       
       g_ptz.location_return.hori_cmd = LOCATION_CMD_OFF;
    }
    if(hori_vert == LOCATION_VERT && g_ptz.location_return.vert_cmd == LOCATION_UART_485_CMD_ON)
    {//转动到垂直位置回复
       g_ptz.location_return.vert_angle[2] = 0xc5;//修改命令
       g_ptz.location_return.vert_angle[3] = LOCATION_VERT;//回传垂直位置到达
       g_ptz.location_return.vert_angle[6] = 
       MotorCalPelcoDSUM(g_ptz.location_return.vert_angle,sizeof(g_ptz.location_return.vert_angle));
       
       //发送数据
       ptz_send_data(PTZ_UART_485, g_ptz.location_return.vert_angle, 
                     sizeof(g_ptz.location_return.vert_angle));
       
       g_ptz.location_return.vert_cmd = LOCATION_CMD_OFF;
    }
  }
}
void ptz_location_return_angle_save(char hori_vert, unsigned char *data, char dev)
{
  if(g_ptz.location_return.uart_485_location_switch == LOCATION_RETURN_ON || 
     g_ptz.location_return.net_udp_location_switch  == LOCATION_RETURN_ON||
     g_ptz.location_return.uart_422_location_switch == LOCATION_RETURN_ON)
  {
    
    //防止角度定位回传和预置位回传发生冲突，即定位回传和预置位到位回传只能回传一个
    g_ptz.preset_bit_return.preset_bit_location_cmd  = 0;
    
    
    //如果是刚刚接收到角度控制指令，则保存数据
    if(hori_vert == LOCATION_HORI)
    {//收到水平角度控制指令 
       //保存具体指令内容
       memcpy(g_ptz.location_return.hori_angle, data, sizeof(g_ptz.location_return.hori_angle));
    }
    if(hori_vert == LOCATION_VERT)
    {//收到垂直角度控制指令
       //保存具体指令内容
       memcpy(g_ptz.location_return.vert_angle, data, sizeof(g_ptz.location_return.vert_angle));
    }
    
    
    if(hori_vert == LOCATION_HORI)
    {
      switch(dev)
      {
        case PTZ_UDP:
          g_ptz.location_return.hori_cmd = LOCATION_UDP_CMD_ON;
          break;
          
        case PTZ_UART_422:
           g_ptz.location_return.hori_cmd = LOCATION_UART_422_CMD_ON;
          break;
          
        case PTZ_UART_485:
           g_ptz.location_return.hori_cmd = LOCATION_UART_485_CMD_ON;
          break;
      }
    }
    
    if(hori_vert == LOCATION_VERT)
    {
      switch(dev)
      {
        case PTZ_UDP:
          g_ptz.location_return.vert_cmd = LOCATION_UDP_CMD_ON;
          break;
          
        case PTZ_UART_422:
           g_ptz.location_return.vert_cmd = LOCATION_UART_422_CMD_ON;
          break;
          
        case PTZ_UART_485:
           g_ptz.location_return.vert_cmd = LOCATION_UART_485_CMD_ON;
          break;
      }
    }

  }

}
char ptz_hori_rotate_plan(char rot_mode)
{
  float M,M1,M2;
  g_ptz.hori_rotate_plan.start_angle = g_ptz.hori_angle_actual;
  g_ptz.hori_rotate_plan.stop_angle  = g_ptz.hori_angle_control;
  g_ptz.hori_rotate_plan.max_speed   = g_ptz.hori_speed_control;
  g_ptz.hori_rotate_plan.rotate_switch = 1;//先置为需要转动
  //首先判断云台速度是否满足要求
  if(g_ptz.hori_rotate_plan.max_speed > PTZ_HORI_MAX_SPEED)
  {
    g_ptz.hori_rotate_plan.max_speed = PTZ_HORI_MAX_SPEED;
  }
  if(g_ptz.hori_rotate_plan.max_speed < PTZ_HORI_MIN_SPEED)
  {
    g_ptz.hori_rotate_plan.max_speed = PTZ_HORI_MIN_SPEED;
  }
  //判断停止角度是否满足要求
  while(g_ptz.hori_rotate_plan.stop_angle < 0)
  {
    g_ptz.hori_rotate_plan.stop_angle = 360.0 + g_ptz.hori_rotate_plan.stop_angle;
  }
  while(g_ptz.hori_rotate_plan.stop_angle > 360.0)
  {
    g_ptz.hori_rotate_plan.stop_angle = g_ptz.hori_rotate_plan.stop_angle - 360.0;
  }
  
  //根据转动要求的模式规划路径
  switch(rot_mode)
  {
    case PTZ_HORI_MIN_DISTANCE://if(rot_mode == PTZ_HORI_MIN_DISTANCE)
      M1 = fabs(g_ptz.hori_rotate_plan.start_angle - g_ptz.hori_rotate_plan.stop_angle);
      if(g_ptz.hori_rotate_plan.start_angle < g_ptz.hori_rotate_plan.stop_angle)
      {
        M2 = fabs(360.0 - g_ptz.hori_rotate_plan.stop_angle) + 
          fabs(g_ptz.hori_rotate_plan.start_angle - 0);
        if(M1 <= M2)
        {
          M = M1;
          g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_RIGHT;
        }
        else //if(M1 > M2)
        {
          M = M2;
          g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_LEFT;
        }
      }
      if(g_ptz.hori_rotate_plan.start_angle > g_ptz.hori_rotate_plan.stop_angle)
      {
        M2 = fabs(360.0 - g_ptz.hori_rotate_plan.start_angle) + 
          fabs(g_ptz.hori_rotate_plan.stop_angle - 0);
        if(M1 < M2)
        {
          M = M1;
          g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_LEFT;
        }
        else //if(M1 >= M2)
        {
          M = M2;
          g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_RIGHT;
        }
      }
      if(g_ptz.hori_rotate_plan.start_angle == g_ptz.hori_rotate_plan.stop_angle)
      {
        M = 0;
        g_ptz.hori_rotate_plan.rotate_switch = 0;
        return 1;
      }
      //转动总距离
      g_ptz.hori_rotate_plan.total_rotate_range = M;
    break;
    
      
    case PTZ_HORI_MAX_DISTANCE://if(rot_mode == PTZ_HORI_MAX_DISTANCE)
      M1 = fabs(g_ptz.hori_rotate_plan.start_angle - g_ptz.hori_rotate_plan.stop_angle);
      if(g_ptz.hori_rotate_plan.start_angle < g_ptz.hori_rotate_plan.stop_angle)
      {
        M2 = fabs(360.0 - g_ptz.hori_rotate_plan.stop_angle) + 
          fabs(g_ptz.hori_rotate_plan.start_angle - 0);
        if(M1 <= M2)
        {
          M = M2;
          g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_LEFT;
        }
        else //if(M1 > M2)
        {
          M = M1;
          g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_RIGHT;
        }
      }
      if(g_ptz.hori_rotate_plan.start_angle > g_ptz.hori_rotate_plan.stop_angle)
      {
        M2 = fabs(360.0 - g_ptz.hori_rotate_plan.start_angle) + 
          fabs(g_ptz.hori_rotate_plan.stop_angle - 0);
        if(M1 < M2)
        {
          M = M2;
          g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_RIGHT;
        }
        else //if(M1 >= M2)
        {
          M = M1;
          g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_LEFT;
        }
      }
      if(g_ptz.hori_rotate_plan.start_angle == g_ptz.hori_rotate_plan.stop_angle)
      {
        M = 0;
        g_ptz.hori_rotate_plan.rotate_switch = 0;
        return 1;
      }
      //转动总距离
      g_ptz.hori_rotate_plan.total_rotate_range = M;
    break;
     
    case PTZ_HORI_RIGHT_ANGLE://if(rot_mode == PTZ_HORI_RIGHT_ANGLE),右转到指定角度
      M1 = fabs(g_ptz.hori_rotate_plan.start_angle - g_ptz.hori_rotate_plan.stop_angle);
      g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_RIGHT;
      if(g_ptz.hori_rotate_plan.start_angle < g_ptz.hori_rotate_plan.stop_angle)
      {
        M = M1;
      }
      if(g_ptz.hori_rotate_plan.start_angle > g_ptz.hori_rotate_plan.stop_angle)
      {
        M2 = fabs(360.0 - g_ptz.hori_rotate_plan.start_angle) + 
          fabs(g_ptz.hori_rotate_plan.stop_angle - 0);
        M = M2;
      }
      if(g_ptz.hori_rotate_plan.start_angle == g_ptz.hori_rotate_plan.stop_angle)
      {
        M = 0;
        g_ptz.hori_rotate_plan.rotate_switch = 0;
        return 1;
      }
      //转动总距离
      g_ptz.hori_rotate_plan.total_rotate_range = M;
    break;
       
    case PTZ_HORI_LEFT_ANGLE://if(rot_mode == PTZ_HORI_LEFT_ANGLE)，左转到指定角度
      M1 = fabs(g_ptz.hori_rotate_plan.start_angle - g_ptz.hori_rotate_plan.stop_angle);
      g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_LEFT;
      if(g_ptz.hori_rotate_plan.start_angle < g_ptz.hori_rotate_plan.stop_angle)
      {
        M2 = fabs(360.0 - g_ptz.hori_rotate_plan.stop_angle) + 
          fabs(g_ptz.hori_rotate_plan.start_angle - 0);
        M = M2;
      }
      if(g_ptz.hori_rotate_plan.start_angle > g_ptz.hori_rotate_plan.stop_angle)
      {
        M = M1;
      }
      if(g_ptz.hori_rotate_plan.start_angle == g_ptz.hori_rotate_plan.stop_angle)
      {
        M = 0;
        g_ptz.hori_rotate_plan.rotate_switch = 0;
        return 1;
      }
      //转动总距离
      g_ptz.hori_rotate_plan.total_rotate_range = M;
    break;
    
    case PTZ_HORI_RIGHT_CYCLE:
      g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_RIGHT;
      M = 360.0;
      g_ptz.hori_rotate_plan.total_rotate_range = M;
      break;
      
    case PTZ_HORI_LEFT_CYCLE:
      g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_LEFT;
      M = 360.0;
      g_ptz.hori_rotate_plan.total_rotate_range = M;
      break;
      
    case PTZ_HORI_RIGHT_KEEP:
      g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_RIGHT;
      return 1;
    break;
    
    case PTZ_HORI_LEFT_KEEP:
      g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_LEFT;
      return 1;
    break; 
    
    case PTZ_HORI_RIGHT_ANGLE_INC://右转角度增量规划
      g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_RIGHT;
      g_ptz.hori_rotate_plan.total_rotate_range = g_ptz.hori_angle_right_inc_control;
      if(g_ptz.hori_rotate_plan.total_rotate_range == 0)
      {
        g_ptz.hori_rotate_plan.rotate_switch = 0;
        return 1;
      }
      break;
      
    case PTZ_HORI_LEFT_ANGLE_INC://左转角度增量规划
      g_ptz.hori_rotate_plan.direction = PTZ_HORI_DIR_LEFT;
      g_ptz.hori_rotate_plan.total_rotate_range = g_ptz.hori_angle_left_inc_control;
      if(g_ptz.hori_rotate_plan.total_rotate_range == 0)
      {
        g_ptz.hori_rotate_plan.rotate_switch = 0;
        return 1;
      }
      break;  
  }
  
  //计算刹车最近点
  if(g_ptz.hori_rotate_plan.direction == PTZ_HORI_DIR_RIGHT)
  {
    /*计算最近刹车点*/
     g_ptz.hori_rotate_plan.stop_near_angle = 
       g_ptz.hori_rotate_plan.stop_angle - PTZ_HORI_STOP_NEAR_DISTANCE ;
     
    if(g_ptz.hori_rotate_plan.stop_near_angle < 0)
    {
      g_ptz.hori_rotate_plan.stop_near_angle = 
        g_ptz.hori_rotate_plan.stop_near_angle + 360.0;
    }
    
    /*计算最远刹车点*/
    g_ptz.hori_rotate_plan.stop_far_angle = 
      g_ptz.hori_rotate_plan.stop_angle + PTZ_HORI_STOP_FAR_DISTANCE;
    
    if(g_ptz.hori_rotate_plan.stop_far_angle > 360.0)
    {
      g_ptz.hori_rotate_plan.stop_far_angle = 
        g_ptz.hori_rotate_plan.stop_far_angle - 360.0;
    }
  }
  if(g_ptz.hori_rotate_plan.direction == PTZ_HORI_DIR_LEFT)
  {
    /*计算最近刹车点*/
    g_ptz.hori_rotate_plan.stop_near_angle = 
      g_ptz.hori_rotate_plan.stop_angle + PTZ_HORI_STOP_NEAR_DISTANCE;
    
    if(g_ptz.hori_rotate_plan.stop_angle > 360.0)
    {
      g_ptz.hori_rotate_plan.stop_near_angle = 
        g_ptz.hori_rotate_plan.stop_near_angle - 360.0;
    }
    /*计算最远刹车点*/
    g_ptz.hori_rotate_plan.stop_far_angle = 
      g_ptz.hori_rotate_plan.stop_angle - PTZ_HORI_STOP_FAR_DISTANCE;
    
    if(g_ptz.hori_rotate_plan.stop_far_angle < 0)
    {
      g_ptz.hori_rotate_plan.stop_far_angle = 
        g_ptz.hori_rotate_plan.stop_far_angle + 360.0;
    }
  }
  
  /* 水平误差补偿 */
  if(g_ptz.hori_angle_erro_switch == 1)
  {
    if(g_ptz.hori_direction_actual != g_ptz.hori_rotate_plan.direction)
    {//两次转向不同，转向不同，有结构间隙，需要消除，误差为结构误差
      if(g_ptz.hori_rotate_plan.direction == PTZ_HORI_DIR_LEFT)
      {//左转，表示上次为右转，角度减小，加上结构误差，编码器角度到位，实际云台角度少走了误差角度,实际输出角度向右增加偏移量
        g_ptz.offset_angle.offset_anle = (g_ptz.hori_angle_error * 3.2);//0.5;结构误差进行放大处理，放大系数待定
        
      }
      if(g_ptz.hori_rotate_plan.direction == PTZ_HORI_DIR_RIGHT)
      {//右转，即上次为左转，实际输出角度向左增加偏移量
        g_ptz.offset_angle.offset_anle = (-1.0)*(g_ptz.hori_angle_error * 3.2);//-0.2;
        
      }
    }
    if(g_ptz.hori_direction_actual == g_ptz.hori_rotate_plan.direction)
    {//两次转向不同，转向不同，有结构间隙，需要消除，误差为结构误差
      if(g_ptz.hori_rotate_plan.direction == PTZ_HORI_DIR_LEFT)
      {//左转，表示上次为右转，角度减小，加上结构误差，编码器角度到位，实际云台角度少走了误差角度
        g_ptz.offset_angle.offset_anle = 0.0;//0.3;
      }
      if(g_ptz.hori_rotate_plan.direction == PTZ_HORI_DIR_RIGHT)
      {//左转，表示上次为右转，角度减小，加上结构误差，编码器角度到位，实际云台角度少走了误差角度
        g_ptz.offset_angle.offset_anle = 0.0;//-0.2;
      }
    }
  }
  
  /*计算刹车范围*/
  g_ptz.hori_rotate_plan.stop_angle_range = 
    PTZ_HORI_STOP_NEAR_DISTANCE + PTZ_HORI_STOP_FAR_DISTANCE;
  return 1;
}


char ptz_vert_rotate_plan(char rot_mode)
{
  g_ptz.vert_rotate_plan.start_angle = g_ptz.vert_angle_actual;
  g_ptz.vert_rotate_plan.stop_angle  = g_ptz.vert_angle_control;
  g_ptz.vert_rotate_plan.max_speed   = g_ptz.vert_speed_control;
  g_ptz.vert_rotate_plan.rotate_switch = 1;//先置为需要转动
  //首先判断云台速度是否满足要求
  if(g_ptz.vert_rotate_plan.max_speed > PTZ_VERT_MAX_SPEED)
  {
    g_ptz.vert_rotate_plan.max_speed = PTZ_VERT_MAX_SPEED;
  }
  if(g_ptz.vert_rotate_plan.max_speed < PTZ_VERT_MIN_SPEED)
  {
    g_ptz.vert_rotate_plan.max_speed = PTZ_VERT_MIN_SPEED;
  }
  //判断停止角度是否满足要求
  if(g_ptz.vert_rotate_plan.stop_angle < g_ptz.vert_angleP.angle_allow_min)
  {
    g_ptz.vert_rotate_plan.stop_angle = g_ptz.vert_angleP.angle_allow_min;
  }
  if(g_ptz.vert_rotate_plan.stop_angle > g_ptz.vert_angleP.angle_allow_max)
  {
    g_ptz.vert_rotate_plan.stop_angle = g_ptz.vert_angleP.angle_allow_max;
  }
  
  //根据转动要求的模式规划路径
  switch(rot_mode)
  {
    case PTZ_VERT_ANGLE:
#ifdef PTZ_PHOTOELECTRIC_SWITCH
      if(g_ptz.vert_rotate_plan.stop_angle > g_ptz.vert_rotate_plan.start_angle)
      {
        g_ptz.vert_rotate_plan.direction = PTZ_VERT_DIR_UP;
        if(g_ptz.vert_ps_sw2_state == PS_COVER)
        {
          g_ptz.vert_rotate_plan.rotate_switch = 0;//不需要转动
          return 1;
        }
      }
      if(g_ptz.vert_rotate_plan.stop_angle < g_ptz.vert_rotate_plan.start_angle)
      {
        g_ptz.vert_rotate_plan.direction = PTZ_VERT_DIR_DOWN;
        if(g_ptz.vert_ps_sw1_state == PS_COVER)
        {
          g_ptz.vert_rotate_plan.rotate_switch = 0;//不需要转动
          return 1;
        }
      }
      if(g_ptz.vert_rotate_plan.stop_angle == g_ptz.vert_rotate_plan.start_angle)
      {
        g_ptz.vert_rotate_plan.rotate_switch = 0;//不需要转动
        return 1;
      }
#endif

      break;
      
    case PTZ_VERT_UP_KEEP:
#ifdef PTZ_PHOTOELECTRIC_SWITCH
      g_ptz.vert_rotate_plan.stop_angle = g_ptz.vert_angleP.angle_allow_max;
      g_ptz.vert_rotate_plan.direction  = PTZ_VERT_DIR_UP;
      if(g_ptz.vert_angle_actual >= g_ptz.vert_angleP.angle_allow_max
         || g_ptz.vert_ps_sw2_state == PS_COVER)
      {
        g_ptz.vert_rotate_plan.rotate_switch = 0;//不需要转动
        return 1;
      }
#endif
      
      break;
      
    case PTZ_VERT_DOWN_KEEP:
#ifdef PTZ_PHOTOELECTRIC_SWITCH
      g_ptz.vert_rotate_plan.stop_angle = g_ptz.vert_angleP.angle_allow_min;
      g_ptz.vert_rotate_plan.direction  = PTZ_VERT_DIR_DOWN;
      if(g_ptz.vert_angle_actual <= g_ptz.vert_angleP.angle_allow_min
         || g_ptz.vert_ps_sw1_state == PS_COVER)
      {
        g_ptz.vert_rotate_plan.rotate_switch = 0;//不需要转动
        return 1;
      }
#endif
      break;
  }
  
  //计算转动总距离
  g_ptz.vert_rotate_plan.total_rotate_range = 
    fabs(g_ptz.vert_rotate_plan.stop_angle - g_ptz.vert_rotate_plan.start_angle);
  //计算刹车最近点和最远点
  if(g_ptz.vert_rotate_plan.direction == PTZ_VERT_DIR_UP)
  {
    /*刹车最近点*/
    g_ptz.vert_rotate_plan.stop_near_angle = 
      g_ptz.vert_rotate_plan.stop_angle - PTZ_VERT_STOP_NEAR_DISTANCE;
    /*刹车最远点*/
    g_ptz.vert_rotate_plan.stop_far_angle = 
      g_ptz.vert_rotate_plan.stop_angle + PTZ_VERT_STOP_FAR_DISTANCE;
  }
  if(g_ptz.vert_rotate_plan.direction == PTZ_VERT_DIR_DOWN)
  {
     /*刹车最近点*/
    g_ptz.vert_rotate_plan.stop_near_angle = 
      g_ptz.vert_rotate_plan.stop_angle + PTZ_VERT_STOP_NEAR_DISTANCE;
    /*刹车最远点*/
    g_ptz.vert_rotate_plan.stop_far_angle = 
      g_ptz.vert_rotate_plan.stop_angle - PTZ_VERT_STOP_FAR_DISTANCE;
  }
  //计算刹车范围
   g_ptz.vert_rotate_plan.stop_angle_range = 
    PTZ_VERT_STOP_NEAR_DISTANCE + PTZ_VERT_STOP_FAR_DISTANCE;
  return 1;
}



//void init_rotate_module()
//{  
//  task_rotate_monitor_init();
//}