/// Agent文件
///
/// 指令处理模块、处理外部接收指令
/// @file    Agent.c
/// @author  
/// @date    2022-04-22
/// @version v2.0

#include "agent_hyt.h"
#include "includes.h"
#include "gd32f4xx.h"
#include "enet_to_udp.h"
#include "ptz_struct.h"
#include "Usart.h"
#include "device_dac_out.h"
#include "ptz_header_file.h"
#include "device_wdog.h"
#include "service_autoreturn.h"
#include "device_heatresistor.h"
#include "full_bridge.h"

#include "mb85rc64.h"

char tiedian_id[3]={0};
int ptz_communicaton;

PTZ_DATA_PACK  ptz_data_pack;

static BSP_OS_SEM cmd_crc_mutex;
static BSP_OS_SEM pack_process_mutex;//共享资源锁

static unsigned char angle_error[7] = {0xff,0x00,0x00,0x00,0x00,0x00,0x00};


int ptz_communicaton = PTZ_CONNECT_STATE_DEAD;


///向指定的IP地址和端口发送数据
void send_udp_data_aim(u_int8_t *buff,u_int16_t buff_size, struct sockaddr* source, socklen_t sourcelen)
{
  BSP_OS_SemWait(&udp_send_data_mutex, 0u);
  if(ptz_communicaton == PTZ_CONNECT_STATE_SUCCESS){
	  int ret = sendto(ptz_sock_fd, buff, buff_size, 0, source, sourcelen);
	if(ret == -1){
		ptz_communicaton = PTZ_CONNECT_STATE_FAILED;
		close(ptz_sock_fd);
	}
  }
  BSP_OS_SemPost(&udp_send_data_mutex);
}
/*******************************************************************************
** 函数名称：uint8_t MotorCalPelcoDSUM(FIFO_DATA_TYPE Data)
** 函数功能：计算派尔高D协议效验码
** 变量:Data，数据头指针
*******************************************************************************/
u_int8_t MotorCalPelcoDSUM(u_int8_t *data,u_int16_t datalen)
{
  BSP_OS_SemWait(&cmd_crc_mutex, 0u);
  uint16_t checksum = 0;
  uint8_t  checksum1 = 0;
  data = data+1;
  datalen = datalen - 2;
  while(datalen--)
  {
    checksum = checksum + (u_int8_t)(*data++);
  }
  checksum1=(uint8_t)(checksum & 0x00ff);
  BSP_OS_SemPost(&cmd_crc_mutex);
  return checksum1;
}

/// 发送udp数据 
void send_udp_data(u_int8_t *buff,u_int16_t buff_size)
{
  BSP_OS_SemWait(&udp_send_data_mutex, 0u);
  if(ptz_communicaton == PTZ_CONNECT_STATE_SUCCESS){
    int ret = sendto(ptz_sock_fd, buff, buff_size, 0, (struct sockaddr*)&ptz_from, ptz_fromlen);
    if(ret == -1){
            ptz_communicaton = PTZ_CONNECT_STATE_FAILED;
            close(ptz_sock_fd);  
    }
  }
  BSP_OS_SemPost(&udp_send_data_mutex);
}
/// 发送udp广播数据 INADDR_BROADCAST
void send_udp_data_broadcast(u_int8_t *buff,u_int16_t buff_size)
{
  BSP_OS_SemWait(&udp_send_data_mutex, 0u);
  if(ptz_communicaton == PTZ_CONNECT_STATE_SUCCESS)
  {
    ptz_from.sin_addr.s_addr = INADDR_BROADCAST;    
    int ret = sendto(ptz_sock_fd, buff, buff_size, 0, 
                     (struct sockaddr*)&ptz_from, ptz_fromlen);
    if(ret == -1)
    {
      ptz_communicaton = PTZ_CONNECT_STATE_FAILED;
      close(ptz_sock_fd);
    }
  }
  BSP_OS_SemPost(&udp_send_data_mutex);
}


///云台指令回复,type回复类型
void ptz_reply(char dev, unsigned char type)
{
  //回复分为3种：
  //1.指令正确，且执行成功，收到什么回复什么；
  //2.指令错误，回复错误；
  //3.指令正确，但执行失败，回复执行失败。
  unsigned char reply_wrong[7] = {0xff,g_ptz.address,0xff,0xff,0xff,0xff,0xff};
  unsigned char reply_fail[7] = {0xff,g_ptz.address,0xee,0xee,0xee,0xee,0xee};
  unsigned char reply_right[7];
  reply_wrong[6] = MotorCalPelcoDSUM(reply_wrong,sizeof(reply_wrong));
  reply_fail[6] = MotorCalPelcoDSUM(reply_fail,sizeof(reply_fail));
  if(g_ptz.cmd_reply_switch == CMD_REPLY)
  {
    switch(type)
    {
      case CMD_RIGHT://指令正确，并执行成功
        memcpy(reply_right, g_ptz.cmd_save.cmd_data, sizeof(reply_right));
        
        ptz_send_data(dev, reply_right, sizeof(reply_right));
        
        break;
        
      case CMD_WRONG://指令错误          
        ptz_send_data(dev, reply_wrong, sizeof(reply_wrong));
        
        break;
        
      case CMD_FAIL://指令正确，但执行失败          
        ptz_send_data(dev, reply_fail, sizeof(reply_fail));
        
        break;
    }
  }
}


//发送数据
void ptz_send_data(char dev, unsigned char *buff, unsigned short int buff_size)
{
  switch(dev)
  {
    case PTZ_UDP:
      send_udp_data(buff,buff_size);
      break;
    case PTZ_UART_485:
      ptz_uart_dev_send(uart_485_handle, buff, buff_size);
      break;
    case PTZ_UART_422:
      ptz_uart_dev_send(uart_422_handle, buff, buff_size);
      break;
  }
}


/// 处理云台接收消息 
void ptz_data_pack_process(char dev, PTZ_DATA_PACK *pack)
{
    
    BSP_OS_SemWait(&pack_process_mutex, 0u);
    
    int16_t   int16_angle;
    PresetBit bit;
    
    //保存指令
    memcpy(g_ptz.cmd_save.cmd_data, (u_int8_t*)pack, sizeof(g_ptz.cmd_save.cmd_data));
    g_ptz.cmd_save.cmd_data_flag = 1;
    
    if(dev == PTZ_UDP)
    {//保存指令来源
      memcpy(&g_ptz.cmd_save.cmd_from,&ptz_from,ptz_fromlen);
      g_ptz.cmd_save.cmd_fromlen = ptz_fromlen;
      g_ptz.cmd_save.cmd_data_udp_flag = 1;
    }
    
    
  //首先判断效验码
    if(MotorCalPelcoDSUM((u_int8_t*)pack,sizeof(PTZ_DATA_PACK)) != pack->checksum)
    {
      ptz_reply(dev, CMD_WRONG);
      BSP_OS_SemPost(&pack_process_mutex);
      return;
    }
  //其次判断云台地址
    if(pack->addr != g_ptz.address)
    {
      ptz_reply(dev, CMD_WRONG);
      BSP_OS_SemPost(&pack_process_mutex);
      return;
    }
    
  //云台指令保存预处理功能
#ifdef PTZ_CMD_SAVE_BEFORE
    if(g_ptz.cmd_before_handle_swtich == CMD_BEFORE_SAVE_ON &&
       pack->command[0] != 0xba &&
       pack->command[0] != 0xbb &&
       pack->command[0] != 0xCE &&
       pack->command[0] != 0XDE &&
       pack->command[0] != 0xC0)
    {
      if(cmd_before_count < CMD_BEFORE_AMOUNT)
      {
        //保存指令
        memcpy(BeforeData[cmd_before_count].cmd_data, (u_int8_t*)pack, sizeof(BeforeData[cmd_before_count].cmd_data));
        
        if(dev == PTZ_UDP)
        {
          memcpy(&BeforeData[cmd_before_count].cmd_from, &ptz_from, ptz_fromlen);
          BeforeData[cmd_before_count].cmd_fromlen = ptz_fromlen;
        }
        
        BeforeData[cmd_before_count].cmd_dev = dev;
        BeforeData[cmd_before_count].swtich = CMD_BEFORE_ON;
        cmd_before_count ++;
      }
      else
      {
        ptz_reply(dev, CMD_FAIL);
        BSP_OS_SemPost(&pack_process_mutex);
        return;
      }
      ptz_reply(dev, CMD_RIGHT); 
      BSP_OS_SemPost(&pack_process_mutex);
      return;
    }
#endif    

/********************************* 云台停止指令 *******************************/  
    
     //最后判断是否是云台停止指令
    if(pack->command[1] == 0 && pack->command[0] == 0
       && pack->data[0] == 0 && pack->data[1] == 0)//如果指令码和数据码全为0，表示停止
    {
      g_ptz.cameralens.foucs_state = CAMERA_LES_FOCUS_STOP;
      g_ptz.cameralens.zoom_state = CAMERA_LES_ZOOM_STOP;
      if(ptz_cmd_execute_judge().manual_stop == CMD_NOT_ALLOW)
      {
        ptz_reply(dev, CMD_FAIL);
        BSP_OS_SemPost(&pack_process_mutex);
        return;
      }
      g_ptz.vert_rotate_monitor_switch = PTZ_VERT_DEC_BRAKE_A;
      g_ptz.hori_rotate_monitor_switch = PTZ_HORI_DEC_BRAKE_A;
      
      ptz_reply(dev, CMD_RIGHT);
      BSP_OS_SemPost(&pack_process_mutex);
      return;   
    }

/********************************* 标准PELCO指令 ******************************/  
    
    if(pack->command[1] != 0 && pack->command[0] == 0)//指令码2不为0,指令码1必须为0
    {
      switch(pack->command[1])
      {
 /********************************** 方位指令 *********************************/ 
      case HYT_PACK_CMD_1_FUNC_KEEP_UP: //0x08: //向上
        if(ptz_cmd_execute_judge().manual_control == CMD_NOT_ALLOW)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        }
//        read_mb_id((uint8_t*)&tiedian_id);//读取铁电ID
        g_ptz.vert_speed_control = pack->data[1] / 10.0;
        ptz_vert_rotate_plan(PTZ_VERT_UP_KEEP);
        g_ptz.vert_rotate_monitor_switch = PTZ_VERT_UP_KEEP; 
        ptz_reply(dev, CMD_RIGHT);
        break;
          
      case HYT_PACK_CMD_1_FUNC_KEEP_DUWN: //0x10://向下
        if(ptz_cmd_execute_judge().manual_control == CMD_NOT_ALLOW)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        }
        g_ptz.vert_speed_control = pack->data[1] / 10.0;
        ptz_vert_rotate_plan(PTZ_VERT_DOWN_KEEP);
        g_ptz.vert_rotate_monitor_switch = PTZ_VERT_DOWN_KEEP;
        ptz_reply(dev, CMD_RIGHT);
        break;
          
      case HYT_PACK_CMD_1_FUNC_KEEP_LEFT: //0x04://向左
        if(ptz_cmd_execute_judge().manual_control == CMD_NOT_ALLOW)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        }
        g_ptz.hori_speed_control = pack->data[0] / 10.0;
        ptz_hori_rotate_plan(PTZ_HORI_LEFT_KEEP);
        g_ptz.hori_rotate_monitor_switch = PTZ_HORI_LEFT_KEEP;
        ptz_reply(dev, CMD_RIGHT);
        break;
          
      case HYT_PACK_CMD_1_FUNC_KEEP_RIGHT: //0x02: //向右
        if(ptz_cmd_execute_judge().manual_control == CMD_NOT_ALLOW)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        }
        g_ptz.hori_speed_control = pack->data[0] / 10.0;
        ptz_hori_rotate_plan(PTZ_HORI_RIGHT_KEEP);
        g_ptz.hori_rotate_monitor_switch = PTZ_HORI_RIGHT_KEEP;
        ptz_reply(dev, CMD_RIGHT);
        break;
          
      case HYT_PACK_CMD_1_FUNC_KEEP_LEFT_UP: //0x0c://左上
        if(ptz_cmd_execute_judge().manual_control == CMD_NOT_ALLOW)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        }
        g_ptz.hori_speed_control = pack->data[0] / 10.0;
        ptz_hori_rotate_plan(PTZ_HORI_LEFT_KEEP);
        g_ptz.hori_rotate_monitor_switch = PTZ_HORI_LEFT_KEEP;
        g_ptz.vert_speed_control = pack->data[1] / 10.0;
        ptz_vert_rotate_plan(PTZ_VERT_UP_KEEP);
        g_ptz.vert_rotate_monitor_switch = PTZ_VERT_UP_KEEP; 
        ptz_reply(dev, CMD_RIGHT);
        break;
    
      case HYT_PACK_CMD_1_FUNC_KEEP_RIGHT_UP: //0x0a://右上
        if(ptz_cmd_execute_judge().manual_control == CMD_NOT_ALLOW)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        }
        g_ptz.hori_speed_control = pack->data[0] / 10.0;
        ptz_hori_rotate_plan(PTZ_HORI_RIGHT_KEEP);
        g_ptz.hori_rotate_monitor_switch = PTZ_HORI_RIGHT_KEEP;
        g_ptz.vert_speed_control = pack->data[1] / 10.0;
        ptz_vert_rotate_plan(PTZ_VERT_UP_KEEP);
        g_ptz.vert_rotate_monitor_switch = PTZ_VERT_UP_KEEP;  
        ptz_reply(dev, CMD_RIGHT);
        break;
        
      case HYT_PACK_CMD_1_FUNC_KEEP_LEFT_DUWN: //0x14://左下
        if(ptz_cmd_execute_judge().manual_control == CMD_NOT_ALLOW)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        }
        g_ptz.hori_speed_control = pack->data[0] / 10.0;
        ptz_hori_rotate_plan(PTZ_HORI_LEFT_KEEP);
        g_ptz.hori_rotate_monitor_switch = PTZ_HORI_LEFT_KEEP;
        g_ptz.vert_speed_control = pack->data[1] / 10.0;
        ptz_vert_rotate_plan(PTZ_VERT_DOWN_KEEP);
        g_ptz.vert_rotate_monitor_switch = PTZ_VERT_DOWN_KEEP;  
        ptz_reply(dev, CMD_RIGHT);
        break;
          
      case HYT_PACK_CMD_1_FUNC_KEEP_RIGHT_DUWN: //0x12://右下
        if(ptz_cmd_execute_judge().manual_control == CMD_NOT_ALLOW)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        }
        g_ptz.hori_speed_control = pack->data[0] / 10.0;
        ptz_hori_rotate_plan(PTZ_HORI_RIGHT_KEEP);
        g_ptz.hori_rotate_monitor_switch = PTZ_HORI_RIGHT_KEEP;
        g_ptz.vert_speed_control = pack->data[1] / 10.0;
        ptz_vert_rotate_plan(PTZ_VERT_DOWN_KEEP);
        g_ptz.vert_rotate_monitor_switch = PTZ_VERT_DOWN_KEEP;
        ptz_reply(dev, CMD_RIGHT);
        break;
        
 /********************************** 角度定位 *********************************/ 
        
      case HYT_PACK_CMD_1_FUNC_LOCATE_VERT_ANGLE: //0x4d://垂直角度控制
        if(ptz_cmd_execute_judge().manual_control == CMD_NOT_ALLOW)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        }
        //云台定位回传
        ptz_location_return_angle_save(LOCATION_VERT, (unsigned char *)pack, dev);
        int16_angle = (pack->data[0] << 8) | pack->data[1];
        g_ptz.vert_angle_control = int16_angle / 100.0;
        g_ptz.vert_speed_control = PTZ_VERT_BEST_SPEED;
        ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
        g_ptz.vert_rotate_monitor_switch = PTZ_VERT_ANGLE;  
        ptz_reply(dev, CMD_RIGHT);
        break;
        
      case HYT_PACK_CMD_1_FUNC_LOCATE_HORI_ANGLE: //0x4b://水平角度控制
        if(ptz_cmd_execute_judge().manual_control == CMD_NOT_ALLOW)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        }
        //云台定位回传
        ptz_location_return_angle_save(LOCATION_HORI, (unsigned char *)pack, dev);
        g_ptz.hori_angle_control = 
          ((pack->data[0] << 8) | pack->data[1]) / 100.0;
        g_ptz.hori_speed_control = PTZ_HORI_BEST_SPEED;
        ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE);
        g_ptz.hori_rotate_monitor_switch = PTZ_HORI_MIN_DISTANCE; 
        ptz_reply(dev, CMD_RIGHT);
        break;
        
 /********************************** 角度查询 *********************************/  
        
      case HYT_PACK_CMD_1_FUNC_INQUIRE_VERT_ANGLE: //0X53://查询垂直角度
        ptz_send_angle(dev, PTZ_VERT);
        break;

      case HYT_PACK_CMD_1_FUNC_INQUIRE_HORI_ANGLE: //0x51://查询水平角度
        ptz_send_angle(dev, PTZ_HORI);
        break;
        
 /********************************* 预置位设置 ********************************/ 
        
      case HYT_PACK_CMD_1_FUNC_SET_PRESET_BIT_V:  //0x03://视频设置预置位
        if(ptz_cmd_execute_judge().preset_bit_set == CMD_NOT_ALLOW || 
          ((pack->data[0] << 8) | pack->data[1]) >= PRESET_BIT_AMOUNT)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        }
        bit.hori_angle = g_ptz.hori_angle_actual;
        bit.vert_angle = g_ptz.vert_angle_actual;
        bit.focus_v    = g_ptz.cameralens.focus_v;
        bit.zoom_v     = g_ptz.cameralens.zoom_v;
        bit.enable     = PRESET_BIT_ENABLE;
        bit.crc = 
        ptz_preset_bit_crc(bit);
        ptz_preset_bit_save(bit ,((pack->data[0] << 8) | pack->data[1]));
        ptz_reply(dev, CMD_RIGHT);
        break;
         
      case HYT_PACK_CMD_1_FUNC_SELECT_PRESET_BIT: //0x07://召回预置位
        if(ptz_cmd_execute_judge().preset_bit_call == CMD_NOT_ALLOW || 
            ((pack->data[0] << 8) | pack->data[1]) >= PRESET_BIT_AMOUNT)
         {
           ptz_reply(dev, CMD_FAIL);
           BSP_OS_SemPost(&pack_process_mutex);
           return;
         }
         //从FLASH读取预置位参数
         bit = ptz_preset_bit_read(((pack->data[0] << 8) | pack->data[1]));
         //首先判断预置位编号是否正确,正确
           if((bit.crc == //判断效验码是否正确
              ptz_preset_bit_crc(bit)) &&
              bit.enable == PRESET_BIT_ENABLE)//判断预置位是否有效,有效
           {
             //调用预置位
             g_ptz.vert_angle_control = bit.vert_angle;
             g_ptz.vert_speed_control = PTZ_VERT_BEST_SPEED;
             ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
             g_ptz.vert_rotate_monitor_switch = PTZ_VERT_ANGLE;  
             
             g_ptz.hori_angle_control = bit.hori_angle;
             g_ptz.hori_speed_control = PTZ_HORI_BEST_SPEED;
             ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE);
             g_ptz.hori_rotate_monitor_switch = PTZ_HORI_MIN_DISTANCE; 
             
             g_ptz.cameralens.focus_v_bit = bit.focus_v;
             g_ptz.cameralens.zoom_v_bit  = bit.zoom_v;
             g_ptz.cameralens.zoom_state  = CAMERA_LES_ZOOM_BIT;
             g_ptz.cameralens.foucs_state = CAMERA_LES_FOCUS_BIT;
             
             ptz_preset_bit_location_return_save(((pack->data[0] << 8) | pack->data[1]),dev);
             
             ptz_reply(dev, CMD_RIGHT);
           }
           else
           {
             ptz_reply(dev, CMD_FAIL);
           }         
        break;
        
      case HYT_PACK_CMD_1_FUNC_DELETE_PRESET_BIT: //0x05://删除预置位
        if((ptz_cmd_execute_judge().preset_bit_del == CMD_NOT_ALLOW || 
           ((pack->data[0] << 8) | pack->data[1]) >= PRESET_BIT_AMOUNT) &&
           (pack->data[1] != 0xff && pack->data[0] != 0xff))
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        }
        if(pack->data[1] == 0xff && pack->data[0] == 0xff)//表示删除所有预置位
        {
         ptz_preset_bit_all_erase();
        }
        else//删除指定预置位
        {
          bit.enable = PRESET_BIT_DISABLE; 
          bit.crc = 
          ptz_preset_bit_crc(bit);
        }
        ptz_preset_bit_save(bit ,((pack->data[0] << 8) | pack->data[1]));
        ptz_reply(dev, CMD_RIGHT);        
        break;

 /******************************** 顶部辅助开关 *******************************/
        
      case HYT_PACK_CMD_1_FUNC_CONTROL_TOP_POWER_ON: //0x09://打开辅助开关
            switch(pack->data[1])
            {
            case 3:
              PHOTO_POWER_PHOTO_ON;
              g_ptz.power.aux_switch_3 = POWER_ON;
              break;
            case 4:
              FARIR_POWER_FARIR_ON;
              g_ptz.power.aux_switch_4 = POWER_ON;
              break;
            }
          // ptz_aux_switch(pack->data[1], POWER_ON);
          // ptz_reply(dev, CMD_RIGHT);
          if ( aux_switch_cfg_save() )
          {
            ptz_reply(dev,CMD_RIGHT);
          }
          else
          {
            ptz_reply(dev,CMD_FAIL);
          }
        break;
            
      case HYT_PACK_CMD_1_FUNC_CONTROL_TOP_POWER_OFF: //0x0b://关闭辅助开关           
            switch(pack->data[1])
            {
            case 3:
              PHOTO_POWER_PHOTO_OFF;
              g_ptz.power.aux_switch_3 = POWER_OFF;
              break;
            case 4:
              FARIR_POWER_FARIR_OFF;
              g_ptz.power.aux_switch_4 = POWER_OFF;
              break;
            }
            // ptz_aux_switch(pack->data[1], POWER_OFF);
            // ptz_reply(dev, CMD_RIGHT);
          if ( aux_switch_cfg_save() )
          {
            ptz_reply(dev,CMD_RIGHT);
          }
          else
          {
            ptz_reply(dev,CMD_FAIL);
          }            
        break;
        
 /********************************* flash擦除 ********************************/ 
        
      case HYT_PACK_CMD_1_FUNC_FLASH_ERASE: //0x88://擦除flash
          FLASH_BulkErase();//FLASH整片擦除
          OSTimeDlyHMSM(0u, 0u, 2u, 0u);
          ptz_reply(dev, CMD_RIGHT);
          OSTimeDlyHMSM(0u, 0u, 1u, 0u);
          GD32_RESET();//系统重启
        break;
        
      case HYT_PACK_CMD_1_HORI_ANGLE_ERROR_SWITCH://0x89,,水平角度误差消除开关
        switch(pack->data[0])
        {
          case 0://水平角度误差消除关闭
            g_ptz.hori_angle_erro_switch = 0;
            break;
          case 1://水平角度误差消除打开
            g_ptz.hori_angle_erro_switch = 1;
            break;
        }
        break;
        
      case 0x8A://误差查询
//        conut_state = 1;
//        g_ptz.hori_as5047d.as5047d_state = 0;//数据不可正常使用
        int16_angle = (unsigned short int)(g_ptz.hori_angle_error * 100 + 0.5);
        angle_error[1] = g_ptz.address;
        angle_error[2] = 0x00;
        angle_error[3] = 0x8A;
        angle_error[4] = (unsigned char)(int16_angle>>8 & 0x00ff);
        angle_error[5] = (unsigned char)(int16_angle & 0x00ff);
        angle_error[6] = MotorCalPelcoDSUM(angle_error,sizeof(angle_error));
        ptz_send_data(dev, angle_error, sizeof(angle_error));
        break;
        
//      case 0x8B://误差放大系数
//        Amplification_factor = pack->data[0];
//        ptz_reply(dev, CMD_RIGHT);
//        break;
        
      case 0x8c:// 上位机打印输出开关 ff01008c01008e
        switch(pack->data[0])
        {
        case 1://打开
          switch(dev)
          {
          case PTZ_UDP:
            g_ptz.ptz_printf.printf_fromlen = ptz_fromlen;
            memcpy(&g_ptz.ptz_printf.printf_from, &ptz_from, ptz_fromlen);
            g_ptz.ptz_printf.net_udp_printf_switch
              = LOCATION_RETURN_ON;
            break;
            
          case PTZ_UART_422:
            g_ptz.ptz_printf.uart_422_printf_switch
              = LOCATION_RETURN_ON;
            break;
            
          case PTZ_UART_485:
            g_ptz.ptz_printf.uart_485_printf_switch 
              = LOCATION_RETURN_ON;
            break;
          }
          ptz_reply(dev, CMD_RIGHT);
          break;
          
        default://关闭
          switch(dev)
          {
          case PTZ_UDP:
            g_ptz.ptz_printf.net_udp_printf_switch
              = LOCATION_RETURN_OFF;
            break;
            
          case PTZ_UART_422:
            g_ptz.ptz_printf.uart_422_printf_switch
              = LOCATION_RETURN_OFF;
            break;
            
          case PTZ_UART_485:
            g_ptz.ptz_printf.uart_485_printf_switch
              = LOCATION_RETURN_OFF;
            break;
          }
          ptz_reply(dev, CMD_RIGHT);
          break;
        }
        break;
        
      }
    }
    
    
/********************************** 自定义协议 ********************************/
    
    if(pack->command[0] != 0)
    {
      switch(pack->command[0])
      {
        
 /******************************** 角度定位（速度） ***************************/
        case HYT_PACK_CMD_0_FUNC_LOCATE_HORI_ANGLE_SPEED: //0x4b:以设定的速度到达指定水平位置
          if(ptz_cmd_execute_judge().manual_control == CMD_NOT_ALLOW)
          {
            ptz_reply(dev, CMD_FAIL);
            BSP_OS_SemPost(&pack_process_mutex);
            return;
          }
          //云台定位回传
          ptz_location_return_angle_save(LOCATION_HORI, (unsigned char *)pack, dev);
          g_ptz.hori_speed_control = pack->command[1] / 10.0;
          g_ptz.hori_angle_control = ((pack->data[0] << 8) | pack->data[1]) / 100.0;
          ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE);
          g_ptz.hori_rotate_monitor_switch = PTZ_HORI_MIN_DISTANCE; 
          ptz_reply(dev, CMD_RIGHT);
          break;
          
        case HYT_PACK_CMD_0_FUNC_LOCATE_VERT_ANGLE_SPEED: //0x4d:以设定的速度到达指定垂直位置
          if(ptz_cmd_execute_judge().manual_control == CMD_NOT_ALLOW)
          {
            ptz_reply(dev, CMD_FAIL);
            BSP_OS_SemPost(&pack_process_mutex);
            return;
          }
          //云台定位回传
          ptz_location_return_angle_save(LOCATION_VERT, (unsigned char *)pack, dev);
          int16_angle = (pack->data[0] << 8) | pack->data[1];
          g_ptz.vert_angle_control = int16_angle / 100.0;
          g_ptz.vert_speed_control = pack->command[1] / 10.0;
          ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
          g_ptz.vert_rotate_monitor_switch = PTZ_VERT_ANGLE;  
          ptz_reply(dev, CMD_RIGHT);
          break; 
          
         case HYT_PACK_CMD_0_FUNC_CONTROL_HORI_ANGLE: //0xbd:水平角度方向定位控制    
          if(ptz_cmd_execute_judge().manual_control == CMD_NOT_ALLOW)
          {
            ptz_reply(dev, CMD_FAIL);
            BSP_OS_SemPost(&pack_process_mutex);
            return;
          }
          //云台定位回传
          ptz_location_return_angle_save(LOCATION_HORI, (unsigned char *)pack, dev);
          switch(pack->command[1])
          {
              case 0x00://向右转动到指定角度
                g_ptz.hori_angle_control = ((pack->data[0] << 8) | pack->data[1]) / 100.0;
                g_ptz.hori_speed_control = PTZ_HORI_BEST_SPEED;
                ptz_hori_rotate_plan(PTZ_HORI_RIGHT_ANGLE);
                g_ptz.hori_rotate_monitor_switch = PTZ_HORI_RIGHT_ANGLE;  
                break;
                
              case 0x01://向左转动到指定角度
                g_ptz.hori_angle_control = ((pack->data[0] << 8) | pack->data[1]) / 100.0;
                g_ptz.hori_speed_control = PTZ_HORI_BEST_SPEED;
                ptz_hori_rotate_plan(PTZ_HORI_LEFT_ANGLE);
                g_ptz.hori_rotate_monitor_switch = PTZ_HORI_LEFT_ANGLE;  
                break;
             
              case 0x02://向右转动一整圈
                g_ptz.hori_angle_control = g_ptz.hori_angle_actual;
                g_ptz.hori_speed_control = PTZ_HORI_BEST_SPEED;
                ptz_hori_rotate_plan(PTZ_HORI_RIGHT_CYCLE);
                g_ptz.hori_rotate_monitor_switch = PTZ_HORI_RIGHT_CYCLE;  
                break;
                
              case 0x03://向左转动一整圈
                g_ptz.hori_angle_control = g_ptz.hori_angle_actual;
                g_ptz.hori_speed_control = PTZ_HORI_BEST_SPEED;
                ptz_hori_rotate_plan(PTZ_HORI_LEFT_CYCLE);
                g_ptz.hori_rotate_monitor_switch = PTZ_HORI_LEFT_CYCLE;   
                break;
                
          }
          ptz_reply(dev, CMD_RIGHT);
          break;
          
        case HYT_PACK_CMD_0_FUNC_ANGLE_LOCATION_RETURN_SWITCH:// 0xc5:角度定位回传打开/关闭
          switch(pack->command[1])
          {
            case LOCATION_RETURN_ON://打开
              switch(dev)
              {
                case PTZ_UDP:
                  g_ptz.location_return.hori_cmd_fromlen = ptz_fromlen;
                  memcpy(&g_ptz.location_return.hori_cmd_from, &ptz_from, ptz_fromlen);
                  g_ptz.location_return.vert_cmd_fromlen = ptz_fromlen;
                  memcpy(&g_ptz.location_return.vert_cmd_from, &ptz_from, ptz_fromlen);
                  
                  g_ptz.location_return.net_udp_location_switch
                    = LOCATION_RETURN_ON;
                  break;
                  
                case PTZ_UART_422:
                  g_ptz.location_return.uart_422_location_switch
                    = LOCATION_RETURN_ON;
                  break;
                  
                case PTZ_UART_485:
                  g_ptz.location_return.uart_485_location_switch 
                    = LOCATION_RETURN_ON;
                  break;
              }
              ptz_reply(dev, CMD_RIGHT);
              break;
              
            default://关闭
              switch(dev)
              {
                case PTZ_UDP:
                  g_ptz.location_return.net_udp_location_switch
                    = LOCATION_RETURN_OFF;
                  break;
                  
                case PTZ_UART_422:
                  g_ptz.location_return.uart_422_location_switch
                    = LOCATION_RETURN_OFF;
                  break;
                  
                case PTZ_UART_485:
                  g_ptz.location_return.uart_485_location_switch 
                    = LOCATION_RETURN_OFF;
                  break;
              }
              ptz_reply(dev, CMD_RIGHT);
              break;
          }
          break;
          
/**************************预置位设置、控制指令********************************/
          
          case HYT_PACK_CMD_0_FUNC_SET_PRESET_BIT_HORI_A:// 0xe4:通过给定角度设置预置位的水平角度
            if(ptz_cmd_execute_judge().preset_bit_set == CMD_NOT_ALLOW ||
               pack->command[1] >= PRESET_BIT_AMOUNT)
            {
              ptz_reply(dev, CMD_FAIL);
              BSP_OS_SemPost(&pack_process_mutex);
              return;
            }
            //从FLASH读取预置位参数
            bit = ptz_preset_bit_read(pack->command[1]);
          
            bit.hori_angle = 
              ((unsigned short int)((pack->data[0] << 8) | pack->data[1])) / 100.0;
            bit.enable = PRESET_BIT_ENABLE;
            bit.crc = 
              ptz_preset_bit_crc(bit);
            ptz_preset_bit_save(bit ,pack->command[1]);
            ptz_reply(dev, CMD_RIGHT);
            break;
            
          case HYT_PACK_CMD_0_FUNC_SET_PRESET_BIT_VERT_A://0xe5:通过给定角度设置预置位的垂直角度
            if(ptz_cmd_execute_judge().preset_bit_set == CMD_NOT_ALLOW ||
               pack->command[1] >= PRESET_BIT_AMOUNT)
            {
              ptz_reply(dev, CMD_FAIL);
              BSP_OS_SemPost(&pack_process_mutex);
              return;
            }
            
            //从FLASH读取预置位参数
            bit = ptz_preset_bit_read(pack->command[1]);
            
            bit.vert_angle = 
              ((short int)((pack->data[0] << 8) | pack->data[1])) / 100.0;
            bit.enable = PRESET_BIT_ENABLE;
            bit.crc = 
              ptz_preset_bit_crc(bit);
            ptz_preset_bit_save(bit, pack->command[1]);
            ptz_reply(dev, CMD_RIGHT);
            break;
            
        case HYT_PACK_CMD_0_FUNC_SET_PRESET_BIT_STOP_TIME://0xf1:设置预置位停留时间
          if(ptz_cmd_execute_judge().preset_bit_set== CMD_NOT_ALLOW ||
             pack->command[1] >= PRESET_BIT_AMOUNT)
          {
            ptz_reply(dev, CMD_FAIL);
            BSP_OS_SemPost(&pack_process_mutex);
            return;
          }
          //从FLASH读取预置位参数
          bit = ptz_preset_bit_read(pack->command[1]);
          bit.step_stop_time = (pack->data[0] << 8) | pack->data[1];
          bit.crc = 
            ptz_preset_bit_crc(bit);
          ptz_preset_bit_save(bit , pack->command[1]);
          ptz_reply(dev, CMD_RIGHT);
          break;
          
        case HYT_PACK_CMD_0_FUNC_SET_PRESET_BIT_SPEED://0xf2:设置到达预置位的速度
          if(ptz_cmd_execute_judge().preset_bit_set== CMD_NOT_ALLOW ||
             pack->command[1] >= PRESET_BIT_AMOUNT)
          {
            ptz_reply(dev, CMD_FAIL);
            BSP_OS_SemPost(&pack_process_mutex);
            return;
          }
          //从FLASH读取预置位参数
          bit = ptz_preset_bit_read(pack->command[1]);
          bit.hori_scan_speed = pack->data[0] / 10.0;
          bit.vert_scan_speed = pack->data[1] / 10.0;
          bit.crc = ptz_preset_bit_crc(bit);
          ptz_preset_bit_save(bit ,pack->command[1]); 
          ptz_reply(dev, CMD_RIGHT);
          break;
        case HYT_PACK_CMD_0_TYPE_SET_PRESET_BIT_MODEL:// 0xf0 打开预置位巡航、暂停预置位巡航、关闭预置位巡航、恢复预置位巡航
          switch(pack->command[1])
          {
            case 0x01://打开预置位扫描,预置位循环扫描
              if(ptz_cmd_execute_judge().preset_bit_scan_start == CMD_NOT_ALLOW ||
                 pack->data[0] >= PRESET_BIT_AMOUNT ||
                 pack->data[1] >= PRESET_BIT_AMOUNT)
              {
                ptz_reply(dev, CMD_FAIL);
                BSP_OS_SemPost(&pack_process_mutex);
                return;
              }
              ptz_return_clear();
              memset(&g_preset_bit_scan, 0, sizeof(g_preset_bit_scan));
              g_preset_bit_scan.start_num = pack->data[0];
              g_preset_bit_scan.end_num = pack->data[1];
              g_preset_bit_scan.state = PRESET_BIT_SCAN_START;
              g_preset_bit_scan.mode = 0;
              ptz_reply(dev, CMD_RIGHT);
              break;
              
            case 0x02://暂停预置位扫描
              if(ptz_cmd_execute_judge().preset_bit_scan_pause == CMD_NOT_ALLOW)
              {
                ptz_reply(dev, CMD_FAIL);
                BSP_OS_SemPost(&pack_process_mutex);
                return;
              }
              g_preset_bit_scan.state = PRESET_BIT_SCAN_PAUSE_A;
              ptz_reply(dev, CMD_RIGHT);
              break;
              
            case 0x03://恢复预置位扫描
              if(ptz_cmd_execute_judge().preset_bit_scan_recovery == CMD_NOT_ALLOW)
              {
                ptz_reply(dev, CMD_FAIL);
                BSP_OS_SemPost(&pack_process_mutex);
                return;
              }
              ptz_return_clear();
              g_preset_bit_scan.state = PRESET_BIT_SCAN_RECOVERY;
              ptz_reply(dev, CMD_RIGHT);
              break;
              
            case 0x04://彻底关闭预置位扫描
              if(ptz_cmd_execute_judge().preset_bit_scan_close == CMD_NOT_ALLOW)
              {
                ptz_reply(dev, CMD_FAIL);
                BSP_OS_SemPost(&pack_process_mutex);
                return;
              }
              g_preset_bit_scan.state = PRESET_BIT_SCAN_CLOSE_A;
              ptz_reply(dev, CMD_RIGHT);
              break;
              
            case 0x05://打开预置位扫描，单次预置位扫描，即从起始预置位扫描到结束预置位就自动关闭扫描
              if(ptz_cmd_execute_judge().preset_bit_scan_start == CMD_NOT_ALLOW ||
                 pack->data[0] >= PRESET_BIT_AMOUNT ||
                 pack->data[1] >= PRESET_BIT_AMOUNT)
              {
                ptz_reply(dev, CMD_FAIL);
                BSP_OS_SemPost(&pack_process_mutex);
                return;
              }
              ptz_return_clear();
              memset(&g_preset_bit_scan, 0, sizeof(g_preset_bit_scan));
              g_preset_bit_scan.start_num = pack->data[0];
              g_preset_bit_scan.end_num = pack->data[1];
              g_preset_bit_scan.state = PRESET_BIT_SCAN_START;
              g_preset_bit_scan.mode = 1;
              ptz_reply(dev, CMD_RIGHT);
              break;
          }
          break; 
          
        case HYT_PACK_CMD_0_TYPE_RETURN_PRESET_BIT_SWITCH: //0x9f,预置位回传开关
          switch(pack->command[1])
          {
            case PRESET_BIT_SCAN_END_RETURN_OFF:        //0//关闭预置位扫描结束回传
              switch(dev)
              {
                case PTZ_UDP:
                  g_ptz.preset_bit_return.preset_bit_scan_end_udp_switch 
                    = PRESET_BIT_RETURN_OFF;
                  break;
                  
                case PTZ_UART_422:
                  g_ptz.preset_bit_return.preset_bit_scan_end_uart_422_switch 
                    = PRESET_BIT_RETURN_OFF;
                  break;
                  
                case PTZ_UART_485:
                  g_ptz.preset_bit_return.preset_bit_scan_end_uart_485_switch 
                    = PRESET_BIT_RETURN_OFF;
                  break;
              }
              break;
              
            case PRESET_BIT_SCAN_END_RETURN_ON:         //1//打开预置位扫描结束回传
              switch(dev)
              {
                case PTZ_UDP:
                  g_ptz.preset_bit_return.preset_bit_scan_end_fromlen = ptz_fromlen;
                  memcpy(&g_ptz.preset_bit_return.preset_bit_scan_end_from, &ptz_from, ptz_fromlen);
                  
                  g_ptz.preset_bit_return.preset_bit_scan_end_udp_switch 
                    = PRESET_BIT_RETURN_ON;
                  break;
                  
                case PTZ_UART_422:
                  g_ptz.preset_bit_return.preset_bit_scan_end_uart_422_switch 
                    = PRESET_BIT_RETURN_ON;
                  break;
                  
                case PTZ_UART_485:
                  g_ptz.preset_bit_return.preset_bit_scan_end_uart_485_switch 
                    = PRESET_BIT_RETURN_ON;
                  break;
              }
              break;
              
            case PRESET_BIT_SCAN_LOCATION_RETURN_ON:    //2//打开预置位扫描到位回传
              switch(dev)
              {
                case PTZ_UDP:
                  g_ptz.preset_bit_return.preset_bit_scan_location_fromlen = ptz_fromlen;
                  memcpy(&g_ptz.preset_bit_return.preset_bit_scan_location_from, &ptz_from, ptz_fromlen);
                  
                  g_ptz.preset_bit_return.preset_bit_scan_location_udp_switch 
                    = PRESET_BIT_RETURN_ON;
                  break;
                  
                case PTZ_UART_422:
                  g_ptz.preset_bit_return.preset_bit_scan_location_uart_422_switch 
                    = PRESET_BIT_RETURN_ON;
                  break;
                  
                case PTZ_UART_485:
                  g_ptz.preset_bit_return.preset_bit_scan_location_uart_485_switch 
                    = PRESET_BIT_RETURN_ON;
                  break;
              }
              break;
              
            case PRESET_BIT_SCAN_LOCATION_RETURN_OFF:   //3//关闭预置位扫描到位回传
              switch(dev)
              {
                case PTZ_UDP:
                  g_ptz.preset_bit_return.preset_bit_scan_location_udp_switch 
                    = PRESET_BIT_RETURN_OFF;
                  break;
                  
                case PTZ_UART_422:
                  g_ptz.preset_bit_return.preset_bit_scan_location_uart_422_switch 
                    = PRESET_BIT_RETURN_OFF;
                  break;
                  
                case PTZ_UART_485:
                  g_ptz.preset_bit_return.preset_bit_scan_location_uart_485_switch 
                    = PRESET_BIT_RETURN_OFF;
                  break;
              }
              break;
              
            case PRESET_BIT_LOCATION_RETURN_ON:         //4//打开普通调用预置位到位回传
              switch(dev)
              {
                case PTZ_UDP:
                  g_ptz.preset_bit_return.preset_bit_location_fromlen = ptz_fromlen;
                  memcpy(&g_ptz.preset_bit_return.preset_bit_location_from, &ptz_from, ptz_fromlen);
                  
                  g_ptz.preset_bit_return.preset_bit_location_udp_switch 
                    = PRESET_BIT_RETURN_ON;
                  break;
                  
                case PTZ_UART_422:
                  g_ptz.preset_bit_return.preset_bit_location_uart_422_switch 
                    = PRESET_BIT_RETURN_ON;
                  break;
                  
                case PTZ_UART_485:
                  g_ptz.preset_bit_return.preset_bit_location_uart_485_switch 
                    = PRESET_BIT_RETURN_ON;
                  break;
              }
              break;
              
            case PRESET_BIT_LOCATION_RETURN_OFF:        //5//关闭普通调用预置位到位回传
              switch(dev)
              {
                case PTZ_UDP:
                  g_ptz.preset_bit_return.preset_bit_location_udp_switch 
                    = PRESET_BIT_RETURN_OFF;
                  break;
                  
                case PTZ_UART_422:
                  g_ptz.preset_bit_return.preset_bit_location_uart_422_switch 
                    = PRESET_BIT_RETURN_OFF;
                  break;
                  
                case PTZ_UART_485:
                  g_ptz.preset_bit_return.preset_bit_location_uart_485_switch 
                    = PRESET_BIT_RETURN_OFF;
                  break;
              }
              break;
          }
          ptz_reply(dev, CMD_RIGHT); 
          break;
          
      case HYT_PACK_CMD_0_TYPE_INQUIRE_PRESET_SCAN_DATA: //0xea预置位参数查询
        if(ptz_cmd_execute_judge().preset_bit_set == CMD_NOT_ALLOW )
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        }
        presetbit_data_return(dev,pack->command[1]);
        ptz_reply(dev, CMD_RIGHT);
        break;
         
/****************************  区域扫描控制指令  ******************************/
         
      case HYT_PACK_CMD_0_FUNC_SET_SCAN_HORI_START_BORDER_V:  //0xe6:通过视频设置扫描水平起始边界
        if(ptz_cmd_execute_judge().area_set == CMD_NOT_ALLOW ||
         pack->command[1] >= AREA_AMOUNT)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        }
        g_area[pack->command[1]].hori_start_angle = g_ptz.hori_angle_actual;
        g_area[pack->command[1]].hsa = 1;
        g_area[pack->command[1]].enable = 
          ptz_area_auto_enable(g_area[pack->command[1]]);
        g_area[pack->command[1]].crc = ptz_area_crc(g_area[pack->command[1]]);
        ptz_reply(dev, CMD_RIGHT);
        break;
        
      case HYT_PACK_CMD_0_FUNC_SET_SCAN_HORI_END_BOTDER_V:  //0xe7:通过视频设置扫描水平结束边界
        if(ptz_cmd_execute_judge().area_set == CMD_NOT_ALLOW ||
         pack->command[1] >= AREA_AMOUNT)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        }
        g_area[pack->command[1]].hori_end_angle = g_ptz.hori_angle_actual;
        g_area[pack->command[1]].hea = 1;
        g_area[pack->command[1]].enable = 
          ptz_area_auto_enable(g_area[pack->command[1]]);
        g_area[pack->command[1]].crc = ptz_area_crc(g_area[pack->command[1]]);
        ptz_reply(dev, CMD_RIGHT);
        break;
        
      case HYT_PACK_CMD_0_FUNC_SET_SCAN_VERT_START_BORDER_V:  //0xe8:通过视频设置扫描垂直起始边界
        if(ptz_cmd_execute_judge().area_set == CMD_NOT_ALLOW ||
         pack->command[1] >= AREA_AMOUNT)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        } 
        g_area[pack->command[1]].vert_start_angle = g_ptz.vert_angle_actual;
        g_area[pack->command[1]].vsa = 1;
        g_area[pack->command[1]].enable = 
          ptz_area_auto_enable(g_area[pack->command[1]]);
        g_area[pack->command[1]].crc = ptz_area_crc(g_area[pack->command[1]]);
        ptz_reply(dev, CMD_RIGHT);
        break;
        
      case HYT_PACK_CMD_0_FUNC_SET_SCAN_VERT_END_BORDER_V:  //0xe9:通过视频设置扫描垂直结束边界
        if(ptz_cmd_execute_judge().area_set == CMD_NOT_ALLOW ||
         pack->command[1] >= AREA_AMOUNT)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        }
        g_area[pack->command[1]].vert_end_angle = g_ptz.vert_angle_actual;
        g_area[pack->command[1]].vea = 1;
        g_area[pack->command[1]].enable = 
          ptz_area_auto_enable(g_area[pack->command[1]]);
        g_area[pack->command[1]].crc = ptz_area_crc(g_area[pack->command[1]]);
        ptz_reply(dev, CMD_RIGHT);
        break;
        
      case HYT_PACK_CMD_0_FUNC_SET_SCAN_HORI_VERT_STOP_TIME:  //0xf6:设置区域扫描水平垂直停止时间
        if(ptz_cmd_execute_judge().area_set == CMD_NOT_ALLOW ||
           pack->command[1] >= AREA_AMOUNT)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        }
        g_area[pack->command[1]].step_stop_time = 
          (pack->data[0] << 8) | pack->data[1];
        g_area[pack->command[1]].crc = ptz_area_crc(g_area[pack->command[1]]);
        ptz_reply(dev, CMD_RIGHT);
        break;
        
      case HYT_PACK_CMD_0_FUNC_SET_SCAN_HORI_START_BORDER_A:  //0xf7:通过角度设置区域扫描水平起始边界
        if(ptz_cmd_execute_judge().area_set == CMD_NOT_ALLOW ||
           pack->command[1] >= AREA_AMOUNT)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        }
        g_area[pack->command[1]].hori_start_angle = 
          ((unsigned short int)((pack->data[0] << 8) | pack->data[1])) / 100.0;
        g_area[pack->command[1]].hsa = 1;
        g_area[pack->command[1]].enable = 
          ptz_area_auto_enable(g_area[pack->command[1]]);
        g_area[pack->command[1]].crc = ptz_area_crc(g_area[pack->command[1]]);
        ptz_reply(dev, CMD_RIGHT);
        break;
        
      case HYT_PACK_CMD_0_FUNC_SET_SCAN_HORI_END_BOTDER_A: //0xf8:通过角度设置区域扫描水平结束边界
        if(ptz_cmd_execute_judge().area_set == CMD_NOT_ALLOW ||
           pack->command[1] >= AREA_AMOUNT)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        } 
        g_area[pack->command[1]].hori_end_angle = 
          ((unsigned short int)((pack->data[0] << 8) | pack->data[1])) / 100.0;
        g_area[pack->command[1]].hea = 1;
        g_area[pack->command[1]].enable = 
          ptz_area_auto_enable(g_area[pack->command[1]]);
        g_area[pack->command[1]].crc = ptz_area_crc(g_area[pack->command[1]]);
        ptz_reply(dev, CMD_RIGHT);
        break;
        
      case HYT_PACK_CMD_0_FUNC_SET_SCAN_VERT_START_BORDER_A:  //0xf9通过角度设置区域扫描垂直起始边界
        if(ptz_cmd_execute_judge().area_set == CMD_NOT_ALLOW ||
           pack->command[1] >= AREA_AMOUNT)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        } 
        g_area[pack->command[1]].vert_start_angle = 
          ((short int)((pack->data[0] << 8) | pack->data[1])) / 100.0;
        //防止角度超出界限
        if(g_area[pack->command[1]].vert_start_angle > g_ptz.vert_angleP.angle_allow_max)
        {
          g_area[pack->command[1]].vert_start_angle = g_ptz.vert_angleP.angle_allow_max;
        }
        if(g_area[pack->command[1]].vert_start_angle < g_ptz.vert_angleP.angle_allow_min)
        {
          g_area[pack->command[1]].vert_start_angle = g_ptz.vert_angleP.angle_allow_min;
        }
        
        g_area[pack->command[1]].vsa = 1;
        g_area[pack->command[1]].enable = 
          ptz_area_auto_enable(g_area[pack->command[1]]);
        g_area[pack->command[1]].crc = ptz_area_crc(g_area[pack->command[1]]);
        ptz_reply(dev, CMD_RIGHT);
        break;
        
      case HYT_PACK_CMD_0_FUNC_SET_SCAN_VERT_END_BORDER_A:  //0xfa:通过角度设置区域扫描垂直结束边界
        if(ptz_cmd_execute_judge().area_set == CMD_NOT_ALLOW ||
           pack->command[1] >= AREA_AMOUNT)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        } 
        g_area[pack->command[1]].vert_end_angle = 
          ((short int)((pack->data[0] << 8) | pack->data[1])) / 100.0;
        //防止角度超出界限
        if(g_area[pack->command[1]].vert_end_angle > g_ptz.vert_angleP.angle_allow_max)
        {
          g_area[pack->command[1]].vert_end_angle = g_ptz.vert_angleP.angle_allow_max;
        }
        if(g_area[pack->command[1]].vert_end_angle < g_ptz.vert_angleP.angle_allow_min)
        {
          g_area[pack->command[1]].vert_end_angle = g_ptz.vert_angleP.angle_allow_min;
        }
        
        g_area[pack->command[1]].vea = 1;
        g_area[pack->command[1]].enable = 
          ptz_area_auto_enable(g_area[pack->command[1]]);
        g_area[pack->command[1]].crc = ptz_area_crc(g_area[pack->command[1]]);
        ptz_reply(dev, CMD_RIGHT);
        break;
        
      case HYT_PACK_CMD_0_FUNC_SET_SCAN_HORI_GAP_ANGLE:  //0xfb通过发送角度设置水平扫描间隔角度
        if(ptz_cmd_execute_judge().area_set == CMD_NOT_ALLOW ||
           pack->command[1] >= AREA_AMOUNT)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        } 
        g_area[pack->command[1]].hori_step_angle = 
          ((unsigned short int)((pack->data[0] << 8) | pack->data[1])) / 100.0;
        g_area[pack->command[1]].h_step = 1;
        g_area[pack->command[1]].enable = 
          ptz_area_auto_enable(g_area[pack->command[1]]);
        g_area[pack->command[1]].crc = ptz_area_crc(g_area[pack->command[1]]);
        ptz_reply(dev, CMD_RIGHT);
        break;
        
      case HYT_PACK_CMD_0_FUNC_SET_SCAN_VERT_GAP_ANGLE: //0xfc:通过发送角度设置垂直扫描间隔角度
        if(ptz_cmd_execute_judge().area_set == CMD_NOT_ALLOW ||
           pack->command[1] >= AREA_AMOUNT)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        } 
        g_area[pack->command[1]].vert_step_angle = 
          ((unsigned short int)((pack->data[0] << 8) | pack->data[1])) / 100.0;
        g_area[pack->command[1]].v_step = 1;
        g_area[pack->command[1]].enable = 
          ptz_area_auto_enable(g_area[pack->command[1]]);
        g_area[pack->command[1]].crc = ptz_area_crc(g_area[pack->command[1]]);
        ptz_reply(dev, CMD_RIGHT);
        break;
        
      case HYT_PACK_CMD_0_FUNC_SET_SCAN_HORI_VERT_SPEED: //0xfd:设置区域扫描水平垂直扫描速度
        if(ptz_cmd_execute_judge().area_set == CMD_NOT_ALLOW ||
           pack->command[1] >= AREA_AMOUNT)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        } 
        g_area[pack->command[1]].hori_scan_speed = pack->data[0] / 10.0;
        g_area[pack->command[1]].vert_scan_speed = pack->data[1] / 10.0;
        g_area[pack->command[1]].crc = ptz_area_crc(g_area[pack->command[1]]);
        ptz_reply(dev, CMD_RIGHT);
        break;
        
      case HYT_PACK_CMD_0_FUNC_SAVE_AREA_SCAN_DATA:  //0xf3:保存区域扫描参数
        ptz_area_save();
        ptz_reply(dev, CMD_RIGHT);
        break;
        
      case HYT_PACK_CMD_0_TYPE_SET_SCAN_STATE:  //0xf4:区域扫描状态，使能或不使能某个扫描区域
        if(ptz_cmd_execute_judge().area_set == CMD_NOT_ALLOW ||
           pack->command[1] >= AREA_AMOUNT)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        }
        if(pack->data[1] == 0xff)//删除所有扫描区域
        {
          memset(g_area, 0, sizeof(g_area));
        }
        else//使能和不使能某个扫描区域
        {
          if(pack->data[0] == AREA_ENABLE)
          {
            g_area[pack->command[1]].enable = AREA_ENABLE;
          }
          else
          {
            g_area[pack->command[1]].enable = AREA_DISABLE;
          }
        }
        ptz_reply(dev, CMD_RIGHT);
        break;
        
      case HYT_PACK_CMD_0_TYPE_SET_AREA_SCAN_FUNC: //0xf5:设置区域扫描扫描模式
        switch(pack->command[1])
        {
          case 1://启动单个区域扫描
          case 0:
            if(ptz_cmd_execute_judge().area_scan_start == CMD_NOT_ALLOW ||
               pack->data[0] >= AREA_AMOUNT)
            {
              ptz_reply(dev, CMD_FAIL);
              BSP_OS_SemPost(&pack_process_mutex);
              return;
            }
            ptz_return_clear();
            g_area_scan.start_num = pack->data[0];
            g_area_scan.sin_mul_mode = AREA_SIN_SCAN;
            g_area_scan.state = AREA_SCAN_START;
            ptz_reply(dev, CMD_RIGHT);
            break;
            
          case 2://启动多个区域扫描
            if(ptz_cmd_execute_judge().area_scan_start == CMD_NOT_ALLOW ||
               pack->data[0] >= AREA_AMOUNT ||
               pack->data[1] >= AREA_AMOUNT)
            {
              ptz_reply(dev, CMD_FAIL);
              BSP_OS_SemPost(&pack_process_mutex);
              return;
            }
            ptz_return_clear();
            g_area_scan.start_num = pack->data[0];
            g_area_scan.end_num   = pack->data[1];
            g_area_scan.sin_mul_mode = AREA_MUL_SCAN;
            g_area_scan.state = AREA_SCAN_START;
            ptz_reply(dev, CMD_RIGHT);
            break;
                      
          case 3://暂停扫描
            if(ptz_cmd_execute_judge().area_scan_pause == CMD_NOT_ALLOW ||
               pack->data[0] >= AREA_AMOUNT ||
               pack->data[1] >= AREA_AMOUNT)
            {
              ptz_reply(dev, CMD_FAIL);
              BSP_OS_SemPost(&pack_process_mutex);
              return;
            }
            g_area_scan.state = AREA_SCAN_PAUSE_A;
            ptz_reply(dev, CMD_RIGHT);
            break;
            
          case 4://恢复扫描
            if(ptz_cmd_execute_judge().area_scan_recovery == CMD_NOT_ALLOW ||
               pack->data[0] >= AREA_AMOUNT ||
               pack->data[1] >= AREA_AMOUNT)
            {
              ptz_reply(dev, CMD_FAIL);
              BSP_OS_SemPost(&pack_process_mutex);
              return;
            }
            ptz_return_clear();
            g_area_scan.state = AREA_SCAN_RECOVERY;
            ptz_reply(dev, CMD_RIGHT);
            break;
          
          case 5://彻底关闭扫描
            if(ptz_cmd_execute_judge().area_scan_close == CMD_NOT_ALLOW ||
               pack->data[0] >= AREA_AMOUNT)
            {
              ptz_reply(dev, CMD_FAIL);
              BSP_OS_SemPost(&pack_process_mutex);
              return;
            }
            g_area_scan.state = AREA_SCAN_CLOSE_A;
            ptz_reply(dev, CMD_RIGHT);
            break;
            
          case 6://设置某个区域的扫描模式
            if(ptz_cmd_execute_judge().area_set == CMD_NOT_ALLOW ||
               pack->data[0] >= AREA_AMOUNT)
            {
              ptz_reply(dev, CMD_FAIL);
              BSP_OS_SemPost(&pack_process_mutex);
              return;
            }
            if(pack->data[1] == AREA_SCAN_MODE_STEP)
            {
              g_area[pack->data[0]].scan_mode = AREA_SCAN_MODE_STEP;
            }
            else// AREA_SCAN_MODE_CONT
            {
              g_area[pack->data[0]].scan_mode = AREA_SCAN_MODE_CONT;
            }
            g_area[pack->data[0]].crc = ptz_area_crc(g_area[pack->data[0]]);
            ptz_reply(dev, CMD_RIGHT);
            break;
        }
        break;
        
      case HYT_PACK_CMD_0_TYPE_AREA_SCAN_RETURN_SWITCH:  //0xc4:设置区域扫描回传开关
        switch(pack->command[1])
          {
            case AREA_SCAN_END_RETURN_ON://打开
              switch(dev)
              {
                case PTZ_UDP:
                  ptz_area_scan_end_return_save(dev);
                  g_ptz.area_scan_return.area_scan_end_udp_switch 
                    = AREA_SCAN_RETURN_ON;
                  break;
                  
                case PTZ_UART_422:
                  g_ptz.area_scan_return.area_scan_end_uart_422_switch 
                    = AREA_SCAN_RETURN_ON;
                  break;
                  
                case PTZ_UART_485:
                  g_ptz.area_scan_return.area_scan_end_uart_485_switch 
                    = AREA_SCAN_RETURN_ON;
                  break;
              }
              break;
              
            case AREA_SCAN_END_RETURN_OFF://关闭
              switch(dev)
              {
                case PTZ_UDP:
                  g_ptz.area_scan_return.area_scan_end_udp_switch 
                    = AREA_SCAN_RETURN_OFF; 
                  break;
                  
                case PTZ_UART_422:
                  g_ptz.area_scan_return.area_scan_end_uart_422_switch 
                    = AREA_SCAN_RETURN_OFF;
                  break;
                  
                case PTZ_UART_485:
                  g_ptz.area_scan_return.area_scan_end_uart_485_switch 
                    = AREA_SCAN_RETURN_OFF;
                  break;
              }
              break;
              
            case AREA_STEP_SCAN_LOCATION_RETURN_ON://打开
              switch(dev)
              {
                case PTZ_UDP:
                  ptz_area_step_scan_location_return_save(dev);
                  g_ptz.area_scan_return.area_step_scan_location_udp_switch 
                    = AREA_SCAN_RETURN_ON;
                  break;
                  
                case PTZ_UART_422:
                  g_ptz.area_scan_return.area_step_scan_location_uart_422_switch 
                    = AREA_SCAN_RETURN_ON;
                  break;
                  
                case PTZ_UART_485:
                  g_ptz.area_scan_return.area_step_scan_location_uart_485_switch 
                    = AREA_SCAN_RETURN_ON;
                  break;
              }
              break;  
              
            case AREA_STEP_SCAN_LOCATION_RETURN_OFF://关闭
              switch(dev)
              {
                case PTZ_UDP:
                  g_ptz.area_scan_return.area_step_scan_location_udp_switch 
                    = AREA_SCAN_RETURN_OFF;
                  break;
                  
                case PTZ_UART_422:
                  g_ptz.area_scan_return.area_step_scan_location_uart_422_switch 
                    = AREA_SCAN_RETURN_OFF;
                  break;
                  
                case PTZ_UART_485:
                  g_ptz.area_scan_return.area_step_scan_location_uart_485_switch 
                    = AREA_SCAN_RETURN_OFF;
                  break;
              }
              break;
          }
          ptz_reply(dev, CMD_RIGHT);
          break;

      case HYT_PACK_CMD_0_TYPE_INQUIRE_AREA_SCAN_DATA:  //0xca:查询区域扫描参数
        if(ptz_cmd_execute_judge().area_set == CMD_NOT_ALLOW ||
           pack->command[1] >= AREA_AMOUNT)
        {
          ptz_reply(dev, CMD_FAIL);
          BSP_OS_SemPost(&pack_process_mutex);
          return;
        } 
        ptz_send_area_data(dev, pack->command[1]);
        ptz_reply(dev, CMD_RIGHT);
        break;
        
/***********************云台状态参数查询及实时回传开关 ************************/  
        
      case HYT_PACK_CMD_0_TYPE_INQUIRE_CURRENT:  //0xc8:电流查询与回复
        ptz_send_current(dev);
        break;
        
      case HYT_PACK_CMD_0_TYPE_INQUIRE_VOLTAGE:  //0xcd:电压查询与回复
        ptz_send_voltage(dev);
        break;
        
      case HYT_PACK_CMD_0_FUNC_INQUIRE_TEMP:  //0xd6:温度查询与回复
        ptz_send_temperature(dev);
        break;
        
        
      case HYT_PACK_CMD_0_BRIDGE_TEMP:  //0xd7:水平垂直侧驱动电路温度查询 
        ptz_send_bridge_temperature(dev);
        break;
        
//      case HYT_PACK_CMD_0_VERT_BRIDGE_TEMP:  //0xd8:垂直侧驱动电路温度查询  
//        ptz_send_v_bridge_temperature(dev);
//        break;
       
        
      case HYT_PACK_CMD_0_TYPE_INQUIRE_SPEED:  //0xd0:速度查询与回复
        ptz_send_speed(dev, pack->command[1]);
        break;
        
      case HYT_PACK_CMD_0_TYPE_INQUIRE_WORK_MODEL: //0xe0:查询工作模式
        ptz_send_work_mode(dev);
        break;
        
      case HYT_PACK_CMD_0_TYPE_INQUIRE_WORK_STATE: //0xdd:查询工作状态
        ptz_send_fault_state(dev, ALL_FAULT_STATE);
        break; 

      case HYT_PACK_CMD_0_FUNC_INQUIRE_ANGLE_STATE:  //0x9d:查询角度状态
        ptz_send_sw_angle_state(dev);
        break;
        
      case HYT_PACK_CMD_0_FUNC_AUTO_RETURN_SPEED_SWITCH:  //0xdc:转速自动回传开关
      case HYT_PACK_CMD_0_FUNC_AUTO_RETURN_ANGLE_SWITCH:  //0xe1:云台角度自动回传开关
      case HYT_PACK_CMD_0_FUNC_AUTO_RETURN_VERF_SWITCH:   //0xbf:云台电机驱动芯片模拟电压输入等级回传
      case HYT_PACK_CMD_0_FUNC_AUTO_RETURN_CURRENT_SWITCH://0xc1:云台工作电流实时回传开关
      case HYT_PACK_CMD_0_FUNC_AUTO_RETURN_VOLTAGE_SWITCH://0xcc:云台工作电压实时回传开关
      case HYT_PACK_CMD_0_FUNC_AUTO_RETURN_TEMP_SWITCH:   //0xd4:云台工作温度实时回传开关
      case HYT_PACK_CMD_0_FUNC_AUTO_RETURN_CPU_SWITCH:    //0xb0:云台CPU占有率实时回传开关
        ptz_auto_return_off_on(dev, pack);
        if(dev == PTZ_UART_485)
        {
          ptz_reply(dev, CMD_FAIL);
        }
        else
        {
          ptz_reply(dev, CMD_RIGHT);
        }
        break;
        
/********************************设置云台基准0位 *****************************/         

      case HYT_PACK_CMD_0_TYPE_SET_ZERO_POSITION:  //0xe3零位设置
        if(ptz_cmd_execute_judge().set_zero == CMD_NOT_ALLOW)
        {
           ptz_reply(dev, CMD_FAIL);
           BSP_OS_SemPost(&pack_process_mutex);
           return;
        }
        ptz_zero_offset_angle_set(dev, pack);
        break;
        
      case HYT_PACK_CMD_0_TYPE_INQUIRE_ZERO_POSITION: //0x3e查询基准零位
        ptz_zero_offset_angle_return(dev);
        break;
        
/********************************云台重启&自检指令 *****************************/  

      case HYT_PACK_CMD_0_FUNC_REBOOT:  //0xde云台重启
        ptz_hori_stop(0);
        ptz_vert_stop(0);
        ptz_reply(dev, CMD_RIGHT);  
#ifdef PTZ_NO_SELF_CHECK 
        g_ptz.no_self_check_force_save = 1;
        OSTimeDlyHMSM(0u, 0u, 2u, 0u);
#endif 
        GD32_RESET();
        break;
        
      case HYT_PACK_CMD_0_FUNC_COMPLETE_CHECK_AGAIN:  //0xce清除自检数据重新进行全范围自检
        ptz_hori_stop(0);
        ptz_vert_stop(0);
        ptz_self_check_data_erase();  
#ifdef PTZ_NO_SELF_CHECK 
        //强制擦除保存的掉电位置数据
        g_ptz.no_self_check_force_erase = 1;
        OSTimeDlyHMSM(0u, 0u, 2u, 0u);
#endif        
        ptz_reply(dev, CMD_RIGHT);
        GD32_RESET();
        break;
        
      case HYT_PACK_CMD_0_FUNC_SIMPLY_CHECK_AGAIN: //0xbb清除掉电数据重新进行简易自检
#ifdef PTZ_NO_SELF_CHECK
        ptz_reply(dev, CMD_RIGHT); 
        switch(pack->command[1])
        {
        case 0x01:
          ptz_hori_stop(0);
          ptz_vert_stop(0);
          g_ptz.no_self_check_force_erase = 1;
          OSTimeDlyHMSM(0u, 0u, 2u, 0u);
          GD32_RESET();
          
//          mb85rc64_page_write(PTZ_MB85RC64_ADD_A, (unsigned char *)&test_data, sizeof(test_data));
          break;
          
        case 0x02:
          //强制保存当前位置数据
          g_ptz.no_self_check_force_save = 1;
//          mb85rc64_add_read(PTZ_MB85RC64_ADD_A, (unsigned char *)&test_bata, sizeof(test_bata));
          break;
        }
#endif         
        break;
        
/***********************************指令回复开关 ******************************/  
        
      case HYT_PACK_CMD_0_TYPE_SET_CMD_REPLY_SWITCH: //0xdf开启或关闭指令回复开关
        ptz_reply(dev, CMD_RIGHT);
        if(pack->data[0] == CMD_REPLY)
        {
          g_ptz.cmd_reply_switch = CMD_REPLY;
        }
        else
        {
          g_ptz.cmd_reply_switch = CMD_NO_REPLY;
        }
        break;
        
/********************************** 加热电阻开关 ******************************/  
        
      case HYT_PACK_CMD_0_TYPE_HEAT_RESISTANCE_SWITCH: //0xcf加热电阻开关
        ptz_heat_resistor_switch(pack);
        ptz_reply(dev, CMD_RIGHT);
        break;
       
/****************************** 指令保存预处理功能 ****************************/  
        
      case HYT_PACK_CMD_0_FUNC_CMD_SAVE_BEFORE: //0xba指令保存预处理功能
#ifdef PTZ_CMD_SAVE_BEFORE
        if(ptz_cmd_before_mode(dev, pack) == 1)
        {
          ptz_reply(dev, CMD_RIGHT); 
        }
        else
        {
          ptz_reply(dev, CMD_FAIL); 
        }
#endif
        break;
        
/********************************* 继电器开关 ********************************/          
        
      case HYT_PACK_CMD_0_TYPE_MOTOR_RELAY_SWITCH: //0xc0继电器开关
        switch(pack->command[1])
        {
           case PTZ_RELAY_ON://打开
              ptz_motor_relay_on();
              break;
              
            default://关闭
              ptz_motor_relay_off();
              break;
        }
        ptz_reply(dev, CMD_RIGHT);
        break;
        
      
/********************************** ***其他*** ********************************/      
/***************************步进电机专用0x50-0x5f******************************/
#ifdef PTZ_STEP_MOTOR

#ifdef TMC2160
        case 0x50://查询步进电机TMC2160的状态
          tmc2160_state_return( dev);
          break;
          
        case 0x51:
        case 0x52:
          tmc2160_parameter_process(dev, pack);
          break;
          
        case 0x53://数据处理
          tmc2160_parameter_process(dev, pack);
          ptz_reply(dev, CMD_RIGHT); 
          break;
          
        case 0x54://水平驱动器电源开关
          if(pack->command[1] == 0)
          {
            TMC2160_H_POWER_OFF;//驱动器电源关
          }
          if(pack->command[1] == 1)
          {
            TMC2160_H_POWER_ON;//驱动器电源开
          }
          ptz_reply(dev, CMD_RIGHT); 
          break;
          
        case 0x55://垂直驱动器电源开关
          if(pack->command[1] == 0)
          {
            TMC2160_V_POWER_OFF;//驱动器电源关
          }
          if(pack->command[1] == 1)
          {
            TMC2160_V_POWER_ON;//驱动器电源开
          }
          ptz_reply(dev, CMD_RIGHT); 
          break;
#endif
          

          
#endif
          
          
#ifdef PTZ_BLDC_MOTOR
          case 0x54://水平驱动器电源开关
          if(pack->command[1] == 0)
          { 
            ptz_hori_stop(PTZ_HORI_STOP_TIME);
            OSTimeDlyHMSM(0u, 0u, 0u, 2u);
            H_BRIDGE_POWER_OFF;//驱动器电源关
          }
          if(pack->command[1] == 1)
          {
            //驱动器电源开
            H_BRIDGE_POWER_ON;
          }
          ptz_reply(dev, CMD_RIGHT); 
          break;
          
        case 0x55://垂直驱动器电源开关
          if(pack->command[1] == 0)
          {
            ptz_vert_stop(PTZ_VERT_STOP_TIME);
            OSTimeDlyHMSM(0u, 0u, 0u, 2u);
            V_BRIDGE_POWER_OFF;//驱动器电源关
          }
          if(pack->command[1] == 1)
          {
            //驱动器电源开
            V_BRIDGE_POWER_ON;
          }
          ptz_reply(dev, CMD_RIGHT); 
          break;
#endif
/******************************************************************************/

      }
    }
    
    BSP_OS_SemPost(&pack_process_mutex);
}


/*

*************************** 不定长协议 ******************************************

*/

static char send_buff[1500];
static char net_rece_buff[1500] = {0x00};
#define PACK_A_R_MAX_LEN     80//接收数据的最大长度
#define PACK_A_S_MAX_LEN     500//发送数据的最大长度

//不定长协议包头校验码计算
unsigned int ptz_data_pack_a_head_crc(PTZ_DATA_PACK_A *pack)
{
  unsigned short int len = 0,len_count = 0;
  unsigned int crc = 0;
  len = sizeof(PTZ_DATA_PACK_A) - sizeof(unsigned int)- sizeof(unsigned char);
  for(len_count = 0; len_count < len; len_count++)
  {
    crc = crc + *(((unsigned char *)pack) + len_count);
  }
  return crc;
}

//云台不定长协议总效验码计算
unsigned int ptz_data_pack_a_crc(PTZ_DATA_PACK_A *pack)
{
  unsigned short int len = 0,len_count = 0;
  unsigned int crc = 0;
  
  len = sizeof(PTZ_DATA_PACK_A) - 1 + pack->cmd_len;
  for(len_count = 0; len_count < len; len_count++)
  {
    crc = crc + *(((unsigned char *)pack) + len_count);
  }
  return crc;
}

//云台不定长协议检查
char ptz_data_pack_a_check(PTZ_DATA_PACK_A *pack)
{
  unsigned int crc,crc_read;
  unsigned short int len = 0,len1 = 0;
  //判断头效验码
  if(pack->head_crc != ptz_data_pack_a_head_crc(pack))
  {
    return 0;
  }
  //判断协议头
  if(pack->head != PACK_A_HEAD)
  {
    return 0;
  }
  //判断地址
  if(pack->addr != g_ptz.address)
  {
    return 0;
  }
  
  //判断长度是否正确,长度不能超过接收缓冲区长度
  len1 = sizeof(PTZ_DATA_PACK_A) - 1 + pack->cmd_len + sizeof(unsigned int) + sizeof(unsigned char);
  if(len1 > PACK_A_R_MAX_LEN)
  {
    return 0;
  }
  
  //判断总效验码
  len = sizeof(PTZ_DATA_PACK_A) - 1 + pack->cmd_len;
  crc = ptz_data_pack_a_crc(pack);
  crc_read = *((unsigned int *)(((unsigned char *)pack) + len));
  if(crc != crc_read)
  {
    return 0;
  }
  //判断结束符
  len = len + sizeof(int);
  if(*((unsigned char *)pack + len) != PACK_A_TAIL)
  {
    return 0;
  }
  return 1;
}

//云台数据封包,返回封包后数据整体长度，并将数据包存放到send_buff中
unsigned short int ptz_data_pack_a_pack(char *data, unsigned short int len,
                                      unsigned char type, unsigned char func)
{
  unsigned short int len1 = 0,len2 = 0;
  PTZ_DATA_PACK_A *p = (PTZ_DATA_PACK_A *)send_buff;
  
  memset(send_buff, 0, sizeof(send_buff));
  p->head = PACK_A_HEAD;
  p->addr = g_ptz.address;
  p->cmd_type = type;
  p->cmd_func = func;
  p->cmd_len = len;
  p->head_crc = ptz_data_pack_a_head_crc(p);
  
  //判断长度是否正确,长度不能超过发送缓冲区长度
  len2 = sizeof(PTZ_DATA_PACK_A) - 2 + p->cmd_len + sizeof(unsigned int) + sizeof(unsigned char);
  if(len2 > PACK_A_S_MAX_LEN)
  {
    p->cmd_len = 0;
    return 0;
  }
  
  if(len > 0)
  {
    memcpy(p->cmd_data, data, len);
  }
  len1 =  sizeof(PTZ_DATA_PACK_A) - 1 + len;
  *((unsigned int *)(((unsigned char *)p) + len1)) = 
    ptz_data_pack_a_crc(p);
  len1 = len1 + sizeof(int);
  *((unsigned char *)p + len1) = PACK_A_TAIL;
  return (len1 + sizeof(char));
}


//云台不定长数据指令回复
char ptz_data_pack_a_reply(char dev, unsigned char reply)
{
  PTZ_DATA_PACK_A *rece = (PTZ_DATA_PACK_A *)net_rece_buff;
  unsigned int len = 0;
  //#define CMDA_REPLY           0XFF
  switch(reply)
  {
    case CMDA_RIGHT:           //0XEE指令正确
      len = ptz_data_pack_a_pack((char *)(rece->cmd_data), rece->cmd_len, 
                                     CMDA_REPLY, CMDA_RIGHT);
      ptz_send_data(dev, (unsigned char*)send_buff, len);
      break;
      
    case CMDA_WRONG:           //0XFF指令错误
      len = ptz_data_pack_a_pack((char *)(rece->cmd_data), 0/*rece->cmd_len*/, 
                                     CMDA_REPLY, CMDA_WRONG);
      ptz_send_data(dev, (unsigned char*)send_buff, len);
      break;
      
    case CMDA_FAIL:            //0XFE指令正确但执行失败
      len = ptz_data_pack_a_pack((char *)(rece->cmd_data), rece->cmd_len, 
                                     CMDA_REPLY, CMDA_FAIL);
      ptz_send_data(dev, (unsigned char*)send_buff, len);
      break;
      
    default :
      len = ptz_data_pack_a_pack((char *)(rece->cmd_data), 0/*rece->cmd_len*/, 
                                     CMDA_REPLY, CMDA_WRONG);
      ptz_send_data(dev, (unsigned char*)send_buff, len);
      break;
  }
  return 1;
}
//派尔高D协议数据包
char ptz_data_pack_pelco_d_check(PTZ_DATA_PACK *pack)
{
  //首先判断效验码
  if(MotorCalPelcoDSUM((u_int8_t*)pack,sizeof(PTZ_DATA_PACK)) != pack->checksum)
  {
    return 0;
  }
  //其次判断数据头
  if(pack ->head != 0xFF)
  {
    return 0;
  }
  //其次判断云台地址
  if(pack->addr != g_ptz.address)
  {
    return 0;
  }
  return 1;
}



static char process_buff[500];
//云台不定长协议处理

void ptz_data_pack_a_process(char dev, PTZ_DATA_PACK_A *pack)
{
  unsigned short int len = 0;
  unsigned short int len1 = 0;
  char cfg_str[100] = {0};
  
  BSP_OS_SemWait(&pack_process_mutex, 0u);

  //判断协议是否正确
  if(ptz_data_pack_a_check(pack) != 1)
  {//回复指令错误
    ptz_data_pack_a_reply(dev, CMDA_WRONG);
    BSP_OS_SemPost(&pack_process_mutex);
    return;
  }
  

  switch(pack->cmd_type)
  {
    case EDIT_CFG_FILE://编辑配置文件
      memset(cfg_str, 0, sizeof(cfg_str));
      memcpy(cfg_str, pack->cmd_data, pack->cmd_len);
      cfg_str[pack->cmd_len] = '\0';
      if(ptz_set_cfg(cfg_str,pack->cmd_func) == TRUE)
      {
        ptz_data_pack_a_reply(dev, CMDA_RIGHT);
      }
      else
      {
        ptz_data_pack_a_reply(dev, CMDA_FAIL);
      }
      break;
      
    case UPLOAD_CFG_DATA://上传配置文件数据 
      if(pack->cmd_func == UPLOAD_CFG_DATA_REQUEST)
      {
        ptz_data_pack_a_reply(dev, CMDA_RIGHT);
        memset(process_buff, 0, sizeof(process_buff));
        len = 0;
        len = ptz_read_cfg_data(process_buff);
        if(len > 0)
        {
          len1 = 0;
          len1 = ptz_data_pack_a_pack(process_buff, len, 
                 UPLOAD_CFG_DATA, UPLOAD_CFG_DATA_RIGHT);
          ptz_send_data(dev, (unsigned char*)send_buff, len1);
        }
        else
        {
          len1 = 0;
          len1 = ptz_data_pack_a_pack(process_buff, len, 
                                          UPLOAD_CFG_DATA, UPLOAD_CFG_DATA_WRONG);
          ptz_send_data(dev, (unsigned char*)send_buff, len1);
        }
      }
      else if(pack->cmd_func == UPLOAD_CFG_DATA_REQUEST_BROADCAST)
      {
        memset(process_buff, 0, sizeof(process_buff));
        len = 0;
        len = ptz_read_cfg_data(process_buff);
        if(len > 0)
        {
          len1 = 0;
          len1 = ptz_data_pack_a_pack(process_buff, len, 
                                          UPLOAD_CFG_DATA, UPLOAD_CFG_DATA_RIGHT);
          
          if(dev == PTZ_UDP)
          {
            send_udp_data_broadcast((unsigned char*)send_buff,len1);
          }
          else
          {
             ptz_send_data(dev, (unsigned char*)send_buff, len1);
          }
        }
        else
        {
          len1 = 0;
          len1 = ptz_data_pack_a_pack(process_buff, len, 
                                          UPLOAD_CFG_DATA, UPLOAD_CFG_DATA_WRONG);
          
          if(dev == PTZ_UDP)
          {
            send_udp_data_broadcast((unsigned char*)send_buff,len1);
          }
          else
          {
            ptz_send_data(dev, (unsigned char*)send_buff, len1);
          }
          
        }
      }
      else
      {
        ptz_data_pack_a_reply(dev, CMDA_FAIL);
      }
      break;
      
    case CMDA_REPLY://指令回复
      
      break;
      
    case UPLOAD_VERSION: //程序版本号回复
      ptz_data_pack_a_reply(dev, CMDA_RIGHT);
      len1 = 0;
      len1 = ptz_data_pack_a_pack((char *)g_program_version_number, 
                                  sizeof(g_program_version_number), 
                                  UPLOAD_VERSION, UPLOAD_VERSION_RIGHT);
      ptz_send_data(dev, (unsigned char*)send_buff, len1);
      OSTimeDlyHMSM(0u, 0u, 0u, 10u);
      len1 = 0;
      len1 = ptz_data_pack_a_pack((char *)g_ptz_type, 
                                  sizeof(g_ptz_type), 
                                  UPLOAD_VERSION, UPLOAD_VERSION_RIGHT);
      ptz_send_data(dev, (unsigned char*)send_buff, len1);
      break;
    
    default ://其他
      
      break;
  }
  
  BSP_OS_SemPost(&pack_process_mutex);
}





/****************************************************************************/
/***********************监听云台消息：以太网、485、422***********************/
/****************************************************************************/

#define TEST_PORT 6666
///以太网通信——UDP
static void task_ptz_udp_server()
{
	while(1)
	{
	  if(ptz_communicaton != PTZ_CONNECT_STATE_SUCCESS)
	  {
		ptz_sock_fd = CreateUDPClientSock();
		if (ptz_sock_fd != -1) {
			// 设置服务器端ip和端口号
			memset(&ptz_from, 0, sizeof(struct sockaddr_in));
			ptz_local.sin_family = PF_INET;
			// 监听端口
			ptz_local.sin_port = htons(TEST_PORT);
			ptz_local.sin_addr.s_addr = INADDR_ANY;
			ptz_fromlen = sizeof(ptz_from);
			if(0!=bind(ptz_sock_fd,(struct sockaddr*)&ptz_local,sizeof(ptz_local))){
				close(ptz_sock_fd);
			}else{
				ptz_communicaton = PTZ_CONNECT_STATE_SUCCESS;

			}
		}
		else{
		    ptz_communicaton = PTZ_CONNECT_STATE_FAILED;
			close(ptz_sock_fd);
		}
	  }
	  if (ptz_communicaton == PTZ_CONNECT_STATE_SUCCESS) {
		    memset(net_rece_buff, 0x00, sizeof(net_rece_buff));//'\0'
			int ret_len = recvfrom(ptz_sock_fd, net_rece_buff, sizeof(net_rece_buff), 0, (struct sockaddr*)&ptz_from, &ptz_fromlen);
			if (ret_len <= 7 && ret_len > 0)
			{
			  ptz_data_pack_process(PTZ_UDP, (PTZ_DATA_PACK *)net_rece_buff);			  
			}
                        if (ret_len >7)
                        {
                          ptz_data_pack_a_process(PTZ_UDP, (PTZ_DATA_PACK_A *)net_rece_buff);                        
                        }
			if(ret_len <= 0)
                        {
				ptz_communicaton = PTZ_CONNECT_STATE_FAILED;
				close(ptz_sock_fd);
			}
	    }
	OSTimeDlyHMSM(0u, 0u, 0u, 10u);
        }
}

static OS_STK task_listen_ptz_server_stk[TASK_LISTEN_PTZ_SERVER_STK_SIZE];
static void  task_create_listen_ptz_server (void)
{ 

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


///串口1，485通信
static void ptz_uart_485_data_process_task()
{
  static u_int8_t uart_485_buffer[PTZ_UART_485_BUFF_SIZE];
  static unsigned short int uart_485_data_len;
  static unsigned char uart_485_timer;
  unsigned short int i;
  static unsigned char k;
  PTZ_DATA_PACK_A *pack_a = (PTZ_DATA_PACK_A *)uart_485_buffer;
  PTZ_DATA_PACK *pack_pelco_d = (PTZ_DATA_PACK *)uart_485_buffer;
  while(1)
  {
    //485串口
    if(ptz_uart_dev_char_present(uart_485_handle) != 0 &&
      (uart_485_state == PTZ_UART_FREE_STATE))//来了第一个字节的数据
    {
      switch(k)
      {
        case 0://判断每帧数据第一个字节是不是0XFF
          memset(uart_485_buffer, 0, sizeof(uart_485_buffer));
          uart_485_data_len = 0;
          uart_485_timer = 0;
          uart_485_buffer[uart_485_data_len] = ptz_uart_dev_in_char(uart_485_handle);
          if(uart_485_buffer[0] == 0XFF)
          {
            uart_485_data_len++;
            k = 1;
          }
          else
          {
            k = 0;
            uart_485_data_len = 0;
          }
          break;

        case 1://判断每帧数据字节头是不是FF ADD的格式
          uart_485_buffer[uart_485_data_len] = ptz_uart_dev_in_char(uart_485_handle);
          uart_485_data_len++;
          k = 0;
          if(uart_485_buffer[0] == 0XFF &&
             uart_485_buffer[1] == g_ptz.address)
          {
            uart_485_state = PTZ_UART_RECV_STATE;
          }
          else
          {
            if(uart_485_buffer[1] == 0XFF)
            {
              memset(uart_485_buffer, 0, sizeof(uart_485_buffer));
              uart_485_data_len = 1;
              uart_485_buffer[0] = 0XFF;
              k = 1;
            }
          }
          break;
      }
    }
    
    if(uart_485_state == PTZ_UART_RECV_STATE)
    {
      for(i = 0; i < PTZ_UART_485_BUFF_SIZE - 2; i++)
      {
        //判断有无数据
        if(ptz_uart_dev_char_present(uart_485_handle) == 0)
        {
          OSTimeDlyHMSM(0u, 0u, 0u, 1u);//等待一段时间
          uart_485_timer ++;
          if(uart_485_timer > PTZ_UART_RECV_OVERTIME)
          {//接收超时，说明一帧数据接收完成
            break;
          }
        }
        else
        {//读取一个字节数据
          uart_485_buffer[uart_485_data_len] = ptz_uart_dev_in_char(uart_485_handle);
          uart_485_data_len++;
          uart_485_timer = 0;
          if(uart_485_data_len % 100 == 0 && uart_485_data_len > 0)
          {
            OSTimeDlyHMSM(0u, 0u, 0u, 2u);
          }
          if(uart_485_data_len >= 7)
          {
            if(ptz_data_pack_pelco_d_check(pack_pelco_d) == 1)
            {//成功读取一帧派尔高D协议或者自定义类似派尔高D协议，长度7字节
              break;
            }
          }
          if(uart_485_data_len >= (sizeof(PTZ_DATA_PACK_A) + sizeof(unsigned int)))
          {
            ptz_reply(uart_485_handle, CMD_WRONG);
            if(ptz_data_pack_a_check(pack_a) == 1)
            {//成功读取一帧自定义超长度协议
              break;
            }
          }
          if(uart_485_data_len >= PTZ_UART_485_BUFF_SIZE)
          {
            break;
          }
        }
      }
      uart_485_timer = 0;
      uart_485_state = PTZ_UART_HAND_STATE;
    }
    //处理串口数据
    if(uart_485_state == PTZ_UART_HAND_STATE)
    {
       if(uart_485_data_len > 7)
       {//不定长数据处理，该数据每一帧都超过7个字节
         ptz_data_pack_a_process(PTZ_UART_485, (PTZ_DATA_PACK_A *)uart_485_buffer);
//         ptz_reply(PTZ_UART_485, CMD_WRONG);                        
       }
       else
       {//7个字节的派尔高D数据以及7个字节自定义类派尔高D数据处理
         ptz_data_pack_process(PTZ_UART_485, (PTZ_DATA_PACK *)uart_485_buffer);
       }       
       memset(uart_485_buffer, 0, sizeof(uart_485_buffer));
       uart_485_data_len = 0;
       uart_485_timer = 0;
       uart_485_state = PTZ_UART_FREE_STATE;
    }

    OSTimeDlyHMSM(0u, 0u, 0u, 5u);  
  }
}

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


///串口2，422通信
static void ptz_uart_422_data_process_task()
{
  static u_int8_t uart_422_buffer[PTZ_UART_422_BUFF_SIZE];
  static unsigned short int uart_422_data_len;
  static unsigned char uart_422_timer;
  unsigned short int i;
  static unsigned char k;
  PTZ_DATA_PACK_A *pack_a = (PTZ_DATA_PACK_A *)uart_422_buffer;
  PTZ_DATA_PACK *pack_pelco_d = (PTZ_DATA_PACK *)uart_422_buffer;
  while(1)
  {
    //422串口
    if(ptz_uart_dev_char_present(uart_422_handle) != 0 &&
      (uart_422_state == PTZ_UART_FREE_STATE))//来了第一个字节的数据
    {
      switch(k)
      {
        case 0://判断每帧数据第一个字节是不是0XFF
          memset(uart_422_buffer, 0, sizeof(uart_422_buffer));
          uart_422_data_len = 0;
          uart_422_timer = 0;
          uart_422_buffer[uart_422_data_len] = ptz_uart_dev_in_char(uart_422_handle);
          if(uart_422_buffer[0] == 0XFF)
          {
            uart_422_data_len++;
            k = 1;
          }
          else
          {
            k = 0;
            uart_422_data_len = 0;
          }
          break;

        case 1://判断每帧数据字节头是不是FF ADD的格式
          uart_422_buffer[uart_422_data_len] = ptz_uart_dev_in_char(uart_422_handle);
          uart_422_data_len++;
          k = 0;
          if(uart_422_buffer[0] == 0XFF &&
             uart_422_buffer[1] == g_ptz.address)
          {
            uart_422_state = PTZ_UART_RECV_STATE;
          }
          else
          {
            if(uart_422_buffer[1] == 0XFF)
            {
              memset(uart_422_buffer, 0, sizeof(uart_422_buffer));
              uart_422_data_len = 1;
              uart_422_buffer[0] = 0XFF;
              k = 1;
            }
          }
          break;
      }
    }
    
    if(uart_422_state == PTZ_UART_RECV_STATE)
    {
      for(i = 0; i < PTZ_UART_422_BUFF_SIZE - 2; i++)
      {
        //判断有无数据
        if(ptz_uart_dev_char_present(uart_422_handle) == 0)
        {
          OSTimeDlyHMSM(0u, 0u, 0u, 1u);//等待一段时间
          uart_422_timer ++;
          if(uart_422_timer > PTZ_UART_RECV_OVERTIME)
          {//接收超时，说明一帧数据接收完成
            break;
          }
        }
        else
        {//读取一个字节数据
          uart_422_buffer[uart_422_data_len] = ptz_uart_dev_in_char(uart_422_handle);
          uart_422_data_len++;
          uart_422_timer = 0;
          if(uart_422_data_len % 100 == 0 && uart_422_data_len > 0)
          {
            OSTimeDlyHMSM(0u, 0u, 0u, 2u);
          }
          if(uart_422_data_len >= 7)
          {
            if(ptz_data_pack_pelco_d_check(pack_pelco_d) == 1)
            {//成功读取一帧派尔高D协议或者自定义类似派尔高D协议，长度7字节
              break;
            }
          }
          if(uart_422_data_len >= (sizeof(PTZ_DATA_PACK_A) + sizeof(unsigned int)))
          {
            if(ptz_data_pack_a_check(pack_a) == 1)
            {//成功读取一帧自定义超长度协议
              break;
            }
          }
          if(uart_422_data_len >= PTZ_UART_422_BUFF_SIZE)
          {
            break;
          }
        }
      }
      uart_422_timer = 0;
      uart_422_state = PTZ_UART_HAND_STATE;
    }
    //处理串口数据
    if(uart_422_state == PTZ_UART_HAND_STATE)
    {
       if(uart_422_data_len > 7)
       {//不定长数据处理，该数据每一帧都超过7个字节
         ptz_data_pack_a_process(PTZ_UART_422, (PTZ_DATA_PACK_A *)uart_422_buffer);
       }
       else
       {//7个字节的派尔高D数据以及7个字节自定义类派尔高D数据处理
         ptz_data_pack_process(PTZ_UART_422, (PTZ_DATA_PACK *)uart_422_buffer);
       }
       
       memset(uart_422_buffer, 0, sizeof(uart_422_buffer));
       uart_422_data_len = 0;
       uart_422_timer = 0;
       uart_422_state = PTZ_UART_FREE_STATE;
    }
    
    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
  
  }
}

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








//=====代理层初始化=====
void init_agent_module()
{
  Usart_init_module();//串口初始化
  BSP_OS_SemCreate(&udp_send_data_mutex,1u,"udp_send_data_mutex");
  BSP_OS_SemCreate(&pack_process_mutex,1u,"pack_process_mutex");
  BSP_OS_SemCreate(&cmd_crc_mutex,1u,"cmd_crc_mutex");
  task_create_listen_ptz_server ();
  creat_task_uart_485_data_process();
  creat_task_uart_422_data_process();
  

}
