#include "ptz_header_file.h"
#include "get_angle.h"
#include "rotate_plan.h"
#include "rotate_bldc.h"
#include "rotate_step.h"
#include "agent_hyt.h"
#include "ptz_struct.h"
#include "math.h"
#include "service_selfcheck.h"
#include "ptz_type_select.h"
#include "ptz_global_variable.h"


///云台角度回复
void ptz_send_angle(char dev,char angle_choice)
{
  unsigned short int uint16_angle;
  unsigned char data[7];
  if(angle_choice == PTZ_HORI)
  {
    uint16_angle = (unsigned short int)(g_ptz.hori_angle_actual * 100 + 0.5);
    data[0] = 0xff;
    data[1] = g_ptz.address;
    data[3] = 0x59;
    data[4] = (u_int16_t)(uint16_angle >> 8);
    data[5] = (u_int16_t)(uint16_angle & 0x00ff);
    data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  }
  else//angle_choice == PTZ_VERT
  {
    uint16_angle = (unsigned short int)(g_ptz.vert_angle_actual * 100 + 0.5);
    data[0] = 0xff;
    data[1] = g_ptz.address;
    data[3] = 0x5b;
    data[4] = (u_int16_t)(uint16_angle >> 8);
    data[5] = (u_int16_t)(uint16_angle & 0x00ff);
    data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  }
  ptz_send_data(dev, data, sizeof(data));
  
  
}



//角度状态查询，仅代理层调用
void ptz_send_sw_angle_state(char dev)
{
  unsigned char data[7];
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x9d;
  data[3] = 0;
  data[4] = g_ptz.hori_angle_state;
  data[5] = g_ptz.vert_angle_state;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  
 
  ptz_send_data(dev, data, sizeof(data));
  
}

//时间计算
unsigned int time_ticks(unsigned int time_now, unsigned time_end)
{
  unsigned int time;
  if(time_now >= time_end)
  {
    time = time_now - time_end;
  }
  else
  {
    time = (time_now - 0) + (4294967295 - time_end);
  }
  return time;
}


#ifdef PTZ_BLDC_MOTOR
static void ptz_hori_ps_delay_ms()
{
  unsigned int hori_ps_time = 0;
#ifdef PTZ_PHOTOELECTRIC_SWITCH 
  if(g_ptz.hori_speed_actual > 0)
  {
    hori_ps_time = (unsigned int)((PTZ_HORI_MAX_SPEED / g_ptz.hori_speed_actual) * PTZ_HORI_PS_TIEM + 0.5);
  }
  else
  {
    hori_ps_time = (unsigned int)((PTZ_HORI_MAX_SPEED / PTZ_HORI_MIN_SPEED) * PTZ_HORI_PS_TIEM + 0.5);
  }
  
  if(hori_ps_time < PTZ_HORI_PS_TIEM)
  {
    hori_ps_time = PTZ_HORI_PS_TIEM;
  }
  
  OSTimeDlyHMSM(0u, 0u, 0u, hori_ps_time);
#endif
}

static void ptz_vert_ps_delay_ms()
{
  unsigned int vert_ps_time = 0;
  
#ifdef PTZ_PHOTOELECTRIC_SWITCH 
  if(g_ptz.vert_speed_actual > 0)
  {
    vert_ps_time = (unsigned int)((PTZ_VERT_MAX_SPEED / g_ptz.vert_speed_actual) * PTZ_VERT_PS_TIEM);
  }
  else
  {
    vert_ps_time = (unsigned int)((PTZ_VERT_MAX_SPEED / PTZ_VERT_MIN_SPEED) * PTZ_VERT_PS_TIEM);
  }
  
  if(vert_ps_time < PTZ_VERT_PS_TIEM)
  {
    vert_ps_time = PTZ_VERT_PS_TIEM;
  }
  
  OSTimeDlyHMSM(0u, 0u, 0u, vert_ps_time);
#endif
}

#endif

#ifdef PTZ_STEP_MOTOR

static void ptz_hori_ps_delay_ms()
{
//  unsigned int hori_ps_time = 0;
//#ifdef PTZ_PHOTOELECTRIC_SWITCH 
//  if(g_ptz.hori_speed_actual > 0)
//  {
//    hori_ps_time = (unsigned int)((PTZ_HORI_MAX_SPEED / g_ptz.hori_speed_actual) * PTZ_HORI_PS_TIEM + 0.5);
//  }
//  else
//  {
//    hori_ps_time = (unsigned int)((PTZ_HORI_MAX_SPEED / PTZ_HORI_MIN_SPEED) * PTZ_HORI_PS_TIEM + 0.5);
//  }
//  
//  if(hori_ps_time < PTZ_HORI_PS_TIEM)
//  {
//    hori_ps_time = PTZ_HORI_PS_TIEM;
//  }
//  
//  OSTimeDlyHMSM(0u, 0u, 0u, hori_ps_time);
//#endif
//  
}

static void ptz_vert_ps_delay_ms()
{
//  unsigned int vert_ps_time = 0;
//  
//#ifdef PTZ_PHOTOELECTRIC_SWITCH 
//  if(g_ptz.vert_speed_actual > 0)
//  {
//    vert_ps_time = (unsigned int)((PTZ_VERT_MAX_SPEED / g_ptz.vert_speed_actual) * PTZ_VERT_PS_TIEM + 0.5);
//  }
//  else
//  {
//    vert_ps_time = (unsigned int)((PTZ_VERT_MAX_SPEED / PTZ_VERT_MIN_SPEED) * PTZ_VERT_PS_TIEM + 0.5);
//  }
//  
//  if(vert_ps_time < PTZ_VERT_PS_TIEM)
//  {
//    vert_ps_time = PTZ_VERT_PS_TIEM;
//  }
//  
//  OSTimeDlyHMSM(0u, 0u, 0u, vert_ps_time);
//#endif
}

#endif
/// 计算云台水平角度
#ifdef PTZ_HORI_ANGLE_AS5047D_ANGLE_ASY
static char ptz_hori_get_angle()
{
  float right_diff_x1 = 0;
  float right_diff_x2 = 0;
  
  float hori_angle[PTZ_HORI_ANGLE_READ_NUM] = {0};
  char  i = 0, j = 0, k = 0;
  float tem = 0;
  float tem1 = 0;
  float tem2 = 0;
  float left_diff_x1 = 0;
  float left_diff_x2 = 0;
  unsigned int tem3 = 0;
  float tem4 = 0;
  static float hori_cycle_angle_right;
  static float hori_cycle_angle_left;
  char flag;
#ifdef PTZ_HORI_GET_ANGLE_MANY
    for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM; i++)
    {
       hori_angle[i] = as5047d_hori_get_angle_a();
       
       
//       test_angle[test_angle_count] = hori_angle[i];
//       test_angle_count ++;
//       if(test_angle_count >= 500)
//       {
//         test_angle_count = 0;
//       }
       
       
       if(hori_angle[i] < 0 || //防止读出的角度为负值
          hori_angle[i] > 360.0||//防止读出的数值超过360
          isnan(hori_angle[i]) == 1)//防止读出的不是一个数
       {//以上错误的数据都该舍弃
         i --;
       }
    }
    
    //由于磁编码器存在0位，防止采集到的数据差距接近360度，对采集到的数据进行优化处理
    //例如0.2、0.1、359.9、359.8这种数据就需要处理
    flag = 0;
    for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM - 1; i++)
    {
      if(fabs(hori_angle[i] - hori_angle[i+1]) > 350.0)
      {
        flag = 1;//说明磁编码器处于0位附近，需要进行数据优化处理
        break;
      }
    }
    if(flag == 1)
    {
      for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM; i++)
      {
        if(hori_angle[i] < 5.0)
        {
          hori_angle[i] = hori_angle[i] + 360.0;
        }
      }
    }
    
    //对采集到的角度值排序
    for(j = 0; j < PTZ_HORI_ANGLE_READ_NUM-1; j++)//采样值由小到大排列
    {
       for(k = 0; k < PTZ_HORI_ANGLE_READ_NUM-j-1; k++)
       {
           if(hori_angle[k] > hori_angle[k+1])
           {
              tem             = hori_angle[k];
              hori_angle[k]   = hori_angle[k+1];
              hori_angle[k+1] = tem;
           }
        }
     }
    tem = 0;
    for(uint8_t i = 1; i < PTZ_HORI_ANGLE_READ_NUM - 1; i++)
    {
      tem = tem + hori_angle[i];//去掉一个最大值和一个最小值
    }
    tem = tem / ((float)(PTZ_HORI_ANGLE_READ_NUM - 2));
    
    if(tem > 360.0)
    {
      tem = tem - 360.0;
    }
    
    g_ptz.hori_as5047d.as5047d_angle_actual = tem;
#endif

#ifdef PTZ_HORI_GET_ANGLE_SINGLE
    tem = as5047d_hori_get_angle_a();
//    hori_as5047d_angle_save[hori_as5047d_angle_save_count] = tem;
//    hori_as5047d_angle_save_count ++;
//    if(hori_as5047d_angle_save_lock >= 1)
//    {
//      hori_as5047d_angle_save_lock ++;
//    }
//    if(hori_as5047d_angle_save_lock >= 10)
//    {
//      hori_as5047d_angle_save_lock = 0;
//    }
//    if(hori_as5047d_angle_save_count > 48)
//    {
//      hori_as5047d_angle_save_count = 0;
//    }
    if(tem < 0 || //防止读出的角度为负值
       tem > 360.0||//防止读出的数值超过360
       isnan(tem) == 1)//防止读出的不是一个数
       {//以上错误的数据都该舍弃,从新读取
         return 0;
       }
    g_ptz.hori_as5047d.as5047d_angle_actual = tem;
    
#endif
    if(g_ptz.hori_as5047d.as5047d_data_reset == 1)
    {
      g_ptz.hori_as5047d.as5047d_cycle_count = 0;
      g_ptz.hori_as5047d.as5047d_cycle_num = 0;
      g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
      g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0;
      g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
      g_ptz.hori_as5047d.as5047d_data_reset = 0;
      tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
      hori_cycle_angle_right = tem1 + 350.0;
      hori_cycle_angle_left = tem1 + 10.0;
    }
    //反转，云台角度减小，磁编码角度增大
    if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
    {
#ifdef PTZ_PHOTOELECTRIC_SWITCH
      //挡片更新角度
      if(g_ptz.hori_as5047d.as5047d_state == 1)
      {
//        if(g_ptz.hori_ps_sw3_right_fall >= 1)
//        {
//          if(PS_HORI_SW3_READ == PS_COVER)
//          {
//            g_ptz.hori_as5047d.as5047d_cycle_count = 0;
//            g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
//            g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0;
//            g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
//            g_ptz.hori_ps_sw3_right_fall = 0;
//          }
//        }
        if(g_ptz.hori_ps_sw3_left_rise >= 1)
        {
#ifdef PTZ_SW3_LEFT_RISE_UPDATE
          ptz_hori_ps_delay_ms();
          if(PS_HORI_SW3_READ == PS_NO_COVER)
          {
            g_ptz.hori_as5047d.as5047d_cycle_count =  g_ptz.hori_as5047d.as5047d_cycle_num;
            g_ptz.hori_as5047d.as5047d_ptz_angle_actual =  g_ptz.hori_as5047d.as5047d_ptz_angle_max;
            g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
            g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
              g_ptz.hori_as5047d.as5047d_angle_actual + g_ptz.hori_as5047d.as5047d_remain_angle;
            if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
            {
              g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
                g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
            }
            g_ptz.hori_angle_state = 1;
            g_ptz.hori_ps_sw3_left_rise = 0;
          }
#endif
          g_ptz.hori_ps_sw3_left_rise = 0;
        }
        
        if(g_ptz.hori_ps_sw3_right_fall >= 1)
        {
          g_ptz.hori_ps_sw3_right_fall = 0;
        }  
        if(g_ptz.hori_ps_sw3_right_rise >= 1)
        {
          g_ptz.hori_ps_sw3_right_rise = 0;
        }
        if(g_ptz.hori_ps_sw3_left_fall >= 1)
        {
          g_ptz.hori_ps_sw3_left_fall = 0;
        }
      }
      
#endif
      
      tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
      hori_cycle_angle_right = tem1 + 350.0;
      hori_cycle_angle_left = tem1 + 10.0;
      //根据计算方法b，计算云台当前角度
      
      //判断磁编码器是否转完一圈
      
      //首先计算当前角度度和初始角度之间的角度距离
      left_diff_x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 
                          g_ptz.hori_as5047d.as5047d_ptz_init_angle);
      
      left_diff_x2 = fabs(360.0 - g_ptz.hori_as5047d.as5047d_ptz_init_angle) + 
                          fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 0);
      
      if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_init_angle
         && left_diff_x1 <= PTZ_LEFT_CYCLE_C
         && g_ptz.hori_as5047d.as5047d_ptz_angle_actual < hori_cycle_angle_left)
      {
        g_ptz.hori_as5047d.as5047d_cycle_count_last = g_ptz.hori_as5047d.as5047d_cycle_count;
        g_ptz.hori_as5047d.as5047d_cycle_count --;//转动圈数减少1
      }
      
      if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_ptz_init_angle
         && left_diff_x2 <= PTZ_LEFT_CYCLE_C
         && g_ptz.hori_as5047d.as5047d_ptz_angle_actual < hori_cycle_angle_left)
      {
        g_ptz.hori_as5047d.as5047d_cycle_count_last = g_ptz.hori_as5047d.as5047d_cycle_count;
        g_ptz.hori_as5047d.as5047d_cycle_count --;//转动圈数减少1
      }
    }
    
    
    //正转，云台角度增大，磁编码角度减小
    if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
    {
#ifdef PTZ_PHOTOELECTRIC_SWITCH
      //挡片更新角度
      if(g_ptz.hori_as5047d.as5047d_state == 1)
      {
//        if(g_ptz.hori_ps_sw3_left_rise >= 1)
//        {
//          if(PS_HORI_SW3_READ == PS_NO_COVER)
//          {
//            g_ptz.hori_as5047d.as5047d_cycle_count =  g_ptz.hori_as5047d.as5047d_cycle_num;
//            g_ptz.hori_as5047d.as5047d_ptz_angle_actual =  g_ptz.hori_as5047d.as5047d_ptz_angle_max;
//            g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
//            g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual + 
//              g_ptz.hori_as5047d.as5047d_remain_angle;
//            if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
//            {
//              g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
//                g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
//            }
//            g_ptz.hori_ps_sw3_left_rise = 0;
//          }
//        }
        if(g_ptz.hori_ps_sw3_right_fall >= 1)
        {
          ptz_hori_ps_delay_ms();
          if(PS_HORI_SW3_READ == PS_COVER)
          {
            g_ptz.hori_as5047d.as5047d_cycle_count = 0;
            g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
            g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0;
            g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
            g_ptz.hori_angle_state = 1;
            g_ptz.hori_ps_sw3_right_fall = 0;
          }
          g_ptz.hori_ps_sw3_right_fall = 0;
        }
        
        if(g_ptz.hori_ps_sw3_left_rise >= 1)
        {
          g_ptz.hori_ps_sw3_left_rise = 0;
        }
        if(g_ptz.hori_ps_sw3_right_rise >= 1)
        {
          g_ptz.hori_ps_sw3_right_rise = 0;
        }
        if(g_ptz.hori_ps_sw3_left_fall >= 1)
        {
          g_ptz.hori_ps_sw3_left_fall = 0;
        }
      }
      
#endif 
      
      tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
      hori_cycle_angle_right = tem1 + 350.0;
      hori_cycle_angle_left = tem1 + 10.0;
      
      //根据计算方法b，计算云台当前角度
      
      //判断磁编码器是否转完一圈
      
      //首先计算当前角度度和初始角度之间的角度距离
      right_diff_x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 
                          g_ptz.hori_as5047d.as5047d_ptz_init_angle);
      
      right_diff_x2 = fabs(360.0 - g_ptz.hori_as5047d.as5047d_angle_actual) + 
                          fabs(g_ptz.hori_as5047d.as5047d_ptz_init_angle - 0);
      
      if(g_ptz.hori_as5047d.as5047d_angle_actual <= g_ptz.hori_as5047d.as5047d_ptz_init_angle
         && right_diff_x1 <= PTZ_RIGHT_CYCLE_C
         && g_ptz.hori_as5047d.as5047d_ptz_angle_actual > hori_cycle_angle_right)
      {
        g_ptz.hori_as5047d.as5047d_cycle_count_last = g_ptz.hori_as5047d.as5047d_cycle_count;
        g_ptz.hori_as5047d.as5047d_cycle_count ++;//转动圈数增加1
      }
     
      if(g_ptz.hori_as5047d.as5047d_angle_actual >= g_ptz.hori_as5047d.as5047d_ptz_init_angle
         && right_diff_x2 <= PTZ_RIGHT_CYCLE_C
         && g_ptz.hori_as5047d.as5047d_ptz_angle_actual > hori_cycle_angle_right)
      {
        g_ptz.hori_as5047d.as5047d_cycle_count_last = g_ptz.hori_as5047d.as5047d_cycle_count;
        g_ptz.hori_as5047d.as5047d_cycle_count ++;//转动圈数增加1
      }
      
    }
   
          //计算当前角度  
          if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_init_angle)
          {
           g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual = 
              fabs(360.0 - g_ptz.hori_as5047d.as5047d_angle_actual) +
                fabs(g_ptz.hori_as5047d.as5047d_ptz_init_angle - 0) +
                  g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
          }
          if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_ptz_init_angle)
          {
            g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual = 
              (g_ptz.hori_as5047d.as5047d_ptz_init_angle - 
                 g_ptz.hori_as5047d.as5047d_angle_actual) + 
                   g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
          }
          if(g_ptz.hori_as5047d.as5047d_angle_actual == g_ptz.hori_as5047d.as5047d_ptz_init_angle)
          {
            g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
          }
          
          //对磁编码器角度进行滤波
          if((fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last) 
              < PTZ_HORI_AS5047D_ANGLE_WF_A) ||
             (fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last) 
              > PTZ_HORI_AS5047D_ANGLE_WF_B))
          {
            g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
            g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
            //break;
            
          }
          else
          {
      //      if(hori_as5047d_angle_save_lock == 0)
      //      {
      //        hori_as5047d_angle_save_lock = 1;
      //      }
      
          }
    
    //防止角度范围超过限度
    if(g_ptz.hori_as5047d.as5047d_state == 1)
    {
      if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_angle_max)
      {
        tem2 = fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_max);
        g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 
          g_ptz.hori_as5047d.as5047d_ptz_angle_actual -
            g_ptz.hori_as5047d.as5047d_ptz_angle_max;
        g_ptz.hori_as5047d.as5047d_cycle_count = 0;
        g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual + tem2;
        if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
        {
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
            g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
        }
      }
      
      if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual < 0)
      {
        tem2 = fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_actual - 0);
        g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 
          g_ptz.hori_as5047d.as5047d_ptz_angle_actual +
            g_ptz.hori_as5047d.as5047d_ptz_angle_max;
        
        g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
        
        g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
          g_ptz.hori_as5047d.as5047d_angle_actual + g_ptz.hori_as5047d.as5047d_remain_angle - tem2;
        if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
        {
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
            g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
        }
        if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
        {
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
            g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
        }
      }
      if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual ==  g_ptz.hori_as5047d.as5047d_ptz_angle_max
         || g_ptz.hori_as5047d.as5047d_ptz_angle_actual == 0)
      {
        if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
        {
          g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
          g_ptz.hori_as5047d.as5047d_cycle_count = 0;
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
        }
        else
        {
          g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
          g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
          g_ptz.hori_as5047d.as5047d_angle_actual + g_ptz.hori_as5047d.as5047d_remain_angle;
          if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
          {
            g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
              g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
          }
        }
      }
      //对云台角度进行四舍五入,保留小数点后3位小数
      tem3 = (unsigned int)((g_ptz.hori_as5047d.as5047d_ptz_angle_actual * 
        g_ptz.hori_as5047d.as5047d_ptz_angle_K) * 1000.0 + 0.5);
      //计算云台实际角度
      tem4 = tem3 / 1000.0;
      g_ptz.hori_angle_actual_a = tem4;
      if(g_ptz.offset_angle.hori_offset_switch == 2)
      {
        tem4 = tem4 - g_ptz.offset_angle.hori_zero_offset_angle;
        if(tem4 < 0)
        {
          tem4 = tem4 + 360.0;
        }
      }
      g_ptz.hori_angle_actual = tem4;
    }
    
    //保存当前磁编码器的值以及云台角度值
    g_ptz.hori_as5047d.as5047d_angle_last = g_ptz.hori_as5047d.as5047d_angle_actual;
    g_ptz.hori_as5047d.as5047d_ptz_angle_last = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
    g_ptz.hori_angle_last   = g_ptz.hori_angle_actual;
    return 1;
}

static char ptz_hori_get_angle_a()
{
  float right_diff_x1 = 0;
  float right_diff_x2 = 0;
  
  float hori_angle[PTZ_HORI_ANGLE_READ_NUM] = {0};
  char  i = 0, j = 0, k = 0, m = 0;
  float tem = 0;
  float tem1 = 0;
  float tem2 = 0;
  float left_diff_x1 = 0;
  float left_diff_x2 = 0;
  float x1 = 0, x2 = 0, x3 = 0;
  unsigned int tem3 = 0;
  float tem4 = 0;
  static float hori_cycle_angle_right;
  static float hori_cycle_angle_left;
  char flag;
  int  cycle_count_save = 0;
  
  
  g_ptz.hori_as5047d.time_end = g_ptz.hori_as5047d.time_now;
  g_ptz.hori_as5047d.time_now = OSTimeGet();
  g_ptz.hori_as5047d.time = time_ticks(g_ptz.hori_as5047d.time_now, g_ptz.hori_as5047d.time_end);
  
#ifdef CLK_50M
       if(g_ptz.hori_as5047d.time < 2)
       {
         g_ptz.hori_as5047d.time = 2;
       }
#else
       if(g_ptz.hori_as5047d.time < 1)
       {
         g_ptz.hori_as5047d.time = 1;
       }
#endif
  
#ifdef PTZ_HORI_GET_ANGLE_MANY
    for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM; i++)
    {
       hori_angle[i] = as5047d_hori_get_angle_a();
       
       
//       test_angle[test_angle_count] = hori_angle[i];
//       test_angle_count ++;
//       if(test_angle_count >= 500)
//       {
//         test_angle_count = 0;
//       }
       
       
       if(hori_angle[i] < 0 || //防止读出的角度为负值
          hori_angle[i] > 360.0||//防止读出的数值超过360
          isnan(hori_angle[i]) == 1)//防止读出的不是一个数
       {//以上错误的数据都该舍弃
         i --;
       }
    }
    
    //由于磁编码器存在0位，防止采集到的数据差距接近360度，对采集到的数据进行优化处理
    //例如0.2、0.1、359.9、359.8这种数据就需要处理
    flag = 0;
    for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM - 1; i++)
    {
      if(fabs(hori_angle[i] - hori_angle[i+1]) > 350.0)
      {
        flag = 1;//说明磁编码器处于0位附近，需要进行数据优化处理
        break;
      }
    }
    if(flag == 1)
    {
      for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM; i++)
      {
        if(hori_angle[i] < 5.0)
        {
          hori_angle[i] = hori_angle[i] + 360.0;
        }
      }
    }
    
    //对采集到的角度值排序
    for(j = 0; j < PTZ_HORI_ANGLE_READ_NUM-1; j++)//采样值由小到大排列
    {
       for(k = 0; k < PTZ_HORI_ANGLE_READ_NUM-j-1; k++)
       {
           if(hori_angle[k] > hori_angle[k+1])
           {
              tem             = hori_angle[k];
              hori_angle[k]   = hori_angle[k+1];
              hori_angle[k+1] = tem;
           }
        }
     }
    tem = 0;
    for(uint8_t i = 1; i < PTZ_HORI_ANGLE_READ_NUM - 1; i++)
    {
      tem = tem + hori_angle[i];//去掉一个最大值和一个最小值
    }
    tem = tem / ((float)(PTZ_HORI_ANGLE_READ_NUM - 2));
    
    if(tem > 360.0)
    {
      tem = tem - 360.0;
    }
    
    g_ptz.hori_as5047d.as5047d_angle_actual = tem;
#endif

#ifdef PTZ_HORI_GET_ANGLE_SINGLE
    tem = as5047d_hori_get_angle_a();
//    hori_as5047d_angle_save[hori_as5047d_angle_save_count] = tem;
//    hori_as5047d_angle_save_count ++;
//    if(hori_as5047d_angle_save_lock >= 1)
//    {
//      hori_as5047d_angle_save_lock ++;
//    }
//    if(hori_as5047d_angle_save_lock >= 10)
//    {
//      hori_as5047d_angle_save_lock = 0;
//    }
//    if(hori_as5047d_angle_save_count > 48)
//    {
//      hori_as5047d_angle_save_count = 0;
//    }
    
    
//    if(tem < 0 || //防止读出的角度为负值
//       tem > 360.0||//防止读出的数值超过360
//       isnan(tem) == 1)//防止读出的不是一个数
//       {//以上错误的数据都该舍弃,从新读取
//         return 0;
//       }
//    g_ptz.hori_as5047d.as5047d_angle_actual = tem;
    
    for(m = 0; m < 5; m++)
    {
      
      tem = as5047d_hori_get_angle_a();
      
      if(tem < 0          || //防止读出的角度为负值
         tem > 360.0      ||//防止读出的数值超过360
         isnan(tem) == 1  ||//防止读出的不是一个数
         ((fabs(tem - g_ptz.hori_as5047d.as5047d_angle_last) > PTZ_HORI_ANGLE_DIFF_A) && (fabs(tem - g_ptz.hori_as5047d.as5047d_angle_last) < PTZ_HORI_ANGLE_DIFF_B)))
      {//以上错误的数据都该舍弃,从新读取
       asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");
      }
      else
      {
        break;
      }
    }
    g_ptz.hori_as5047d.as5047d_angle_actual = tem;
    
#endif
   
    if(g_ptz.hori_as5047d.as5047d_data_reset == 1)
    {
      g_ptz.hori_as5047d.as5047d_cycle_count = 0;
      g_ptz.hori_as5047d.as5047d_cycle_num = 0;
      g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
      g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0;
      g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
      g_ptz.hori_as5047d.as5047d_data_reset = 0;
      tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
      hori_cycle_angle_right = tem1 + 350.0;
      hori_cycle_angle_left = tem1 + 10.0;
    }
    
    x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 
              g_ptz.hori_as5047d.as5047d_angle_last);
    
    x2 = (g_ptz.hori_as5047d.as5047d_angle_actual - 0) +
         (360.0 - g_ptz.hori_as5047d.as5047d_angle_last);
    
    x3 = (g_ptz.hori_as5047d.as5047d_angle_last - 0) +
         (360.0 - g_ptz.hori_as5047d.as5047d_angle_actual);
           
    //首先判断磁编码器角度判断磁编码器转向
    //磁编码器正转角度增大       
    if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_angle_last
       && x1 < x3)
    {
      g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_FWD;
    }
    
    if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_angle_last
       && x2 < x1)
    {
      g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_FWD;
    } 
    //磁编码器反转角度减小      
    if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_angle_last
       && x3 < x1)
    {
      g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_REV;
    }
    
    if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_angle_last
       && x1 < x2)
    {
      g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_REV;
    } 
    
    if(x1 == x2 || x1 == x3)
    {
      if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
      {
        g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_FWD;
      }
      if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
      {
        g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_REV;
      }
    }
    
    //反转，云台角度减小，磁编码角度增大
    if(g_ptz.hori_as5047d.as5047d_dir == AS5047D_DIR_FWD)
    {
#ifdef PTZ_PHOTOELECTRIC_SWITCH
      //挡片更新角度
      if(g_ptz.hori_as5047d.as5047d_state == 1)
      {
//        if(g_ptz.hori_ps_sw3_right_fall >= 1)
//        {
//          if(PS_HORI_SW3_READ == PS_COVER)
//          {
//            g_ptz.hori_as5047d.as5047d_cycle_count = 0;
//            g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
//            g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0;
//            g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
//            g_ptz.hori_ps_sw3_right_fall = 0;
//          }
//        }
        if(g_ptz.hori_ps_sw3_left_rise >= 1)
        {
#ifdef PTZ_SW3_LEFT_RISE_UPDATE
          if(g_ptz.hori_angle_erro_switch == 0)
          {//误差消除开关关闭，使能左转上升沿角度更新，，打开则失能
            ptz_hori_ps_delay_ms();
            if(PS_HORI_SW3_READ == PS_NO_COVER)
            {
              g_ptz.hori_as5047d.as5047d_cycle_count =  g_ptz.hori_as5047d.as5047d_cycle_num;
              g_ptz.hori_as5047d.as5047d_ptz_angle_actual =  g_ptz.hori_as5047d.as5047d_ptz_angle_max;
              g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
              g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
                g_ptz.hori_as5047d.as5047d_angle_actual + g_ptz.hori_as5047d.as5047d_remain_angle;
              if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
              {
                g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
                  g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
              }
              g_ptz.hori_angle_state = 1;
              g_ptz.hori_ps_sw3_left_rise = 0;
            }
          }
#endif
          g_ptz.hori_ps_sw3_left_rise = 0;
        }
        
        if(g_ptz.hori_ps_sw3_right_fall >= 1)
        {
          g_ptz.hori_ps_sw3_right_fall = 0;
        }
        if(g_ptz.hori_ps_sw3_right_rise >= 1)
        {
          g_ptz.hori_ps_sw3_right_rise = 0;
        }
        if(g_ptz.hori_ps_sw3_left_fall >= 1)
        {
          g_ptz.hori_ps_sw3_left_fall = 0;
        }
      }
     
#endif
      
      tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
      hori_cycle_angle_right = tem1 + 350.0;
      hori_cycle_angle_left = tem1 + 10.0;
      //根据计算方法b，计算云台当前角度
      
      //判断磁编码器是否转完一圈
      
      //首先计算当前角度度和初始角度之间的角度距离
      left_diff_x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 
                          g_ptz.hori_as5047d.as5047d_ptz_init_angle);
      
      left_diff_x2 = fabs(360.0 - g_ptz.hori_as5047d.as5047d_ptz_init_angle) + 
                          fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 0);
      
      if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_init_angle
         && left_diff_x1 <= PTZ_LEFT_CYCLE_C
         && g_ptz.hori_as5047d.as5047d_ptz_angle_actual < hori_cycle_angle_left)
      {
        g_ptz.hori_as5047d.as5047d_cycle_count_last = g_ptz.hori_as5047d.as5047d_cycle_count;
        g_ptz.hori_as5047d.as5047d_cycle_count --;//转动圈数减少1
      }
      
      if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_ptz_init_angle
         && left_diff_x2 <= PTZ_LEFT_CYCLE_C
         && g_ptz.hori_as5047d.as5047d_ptz_angle_actual < hori_cycle_angle_left)
      {
        g_ptz.hori_as5047d.as5047d_cycle_count_last = g_ptz.hori_as5047d.as5047d_cycle_count;
        g_ptz.hori_as5047d.as5047d_cycle_count --;//转动圈数减少1
      }
    }
    
    
    //正转，云台角度增大，磁编码角度减小
    if(g_ptz.hori_as5047d.as5047d_dir == AS5047D_DIR_REV)
    {
#ifdef PTZ_PHOTOELECTRIC_SWITCH
      //挡片更新角度
      if(g_ptz.hori_as5047d.as5047d_state == 1)
      {
//        if(g_ptz.hori_ps_sw3_left_rise >= 1)
//        {
//          if(PS_HORI_SW3_READ == PS_NO_COVER)
//          {
//            g_ptz.hori_as5047d.as5047d_cycle_count =  g_ptz.hori_as5047d.as5047d_cycle_num;
//            g_ptz.hori_as5047d.as5047d_ptz_angle_actual =  g_ptz.hori_as5047d.as5047d_ptz_angle_max;
//            g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
//            g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual + 
//              g_ptz.hori_as5047d.as5047d_remain_angle;
//            if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
//            {
//              g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
//                g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
//            }
//            g_ptz.hori_ps_sw3_left_rise = 0;
//          }
//        }
        if(g_ptz.hori_ps_sw3_right_fall >= 1)
        {
          ptz_hori_ps_delay_ms();
          if(PS_HORI_SW3_READ == PS_COVER)
          {
            g_ptz.hori_as5047d.as5047d_cycle_count = 0;
            g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
            g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0;
            g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
            g_ptz.hori_angle_state = 1;
            g_ptz.hori_ps_sw3_right_fall = 0;
          }
          g_ptz.hori_ps_sw3_right_fall = 0;
        }
        
        if(g_ptz.hori_ps_sw3_left_rise >= 1)
        {
          g_ptz.hori_ps_sw3_left_rise = 0;
        }
        if(g_ptz.hori_ps_sw3_right_rise >= 1)
        {
          g_ptz.hori_ps_sw3_right_rise = 0;
        }
        if(g_ptz.hori_ps_sw3_left_fall >= 1)
        {
          g_ptz.hori_ps_sw3_left_fall = 0;
        }
      }
      
#endif

      tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
      hori_cycle_angle_right = tem1 + 350.0;
      hori_cycle_angle_left = tem1 + 10.0;
      
      //根据计算方法b，计算云台当前角度
      
      //判断磁编码器是否转完一圈
      
      //首先计算当前角度度和初始角度之间的角度距离
      right_diff_x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 
                          g_ptz.hori_as5047d.as5047d_ptz_init_angle);
      
      right_diff_x2 = fabs(360.0 - g_ptz.hori_as5047d.as5047d_angle_actual) + 
                          fabs(g_ptz.hori_as5047d.as5047d_ptz_init_angle - 0);
      
      if(g_ptz.hori_as5047d.as5047d_angle_actual <= g_ptz.hori_as5047d.as5047d_ptz_init_angle
         && right_diff_x1 <= PTZ_RIGHT_CYCLE_C
         && g_ptz.hori_as5047d.as5047d_ptz_angle_actual > hori_cycle_angle_right)
      {
        g_ptz.hori_as5047d.as5047d_cycle_count_last = g_ptz.hori_as5047d.as5047d_cycle_count;
        g_ptz.hori_as5047d.as5047d_cycle_count ++;//转动圈数增加1
      }
     
      if(g_ptz.hori_as5047d.as5047d_angle_actual >= g_ptz.hori_as5047d.as5047d_ptz_init_angle
         && right_diff_x2 <= PTZ_RIGHT_CYCLE_C
         && g_ptz.hori_as5047d.as5047d_ptz_angle_actual > hori_cycle_angle_right)
      {
        g_ptz.hori_as5047d.as5047d_cycle_count_last = g_ptz.hori_as5047d.as5047d_cycle_count;
        g_ptz.hori_as5047d.as5047d_cycle_count ++;//转动圈数增加1
      }
      
    }
    
    //保存当前磁编码器圈数
    cycle_count_save = g_ptz.hori_as5047d.as5047d_cycle_count;
    
    for(i = 0; i < 4; i++)
    {//这个循环的目的是为了防止磁编码器圈数计算错误，恢复正常的磁编码圈数    
    //计算当前角度  
        if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_init_angle)
        {
         g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual = 
            fabs(360.0 - g_ptz.hori_as5047d.as5047d_angle_actual) +
              fabs(g_ptz.hori_as5047d.as5047d_ptz_init_angle - 0) +
                g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
        }
        if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_ptz_init_angle)
        {
          g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual = 
            (g_ptz.hori_as5047d.as5047d_ptz_init_angle - 
               g_ptz.hori_as5047d.as5047d_angle_actual) + 
                 g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
        }
        if(g_ptz.hori_as5047d.as5047d_angle_actual == g_ptz.hori_as5047d.as5047d_ptz_init_angle)
        {
          g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
        }
        
        //对磁编码器总角度进行滤波
        if((fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last) 
            < PTZ_HORI_AS5047D_ANGLE_WF_A) ||
           (fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last) 
            > PTZ_HORI_AS5047D_ANGLE_WF_B))
        {
          g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
          g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
          break;
        }
        else
        {
          //根据判断，恢复实际的磁编码器圈数
          switch(i)
          {
            case 0:
              g_ptz.hori_as5047d.as5047d_cycle_count = cycle_count_save;
              g_ptz.hori_as5047d.as5047d_cycle_count --;//转动圈数减少1
              break;
                
            case 1:
              g_ptz.hori_as5047d.as5047d_cycle_count = cycle_count_save;
              g_ptz.hori_as5047d.as5047d_cycle_count ++;//转动圈数增加1
              break;

            case 2: //磁编码器实际圈数恢复失败
              g_ptz.hori_as5047d.as5047d_cycle_count = cycle_count_save;
              break; 
              
            case 3:
              g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
              g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
              break;
          }
        }
    }
    //防止角度范围超过限度
    if(g_ptz.hori_as5047d.as5047d_state == 1)
    {
      if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_angle_max)
      {
        tem2 = fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_max);
        
        g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_max;
        
        g_ptz.hori_as5047d.as5047d_cycle_count = (int)(g_ptz.hori_as5047d.as5047d_ptz_angle_actual / 360.0);
        
        g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual + tem2;
        if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
        {
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
            g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
        }
      }
      
      if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual < 0)
      {
        tem2 = fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_actual - 0);
        
        g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_actual + g_ptz.hori_as5047d.as5047d_ptz_angle_max;
        
        g_ptz.hori_as5047d.as5047d_cycle_count = (int)(g_ptz.hori_as5047d.as5047d_ptz_angle_actual / 360.0);
        
        g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual + g_ptz.hori_as5047d.as5047d_remain_angle - tem2;
        
        if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
        {
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
            g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
        }
        if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
        {
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
            g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
        }
      }
      if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual ==  g_ptz.hori_as5047d.as5047d_ptz_angle_max
         || g_ptz.hori_as5047d.as5047d_ptz_angle_actual == 0)
      {
        if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
        {
          g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
          g_ptz.hori_as5047d.as5047d_cycle_count = 0;
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
        }
        else
        {
          g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
          g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
          g_ptz.hori_as5047d.as5047d_angle_actual + g_ptz.hori_as5047d.as5047d_remain_angle;
          if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
          {
            g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
              g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
          }
        }
      }
      //对云台角度进行四舍五入,保留小数点后3位小数
      tem3 = (unsigned int)((g_ptz.hori_as5047d.as5047d_ptz_angle_actual * 
        g_ptz.hori_as5047d.as5047d_ptz_angle_K) * 1000.0 + 0.5);
      //计算云台实际角度
      tem4 = tem3 / 1000.0;
      g_ptz.hori_angle_actual_a = tem4;
      if(g_ptz.offset_angle.hori_offset_switch == 2)
      {
        tem4 = tem4 - g_ptz.offset_angle.hori_zero_offset_angle;
        if(tem4 < 0)
        {
          tem4 = tem4 + 360.0;
        }
      }
      g_ptz.hori_angle_actual = tem4;
    }
    
    //保存当前磁编码器的值以及云台角度值
    g_ptz.hori_as5047d.as5047d_angle_last = g_ptz.hori_as5047d.as5047d_angle_actual;
    g_ptz.hori_as5047d.as5047d_ptz_angle_last = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
    g_ptz.hori_angle_last   = g_ptz.hori_angle_actual;
    return 1;
}

#endif


/// 计算云台垂直角度
#ifdef PTZ_VERT_ANGLE_AS5047D_ANGLE_ASY
static char ptz_vert_get_angle()
{
  float up_diff_x1 = 0;
  float up_diff_x2 = 0;
  
  float vert_angle[PTZ_VERT_ANGLE_READ_NUM] = {0};
  char  i = 0, j = 0, k = 0;
  float tem = 0;
  float tem1 = 0;
  static float vert_cycle_angle_up;
  static float vert_cycle_angle_down;
  long int tem3 = 0;
  float  tem4 = 0;
  float down_diff_x1 = 0;
  float down_diff_x2 = 0;  
  char flag;
//  static float vert_angle_actual;
//  static float vert_angle_last;
#ifdef PTZ_VERT_GET_ANGLE_MANY
    for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM; i++)
    {
       vert_angle[i] = as5047d_vert_get_angle_a();
       
//       test_angle[test_angle_count] = vert_angle[i];
//       test_angle_count ++;
//       if(test_angle_count >= 500)
//       {
//         test_angle_count = 0;
//       }
//       
       if(vert_angle[i] < 0 || //防止读出的角度为负值
          vert_angle[i] > 360.0||//防止读出的数值超过360
          isnan(vert_angle[i]) == 1)//防止读出的不是一个数
       {//以上错误的数据都该舍弃
         i --;
       }
    }
    
    //由于磁编码器存在0位，防止采集到的数据差距接近360度，对采集到的数据进行优化处理
    //例如0.2、0.1、359.9、359.8这种数据就需要处理
    flag = 0;
    for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM - 1; i++)
    {
      if(fabs(vert_angle[i] - vert_angle[i+1]) > 350.0)
      {
        flag = 1;//说明磁编码器处于0位附近，需要进行数据优化处理
        break;
      }
    }
    if(flag == 1)
    {
      for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM; i++)
      {
        if(vert_angle[i] < 5.0)
        {
          vert_angle[i] = vert_angle[i] + 360.0;
        }
      }
    }
    
    //对采集到的角度值排序
    for(j = 0; j < PTZ_VERT_ANGLE_READ_NUM-1; j++)//采样值由小到大排列
    {
       for(k = 0; k < PTZ_VERT_ANGLE_READ_NUM-j-1; k++)
       {
           if(vert_angle[k] > vert_angle[k+1])
           {
              tem             = vert_angle[k];
              vert_angle[k]   = vert_angle[k+1];
              vert_angle[k+1] = tem;
           }
        }
     }
    tem = 0;
    for(uint8_t i = 1; i < PTZ_VERT_ANGLE_READ_NUM - 1; i++)
    {
      tem = tem + vert_angle[i];//去掉一个最大值和一个最小值
    }
    tem = tem / ((float)(PTZ_VERT_ANGLE_READ_NUM - 2));
    
    if(tem > 360.0)
    {
      tem = tem - 360.0;
    }
    
    g_ptz.vert_as5047d.as5047d_angle_actual = tem;
#endif
    
#ifdef PTZ_VERT_GET_ANGLE_SINGLE
    tem = as5047d_vert_get_angle_a();
    
    if(tem < 0 || //防止读出的角度为负值
       tem > 360.0||//防止读出的数值超过360
       isnan(tem) == 1)//防止读出的不是一个数
       {//以上错误的数据都该舍弃,从新读取
         return 0;
       }
    g_ptz.vert_as5047d.as5047d_angle_actual = tem;
#endif
    
    if(g_ptz.vert_as5047d.as5047d_data_reset == 1)
    {
      g_ptz.vert_as5047d.as5047d_cycle_count = 0;
      g_ptz.vert_as5047d.as5047d_cycle_num = 0;
      g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
      g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
      g_ptz.vert_as5047d.as5047d_data_reset = 0;
      g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
      tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
      vert_cycle_angle_up = tem1 + 350.0;
      vert_cycle_angle_down = tem1 + 10.0;
    }
    
    //反转，云台角度减小，磁编码角度增大
    if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
    {
#ifdef PTZ_PHOTOELECTRIC_SWITCH
      //挡片更新角度
      if(g_ptz.vert_as5047d.as5047d_state == 1)
      {
        if(g_ptz.vert_ps_sw1_up_rise > 0)
        {
          g_ptz.vert_ps_sw1_up_rise = 0;
        }
        if( g_ptz.vert_ps_sw1_down_fall > 0)
        {
#ifdef PTZ_SW1_DOWN_FALL_UPDATE
          ptz_vert_ps_delay_ms();
          if(PS_VERT_SW1_READ == PS_COVER)
          {
            g_ptz.vert_as5047d.as5047d_cycle_count = 0;
            g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
            g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
            g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
            g_ptz.vert_angle_state = 1;
            g_ptz.vert_ps_sw1_down_fall = 0; 
          }
#endif
          g_ptz.vert_ps_sw1_down_fall = 0; 
        }
        
//        if(g_ptz.vert_ps_sw2_down_rise > 0)
//        {
//          g_ptz.vert_as5047d.as5047d_cycle_count =  g_ptz.vert_as5047d.as5047d_cycle_num;
//          g_ptz.vert_as5047d.as5047d_ptz_angle_actual =  g_ptz.vert_as5047d.as5047d_ptz_angle_max;
//          g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
//          g_ptz.vert_as5047d.as5047d_ptz_init_angle = 
//          g_ptz.vert_as5047d.as5047d_angle_actual + g_ptz.vert_as5047d.as5047d_remain_angle;
//          if(g_ptz.vert_as5047d.as5047d_ptz_init_angle >= 360.0)
//          {
//            g_ptz.vert_as5047d.as5047d_ptz_init_angle = 
//              g_ptz.vert_as5047d.as5047d_ptz_init_angle - 360.0;
//          }
//          g_ptz.vert_ps_sw2_down_rise = 0;
//        }
        
        if(g_ptz.vert_ps_sw2_down_rise > 0)
        {
          g_ptz.vert_ps_sw2_down_rise = 0;
        }
        
        if(g_ptz.vert_ps_sw2_up_fall > 0)
        {
          g_ptz.vert_ps_sw2_up_fall = 0;
        }
      }
      
#endif  
      
      tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
      vert_cycle_angle_up = tem1 + 350.0;
      vert_cycle_angle_down = tem1 + 10.0;
      //根据计算方法b，计算云台当前角度
      
      //判断磁编码器是否转完一圈
      
      //首先计算当前角度度和初始角度之间的角度距离
      down_diff_x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 
                          g_ptz.vert_as5047d.as5047d_ptz_init_angle);
      
      down_diff_x2 = fabs(360.0 - g_ptz.vert_as5047d.as5047d_ptz_init_angle) + 
                          fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 0);
      
      if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_ptz_init_angle
         && down_diff_x1 <= PTZ_DOWN_CYCLE_C
         && g_ptz.vert_as5047d.as5047d_ptz_angle_actual < vert_cycle_angle_down)
      {
        g_ptz.vert_as5047d.as5047d_cycle_count --;//转动圈数减少1
      }
      if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_ptz_init_angle
         && down_diff_x2 <= PTZ_DOWN_CYCLE_C
         && g_ptz.vert_as5047d.as5047d_ptz_angle_actual < vert_cycle_angle_down)
      {
        g_ptz.vert_as5047d.as5047d_cycle_count --;//转动圈数减少1
      }

    }
    
    
    //正转，云台角度增大，磁编码角度减小
    if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
    {
#ifdef PTZ_PHOTOELECTRIC_SWITCH
      //挡片更新角度
      if(g_ptz.vert_as5047d.as5047d_state == 1)
      {
//        if(g_ptz.vert_ps_sw1_up_rise > 0)
//        {
//          g_ptz.vert_as5047d.as5047d_cycle_count = 0;
//          g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
//          g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
//          g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
//
//          g_ptz.vert_ps_sw1_up_rise = 0;
//        }
        if(g_ptz.vert_ps_sw1_up_rise > 0)
        {
#ifdef PTZ_SW1_UP_RISE_UPDATE
          ptz_vert_ps_delay_ms();
          if(PS_VERT_SW1_READ == PS_NO_COVER)
          {
            g_ptz.vert_as5047d.as5047d_cycle_count = 0;
            g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
            g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
            g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
            g_ptz.vert_angle_state = 1;
            g_ptz.vert_ps_sw1_up_rise = 0; 
          }
#endif
          g_ptz.vert_ps_sw1_up_rise = 0;
        }
        if( g_ptz.vert_ps_sw1_down_fall > 0)
        {
          g_ptz.vert_ps_sw1_down_fall = 0;
        }
        
        if(g_ptz.vert_ps_sw2_down_rise > 0)
        {
          g_ptz.vert_ps_sw2_down_rise = 0;
        }
        
        if(g_ptz.vert_ps_sw2_up_fall > 0)
        {
          ptz_vert_ps_delay_ms();
          if(PS_VERT_SW2_READ == PS_COVER)
          {
            g_ptz.vert_as5047d.as5047d_cycle_count =  g_ptz.vert_as5047d.as5047d_cycle_num;
            g_ptz.vert_as5047d.as5047d_ptz_angle_actual =  g_ptz.vert_as5047d.as5047d_ptz_angle_max;
            g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
            g_ptz.vert_as5047d.as5047d_ptz_init_angle = 
            g_ptz.vert_as5047d.as5047d_angle_actual + g_ptz.vert_as5047d.as5047d_remain_angle;
            if(g_ptz.vert_as5047d.as5047d_ptz_init_angle >= 360.0)
            {
              g_ptz.vert_as5047d.as5047d_ptz_init_angle = 
                g_ptz.vert_as5047d.as5047d_ptz_init_angle - 360.0;
            }
            g_ptz.vert_angle_state = 1;
            g_ptz.vert_ps_sw2_up_fall = 0;
          }
          g_ptz.vert_ps_sw2_up_fall = 0;
        }
      }
      
#endif
      
      tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
      vert_cycle_angle_up = tem1 + 350.0;
      vert_cycle_angle_down = tem1 + 10.0;
      //根据计算方法b，计算云台当前角度
      
      //判断磁编码器是否转完一圈
      
      //首先计算当前角度度和初始角度之间的角度距离
      up_diff_x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 
                          g_ptz.vert_as5047d.as5047d_ptz_init_angle);
      
      up_diff_x2 = fabs(360.0 - g_ptz.vert_as5047d.as5047d_angle_actual) + 
                          fabs(g_ptz.vert_as5047d.as5047d_ptz_init_angle - 0);
      
      if(g_ptz.vert_as5047d.as5047d_angle_actual <= g_ptz.vert_as5047d.as5047d_ptz_init_angle
         && up_diff_x1 <= PTZ_UP_CYCLE_C
         && g_ptz.vert_as5047d.as5047d_ptz_angle_actual > vert_cycle_angle_up)
      {
        g_ptz.vert_as5047d.as5047d_cycle_count ++;//转动圈数增加1
      }
      if(g_ptz.vert_as5047d.as5047d_angle_actual >= g_ptz.vert_as5047d.as5047d_ptz_init_angle
         && up_diff_x2 <= PTZ_UP_CYCLE_C
         && g_ptz.vert_as5047d.as5047d_ptz_angle_actual > vert_cycle_angle_up)
      {
        g_ptz.vert_as5047d.as5047d_cycle_count ++;//转动圈数增加1
      }
    }
   
    
    //计算当前角度  
    if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_ptz_init_angle)
    {
      g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual = 
        fabs(360.0 - g_ptz.vert_as5047d.as5047d_angle_actual) +
          fabs(g_ptz.vert_as5047d.as5047d_ptz_init_angle - 0) +
            g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;

    }
    if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_ptz_init_angle)
    {
      g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual = 
        (g_ptz.vert_as5047d.as5047d_ptz_init_angle - 
           g_ptz.vert_as5047d.as5047d_angle_actual) + 
             g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
    }
    if(g_ptz.vert_as5047d.as5047d_angle_actual == g_ptz.vert_as5047d.as5047d_ptz_init_angle)
    {
      g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
    }
    
    //对算出的磁编码器总角度进行滤波处理
    if(fabs(g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last) 
       < PTZ_VERT_AS5047D_ANGLE_WF_A)
    {
      g_ptz.vert_as5047d.as5047d_ptz_angle_actual = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
      g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
    }
    

    if(g_ptz.vert_as5047d.as5047d_state == 1)
    {
      //对云台角度进行四舍五入,保留小数点后3位小数
      tem3 = (long int)((g_ptz.vert_as5047d.as5047d_ptz_angle_K * 
              (g_ptz.vert_as5047d.as5047d_ptz_angle_actual -
                g_ptz.vert_as5047d.as5047d_ptz_angle_max / 2.0)) * 1000.0 + 0.5);
      
      
      tem4 = tem3 / 1000.0;
      g_ptz.vert_angle_actual_a = tem4;
      if(g_ptz.offset_angle.vert_offset_switch == 2)
      {
        tem4 = tem4 - g_ptz.offset_angle.vert_zero_offset_angle;
      }
      g_ptz.vert_angle_actual = tem4;
      
    }
    
    //保存当前磁编码器的值以及云台角度值
    g_ptz.vert_as5047d.as5047d_angle_last = g_ptz.vert_as5047d.as5047d_angle_actual;
    g_ptz.vert_as5047d.as5047d_ptz_angle_last = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
    g_ptz.vert_angle_last   = g_ptz.vert_angle_actual;
    return 1;
}

static char ptz_vert_get_angle_a()
{
  float up_diff_x1 = 0;
  float up_diff_x2 = 0;
  float x1 = 0, x2 = 0, x3 = 0;
  float vert_angle[PTZ_VERT_ANGLE_READ_NUM] = {0};
  char  i = 0, j = 0, k = 0, m = 0;
  float tem = 0;
  float tem1 = 0;
  static float vert_cycle_angle_up;
  static float vert_cycle_angle_down;
  long int tem3 = 0;
  float down_diff_x1 = 0;
  float down_diff_x2 = 0;
  float tem4 = 0;  
  char flag;
  int  cycle_count_save = 0;
//  static float vert_angle_actual;
//  static float vert_angle_last;
  
  
  g_ptz.vert_as5047d.time_end = g_ptz.vert_as5047d.time_now;
  g_ptz.vert_as5047d.time_now = OSTimeGet();
  g_ptz.vert_as5047d.time = time_ticks(g_ptz.vert_as5047d.time_now, g_ptz.vert_as5047d.time_end);
  
#ifdef CLK_50M
       if(g_ptz.vert_as5047d.time < 2)
       {
         g_ptz.vert_as5047d.time = 2;
       }
#else
       if(g_ptz.vert_as5047d.time < 1)
       {
         g_ptz.vert_as5047d.time = 1;
       }
#endif
    
       
#ifdef PTZ_VERT_GET_ANGLE_MANY
    for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM; i++)
    {
       vert_angle[i] = as5047d_vert_get_angle_a();
       
//       test_angle[test_angle_count] = vert_angle[i];
//       test_angle_count ++;
//       if(test_angle_count >= 500)
//       {
//         test_angle_count = 0;
//       }
//       
       if(vert_angle[i] < 0 || //防止读出的角度为负值
          vert_angle[i] > 360.0||//防止读出的数值超过360
          isnan(vert_angle[i]) == 1)//防止读出的不是一个数
       {//以上错误的数据都该舍弃
         i --;
       }
    }
    
    //由于磁编码器存在0位，防止采集到的数据差距接近360度，对采集到的数据进行优化处理
    //例如0.2、0.1、359.9、359.8这种数据就需要处理
    flag = 0;
    for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM - 1; i++)
    {
      if(fabs(vert_angle[i] - vert_angle[i+1]) > 350.0)
      {
        flag = 1;//说明磁编码器处于0位附近，需要进行数据优化处理
        break;
      }
    }
    if(flag == 1)
    {
      for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM; i++)
      {
        if(vert_angle[i] < 5.0)
        {
          vert_angle[i] = vert_angle[i] + 360.0;
        }
      }
    }
    
    //对采集到的角度值排序
    for(j = 0; j < PTZ_VERT_ANGLE_READ_NUM-1; j++)//采样值由小到大排列
    {
       for(k = 0; k < PTZ_VERT_ANGLE_READ_NUM-j-1; k++)
       {
           if(vert_angle[k] > vert_angle[k+1])
           {
              tem             = vert_angle[k];
              vert_angle[k]   = vert_angle[k+1];
              vert_angle[k+1] = tem;
           }
        }
     }
    tem = 0;
    for(uint8_t i = 1; i < PTZ_VERT_ANGLE_READ_NUM - 1; i++)
    {
      tem = tem + vert_angle[i];//去掉一个最大值和一个最小值
    }
    tem = tem / ((float)(PTZ_VERT_ANGLE_READ_NUM - 2));
    
    if(tem > 360.0)
    {
      tem = tem - 360.0;
    }
    
    g_ptz.vert_as5047d.as5047d_angle_actual = tem;
#endif
    
#ifdef PTZ_VERT_GET_ANGLE_SINGLE
//    tem = as5047d_vert_get_angle_a();
//    
//    if(tem < 0 || //防止读出的角度为负值
//       tem > 360.0||//防止读出的数值超过360
//       isnan(tem) == 1)//防止读出的不是一个数
//       {//以上错误的数据都该舍弃,从新读取
//         return 0;
//       }
//    g_ptz.vert_as5047d.as5047d_angle_actual = tem;
    
    
    for(m = 0; m < 5; m++)
    {
      
      tem = as5047d_vert_get_angle_a();
      
      if(tem < 0          || //防止读出的角度为负值
         tem > 360.0      ||//防止读出的数值超过360
         isnan(tem) == 1  ||
         ((fabs(tem - g_ptz.vert_as5047d.as5047d_angle_last) > PTZ_VERT_ANGLE_DIFF_A) && (fabs(tem - g_ptz.vert_as5047d.as5047d_angle_last) < PTZ_VERT_ANGLE_DIFF_B)))//防止读出的不是一个数
      {//以上错误的数据都该舍弃,从新读取
       asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");
      }
      else
      {
        break;
      }
    }
    g_ptz.vert_as5047d.as5047d_angle_actual = tem;
    
    
#endif
    
    if(g_ptz.vert_as5047d.as5047d_data_reset == 1)
    {
      g_ptz.vert_as5047d.as5047d_cycle_count = 0;
      g_ptz.vert_as5047d.as5047d_cycle_num = 0;
      g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
      g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
      g_ptz.vert_as5047d.as5047d_data_reset = 0;
      g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
      tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
      vert_cycle_angle_up = tem1 + 350.0;
      vert_cycle_angle_down = tem1 + 10.0;
    }
    
    
    x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 
              g_ptz.vert_as5047d.as5047d_angle_last);
    
    x2 = (g_ptz.vert_as5047d.as5047d_angle_actual - 0) +
         (360.0 - g_ptz.vert_as5047d.as5047d_angle_last);
    
    x3 = (g_ptz.vert_as5047d.as5047d_angle_last - 0) +
         (360.0 - g_ptz.vert_as5047d.as5047d_angle_actual);
           
    //首先判断磁编码器角度判断磁编码器转向
    //磁编码器正转角度增大       
    if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_angle_last
       && x1 < x3)
    {
      g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_FWD;
    }
    
    if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_angle_last
       && x2 < x1)
    {
      g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_FWD;
    } 
    //磁编码器反转角度减小      
    if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_angle_last
       && x3 < x1)
    {
      g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_REV;
    }
    
    if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_angle_last
       && x1 < x2)
    {
      g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_REV;
    } 
    
    if(x1 == x2 || x1 == x3)
    {
      if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
      {
        g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_FWD;
      }
      if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
      {
        g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_REV;
      }
    }
    
    
    //反转，云台角度减小，磁编码角度增大
    if(g_ptz.vert_as5047d.as5047d_dir == AS5047D_DIR_FWD)
    {
#ifdef PTZ_PHOTOELECTRIC_SWITCH
      //挡片更新角度
      if(g_ptz.vert_as5047d.as5047d_state == 1)
      {
        if(g_ptz.vert_ps_sw1_up_rise > 0)
        {
          g_ptz.vert_ps_sw1_up_rise = 0;
        }
        if( g_ptz.vert_ps_sw1_down_fall > 0)
        {
#ifdef PTZ_SW1_DOWN_FALL_UPDATE
          ptz_vert_ps_delay_ms();
          if(PS_VERT_SW1_READ == PS_COVER)
          {
            g_ptz.vert_as5047d.as5047d_cycle_count = 0;
            g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
            g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
            g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
            g_ptz.vert_angle_state = 1;
            g_ptz.vert_ps_sw1_down_fall = 0; 
          }
#endif
          g_ptz.vert_ps_sw1_down_fall = 0;
        }
        
//       
        
        if(g_ptz.vert_ps_sw2_down_rise > 0)
        {
          g_ptz.vert_ps_sw2_down_rise = 0;
        }
        if(g_ptz.vert_ps_sw2_up_fall > 0)
        {
          g_ptz.vert_ps_sw2_up_fall = 0;
        }
      }
      
#endif
      
      tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
      vert_cycle_angle_up = tem1 + 350.0;
      vert_cycle_angle_down = tem1 + 10.0;
      //根据计算方法b，计算云台当前角度
      
      //判断磁编码器是否转完一圈
      
      //首先计算当前角度度和初始角度之间的角度距离
      down_diff_x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 
                          g_ptz.vert_as5047d.as5047d_ptz_init_angle);
      
      down_diff_x2 = fabs(360.0 - g_ptz.vert_as5047d.as5047d_ptz_init_angle) + 
                          fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 0);
      
      if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_ptz_init_angle
         && down_diff_x1 <= PTZ_DOWN_CYCLE_C
         && g_ptz.vert_as5047d.as5047d_ptz_angle_actual < vert_cycle_angle_down)
      {
        g_ptz.vert_as5047d.as5047d_cycle_count --;//转动圈数减少1
      }
      if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_ptz_init_angle
         && down_diff_x2 <= PTZ_DOWN_CYCLE_C
         && g_ptz.vert_as5047d.as5047d_ptz_angle_actual < vert_cycle_angle_down)
      {
        g_ptz.vert_as5047d.as5047d_cycle_count --;//转动圈数减少1
      }

    }
    
    
    //正转，云台角度增大，磁编码角度减小
    if(g_ptz.vert_as5047d.as5047d_dir == AS5047D_DIR_REV)
    {
#ifdef PTZ_PHOTOELECTRIC_SWITCH
      //挡片更新角度
      if(g_ptz.vert_as5047d.as5047d_state == 1)
      {
//        if(g_ptz.vert_ps_sw1_up_rise > 0)
//        {
//          g_ptz.vert_as5047d.as5047d_cycle_count = 0;
//          g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
//          g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
//          g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
//
//          g_ptz.vert_ps_sw1_up_rise = 0;
//        }
        if(g_ptz.vert_ps_sw1_up_rise > 0)
        {
#ifdef PTZ_SW1_UP_RISE_UPDATE         
          ptz_vert_ps_delay_ms();
          if(PS_VERT_SW1_READ == PS_NO_COVER)
          {
            g_ptz.vert_as5047d.as5047d_cycle_count = 0;
            g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
            g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
            g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
            g_ptz.vert_angle_state = 1;
            g_ptz.vert_ps_sw1_up_rise = 0; 
          }
#endif
          g_ptz.vert_ps_sw1_up_rise = 0;
        }
        if( g_ptz.vert_ps_sw1_down_fall > 0)
        {
          g_ptz.vert_ps_sw1_down_fall = 0;
        }
        
        if(g_ptz.vert_ps_sw2_down_rise > 0)
        {
          g_ptz.vert_ps_sw2_down_rise = 0;
        }
        
        if(g_ptz.vert_ps_sw2_up_fall > 0)
        {
          ptz_vert_ps_delay_ms();
          if(PS_VERT_SW2_READ == PS_COVER)
          {
            g_ptz.vert_as5047d.as5047d_cycle_count =  g_ptz.vert_as5047d.as5047d_cycle_num;
            g_ptz.vert_as5047d.as5047d_ptz_angle_actual =  g_ptz.vert_as5047d.as5047d_ptz_angle_max;
            g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
            g_ptz.vert_as5047d.as5047d_ptz_init_angle = 
            g_ptz.vert_as5047d.as5047d_angle_actual + g_ptz.vert_as5047d.as5047d_remain_angle;
            if(g_ptz.vert_as5047d.as5047d_ptz_init_angle >= 360.0)
            {
              g_ptz.vert_as5047d.as5047d_ptz_init_angle = 
                g_ptz.vert_as5047d.as5047d_ptz_init_angle - 360.0;
            }
            g_ptz.vert_angle_state = 1;
            g_ptz.vert_ps_sw2_up_fall = 0;
          }
          g_ptz.vert_ps_sw2_up_fall = 0;
        }
      }
      
#endif
     
      tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
      vert_cycle_angle_up = tem1 + 350.0;
      vert_cycle_angle_down = tem1 + 10.0;
      //根据计算方法b，计算云台当前角度
      
      //判断磁编码器是否转完一圈
      
      //首先计算当前角度度和初始角度之间的角度距离
      up_diff_x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 
                          g_ptz.vert_as5047d.as5047d_ptz_init_angle);
      
      up_diff_x2 = fabs(360.0 - g_ptz.vert_as5047d.as5047d_angle_actual) + 
                          fabs(g_ptz.vert_as5047d.as5047d_ptz_init_angle - 0);
      
      if(g_ptz.vert_as5047d.as5047d_angle_actual <= g_ptz.vert_as5047d.as5047d_ptz_init_angle
         && up_diff_x1 <= PTZ_UP_CYCLE_C
         && g_ptz.vert_as5047d.as5047d_ptz_angle_actual > vert_cycle_angle_up)
      {
        g_ptz.vert_as5047d.as5047d_cycle_count ++;//转动圈数增加1
      }
      if(g_ptz.vert_as5047d.as5047d_angle_actual >= g_ptz.vert_as5047d.as5047d_ptz_init_angle
         && up_diff_x2 <= PTZ_UP_CYCLE_C
         && g_ptz.vert_as5047d.as5047d_ptz_angle_actual > vert_cycle_angle_up)
      {
        g_ptz.vert_as5047d.as5047d_cycle_count ++;//转动圈数增加1
      }
    }
   
    //保存当前磁编码器圈数
    cycle_count_save = g_ptz.vert_as5047d.as5047d_cycle_count;
    
    for(i = 0; i < 4; i++)
    {//这个循环的目的是为了防止磁编码器圈数计算错误，恢复正常的磁编码圈数 
          //计算当前角度  
          if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_ptz_init_angle)
          {
            g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual = 
              fabs(360.0 - g_ptz.vert_as5047d.as5047d_angle_actual) +
                fabs(g_ptz.vert_as5047d.as5047d_ptz_init_angle - 0) +
                  g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;

          }
          if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_ptz_init_angle)
          {
            g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual = 
              (g_ptz.vert_as5047d.as5047d_ptz_init_angle - 
                 g_ptz.vert_as5047d.as5047d_angle_actual) + 
                   g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
          }
          if(g_ptz.vert_as5047d.as5047d_angle_actual == g_ptz.vert_as5047d.as5047d_ptz_init_angle)
          {
            g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
          }
          
          //对算出的磁编码器总角度进行滤波处理
          if(fabs(g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last) 
             < PTZ_VERT_AS5047D_ANGLE_WF_A)
          {
            g_ptz.vert_as5047d.as5047d_ptz_angle_actual = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
            g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
            break;
          }
          else
          {
             //根据判断，恢复实际的磁编码器圈数
                switch(i)
                {
                  case 0:
                    g_ptz.vert_as5047d.as5047d_cycle_count = cycle_count_save;
                    g_ptz.vert_as5047d.as5047d_cycle_count --;//转动圈数减少1
                    break;
                      
                  case 1:
                    g_ptz.vert_as5047d.as5047d_cycle_count = cycle_count_save;
                    g_ptz.vert_as5047d.as5047d_cycle_count ++;//转动圈数增加1
                    break;

                  case 2: //磁编码器实际圈数恢复失败
                    g_ptz.vert_as5047d.as5047d_cycle_count = cycle_count_save;
                    break; 
                    
                  case 3:
                    g_ptz.vert_as5047d.as5047d_ptz_angle_actual = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
                    g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
                    break;
                }
          }
    }

    if(g_ptz.vert_as5047d.as5047d_state == 1)
    {
      //对云台角度进行四舍五入,保留小数点后3位小数
      tem3 = (long int)((g_ptz.vert_as5047d.as5047d_ptz_angle_K * 
              (g_ptz.vert_as5047d.as5047d_ptz_angle_actual -
                g_ptz.vert_as5047d.as5047d_ptz_angle_max / 2.0)) * 1000.0 + 0.5);
      
      
      tem4 = tem3 / 1000.0;
      g_ptz.vert_angle_actual_a = tem4;
      if(g_ptz.offset_angle.vert_offset_switch == 2)
      {
        tem4 = tem4 - g_ptz.offset_angle.vert_zero_offset_angle;
      }
      g_ptz.vert_angle_actual = tem4;
      
      
    }
    
    //保存当前磁编码器的值以及云台角度值
    g_ptz.vert_as5047d.as5047d_angle_last = g_ptz.vert_as5047d.as5047d_angle_actual;
    g_ptz.vert_as5047d.as5047d_ptz_angle_last = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
    g_ptz.vert_angle_last   = g_ptz.vert_angle_actual;
    return 1;
}

#endif


/// 计算云台水平角度
#ifdef PTZ_HORI_ANGLE_AS5047D_ANGLE_SYN
static char ptz_hori_get_angle()
{
  float right_diff_x1 = 0;
  float right_diff_x2 = 0;
  
  float hori_angle[PTZ_HORI_ANGLE_READ_NUM] = {0};
  char  i = 0, j = 0, k = 0;
  float tem = 0;
  float tem1 = 0;
  float tem2 = 0;
  float left_diff_x1 = 0;
  float left_diff_x2 = 0;
  unsigned int tem3 = 0;
  float tem4 = 0;
  static float hori_cycle_angle_right;
  static float hori_cycle_angle_left;
  char flag;
#ifdef PTZ_HORI_GET_ANGLE_MANY
    for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM; i++)
    {
       hori_angle[i] = as5047d_hori_get_angle_a();
       if(hori_angle[i] < 0 || //防止读出的角度为负值
          hori_angle[i] > 360.0||//防止读出的数值超过360
          isnan(hori_angle[i]) == 1)//防止读出的不是一个数
       {//以上错误的数据都该舍弃
         i --;
       }
    }
    
    //由于磁编码器存在0位，防止采集到的数据差距接近360度，对采集到的数据进行优化处理
    //例如0.2、0.1、359.9、359.8这种数据就需要处理
    flag = 0;
    for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM - 1; i++)
    {
      if(fabs(hori_angle[i] - hori_angle[i+1]) > 350.0)
      {
        flag = 1;//说明磁编码器处于0位附近，需要进行数据优化处理
        break;
      }
    }
    if(flag == 1)
    {
      for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM; i++)
      {
        if(hori_angle[i] < 5.0)
        {
          hori_angle[i] = hori_angle[i] + 360.0;
        }
      }
    }
    
    //对采集到的角度值排序
    for(j = 0; j < PTZ_HORI_ANGLE_READ_NUM-1; j++)//采样值由小到大排列
    {
       for(k = 0; k < PTZ_HORI_ANGLE_READ_NUM-j-1; k++)
       {
           if(hori_angle[k] > hori_angle[k+1])
           {
              tem             = hori_angle[k];
              hori_angle[k]   = hori_angle[k+1];
              hori_angle[k+1] = tem;
           }
        }
     }
    tem = 0;
    for(uint8_t i = 1; i < PTZ_HORI_ANGLE_READ_NUM - 1; i++)
    {
      tem = tem + hori_angle[i];//去掉一个最大值和一个最小值
    }
    tem = tem / ((float)(PTZ_HORI_ANGLE_READ_NUM - 2));
    
    if(tem > 360.0)
    {
      tem = tem - 360.0;
    }
    
    g_ptz.hori_as5047d.as5047d_angle_actual = tem;
#endif
    
#ifdef PTZ_HORI_GET_ANGLE_SINGLE
    tem = as5047d_hori_get_angle_a();
    
    if(tem < 0 || //防止读出的角度为负值
       tem > 360.0||//防止读出的数值超过360
       isnan(tem) == 1)//防止读出的不是一个数
       {//以上错误的数据都该舍弃,从新读取
         return 0;
       }
    g_ptz.hori_as5047d.as5047d_angle_actual = tem;
#endif
    if(g_ptz.hori_as5047d.as5047d_data_reset == 1)
    {
      g_ptz.hori_as5047d.as5047d_cycle_count = 0;
      g_ptz.hori_as5047d.as5047d_cycle_num = 0;
      g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
      g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0;
      g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
      g_ptz.hori_as5047d.as5047d_data_reset = 0;
      tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
      hori_cycle_angle_right = tem1 + 350.0;
      hori_cycle_angle_left = tem1 + 10.0;
    }
    //反转，云台角度减小，磁编码角度减小
    if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
    {
#ifdef PTZ_PHOTOELECTRIC_SWITCH
      //挡片更新角度
      if(g_ptz.hori_as5047d.as5047d_state == 1)
      {
//        if(g_ptz.hori_ps_sw3_right_fall >= 1)
//        {
//          if(PS_HORI_SW3_READ == PS_COVER)
//          {
//            g_ptz.hori_as5047d.as5047d_cycle_count = 0;
//            g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
//            g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0;
//            g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
//            g_ptz.hori_ps_sw3_right_fall = 0;
//          }
//        }
        if(g_ptz.hori_ps_sw3_left_rise >= 1)
        {
#ifdef PTZ_SW3_LEFT_RISE_UPDATE 
          ptz_hori_ps_delay_ms();
          if(PS_HORI_SW3_READ == PS_NO_COVER)
          {
            g_ptz.hori_as5047d.as5047d_cycle_count =  g_ptz.hori_as5047d.as5047d_cycle_num;
            g_ptz.hori_as5047d.as5047d_ptz_angle_actual =  g_ptz.hori_as5047d.as5047d_ptz_angle_max;
            g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
            g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
              g_ptz.hori_as5047d.as5047d_angle_actual - g_ptz.hori_as5047d.as5047d_remain_angle;
            if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
            {
              g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
                g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
            }
            g_ptz.hori_angle_state = 1;
            g_ptz.hori_ps_sw3_left_rise = 0;
          }
#endif
          g_ptz.hori_ps_sw3_left_rise = 0;
        }
        
        if(g_ptz.hori_ps_sw3_right_fall >= 1)
        {
          g_ptz.hori_ps_sw3_right_fall = 0;
        }
        if(g_ptz.hori_ps_sw3_right_rise >= 1)
        {
          g_ptz.hori_ps_sw3_right_rise = 0;
        }
        if(g_ptz.hori_ps_sw3_left_fall >= 1)
        {
          g_ptz.hori_ps_sw3_left_fall = 0;
        }
      }
      
#endif
      
      tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
      hori_cycle_angle_right = tem1 + 350.0;
      hori_cycle_angle_left = tem1 + 10.0;
      //根据计算方法b，计算云台当前角度
      
      //判断磁编码器是否转完一圈
      
      //首先计算当前角度度和初始角度之间的角度距离
      left_diff_x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 
                          g_ptz.hori_as5047d.as5047d_ptz_init_angle);
      
      left_diff_x2 = fabs(360.0 - g_ptz.hori_as5047d.as5047d_angle_actual) + 
                          fabs(g_ptz.hori_as5047d.as5047d_ptz_init_angle - 0);
      
      if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_ptz_init_angle
         && left_diff_x1 <= PTZ_LEFT_CYCLE_C
         && g_ptz.hori_as5047d.as5047d_ptz_angle_actual < hori_cycle_angle_left)
      {
        g_ptz.hori_as5047d.as5047d_cycle_count --;//转动圈数减少1
      }
      
      if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_init_angle
         && left_diff_x2 <= PTZ_LEFT_CYCLE_C
         && g_ptz.hori_as5047d.as5047d_ptz_angle_actual < hori_cycle_angle_left)
      {
        g_ptz.hori_as5047d.as5047d_cycle_count --;//转动圈数减少1
      }
    }
    
    
    //正转，云台角度增大，磁编码角度增大
    if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
    {
#ifdef PTZ_PHOTOELECTRIC_SWITCH
      //挡片更新角度
      if(g_ptz.hori_as5047d.as5047d_state == 1)
      {
//        if(g_ptz.hori_ps_sw3_left_rise >= 1)
//        {
//          if(PS_HORI_SW3_READ == PS_NO_COVER)
//          {
//            g_ptz.hori_as5047d.as5047d_cycle_count =  g_ptz.hori_as5047d.as5047d_cycle_num;
//            g_ptz.hori_as5047d.as5047d_ptz_angle_actual =  g_ptz.hori_as5047d.as5047d_ptz_angle_max;
//            g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
//            g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
//              g_ptz.hori_as5047d.as5047d_angle_actual - g_ptz.hori_as5047d.as5047d_remain_angle;
//            if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
//            {
//              g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
//                g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
//            }
//            g_ptz.hori_ps_sw3_left_rise = 0;
//          }
//        }
        if(g_ptz.hori_ps_sw3_right_fall >= 1)
        {
          ptz_hori_ps_delay_ms();
          if(PS_HORI_SW3_READ == PS_COVER)
          {
            g_ptz.hori_as5047d.as5047d_cycle_count = 0;
            g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
            g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0;
            g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
            g_ptz.hori_ps_sw3_right_fall = 0;
            g_ptz.hori_angle_state = 1;
          }
          g_ptz.hori_ps_sw3_right_fall = 0;
        }
        
        if(g_ptz.hori_ps_sw3_left_rise >= 1)
        {
          g_ptz.hori_ps_sw3_left_rise = 0;
        }
        if(g_ptz.hori_ps_sw3_right_rise >= 1)
        {
          g_ptz.hori_ps_sw3_right_rise = 0;
        }
        if(g_ptz.hori_ps_sw3_left_fall >= 1)
        {
          g_ptz.hori_ps_sw3_left_fall = 0;
        }
      }
      
#endif
      
      tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
      hori_cycle_angle_right = tem1 + 350.0;
      hori_cycle_angle_left = tem1 + 10.0;
      
      //根据计算方法b，计算云台当前角度
      
      //判断磁编码器是否转完一圈
      
      //首先计算当前角度度和初始角度之间的角度距离
      right_diff_x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 
                          g_ptz.hori_as5047d.as5047d_ptz_init_angle);
      
      right_diff_x2 = fabs(360.0 - g_ptz.hori_as5047d.as5047d_ptz_init_angle) + 
                          fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 0);
      
      if(g_ptz.hori_as5047d.as5047d_angle_actual >= g_ptz.hori_as5047d.as5047d_ptz_init_angle
         && right_diff_x1 <= PTZ_RIGHT_CYCLE_C
         && g_ptz.hori_as5047d.as5047d_ptz_angle_actual > hori_cycle_angle_right)
      {
        g_ptz.hori_as5047d.as5047d_cycle_count ++;//转动圈数增加1
      }
     
      if(g_ptz.hori_as5047d.as5047d_angle_actual <= g_ptz.hori_as5047d.as5047d_ptz_init_angle
         && right_diff_x2 <= PTZ_RIGHT_CYCLE_C
         && g_ptz.hori_as5047d.as5047d_ptz_angle_actual > hori_cycle_angle_right)
      {
        g_ptz.hori_as5047d.as5047d_cycle_count ++;//转动圈数增加1
      }
    }
   
    
    //计算当前角度  
    if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_init_angle)
    {
      g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual =
        (g_ptz.hori_as5047d.as5047d_angle_actual - 
          g_ptz.hori_as5047d.as5047d_ptz_init_angle) + 
            g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
    }
    if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_ptz_init_angle)
    {
      g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual =
        fabs(360.0 - g_ptz.hori_as5047d.as5047d_ptz_init_angle) +
          fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 0) +
            g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
    }
    if(g_ptz.hori_as5047d.as5047d_angle_actual == g_ptz.hori_as5047d.as5047d_ptz_init_angle)
    {
      g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
    }
    
    //对磁编码器角度进行滤波
    if((fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last) 
        < PTZ_HORI_AS5047D_ANGLE_WF_A) ||
       (fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last) 
        > PTZ_HORI_AS5047D_ANGLE_WF_B))
    {
      g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
      g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
    }
    
    //防止角度范围超过限度
    if(g_ptz.hori_as5047d.as5047d_state == 1)
    {
      if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_angle_max)
      {
        tem2 = fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_max);
        g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 
          g_ptz.hori_as5047d.as5047d_ptz_angle_actual -
            g_ptz.hori_as5047d.as5047d_ptz_angle_max;
        g_ptz.hori_as5047d.as5047d_cycle_count = 0;
        g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual - tem2;
        if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
        {
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
            g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
        }
      }
      
      if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual < 0)
      {
        tem2 = fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_actual - 0);
        g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 
          g_ptz.hori_as5047d.as5047d_ptz_angle_actual +
            g_ptz.hori_as5047d.as5047d_ptz_angle_max;
        
        g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
        
        g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
          g_ptz.hori_as5047d.as5047d_angle_actual - g_ptz.hori_as5047d.as5047d_remain_angle + tem2;
        if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
        {
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
            g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
        }
        if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
        {
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
            g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
        }
      }
      if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual ==  g_ptz.hori_as5047d.as5047d_ptz_angle_max
         || g_ptz.hori_as5047d.as5047d_ptz_angle_actual == 0)
      {
        if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
        {
          g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
          g_ptz.hori_as5047d.as5047d_cycle_count = 0;
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
        }
        else
        {
          g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
          g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
          g_ptz.hori_as5047d.as5047d_angle_actual - g_ptz.hori_as5047d.as5047d_remain_angle;
          if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
          {
            g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
              g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
          }
        }
      }
      //对云台角度进行四舍五入,保留小数点后3位小数
      tem3 = (unsigned int)((g_ptz.hori_as5047d.as5047d_ptz_angle_actual * 
        g_ptz.hori_as5047d.as5047d_ptz_angle_K) * 1000.0 + 0.5);
      //计算云台角度
      tem4 = tem3 / 1000.0;
      g_ptz.hori_angle_actual_a = tem4;
      if(g_ptz.offset_angle.hori_offset_switch == 2)
      {
        tem4 = tem4 - g_ptz.offset_angle.hori_zero_offset_angle;
        if(tem4 < 0)
        {
          tem4 = tem4 + 360.0;
        }
      }
      g_ptz.hori_angle_actual = tem4;
    }
    
    //保存当前磁编码器的值以及云台角度值
    g_ptz.hori_as5047d.as5047d_angle_last = g_ptz.hori_as5047d.as5047d_angle_actual;
    g_ptz.hori_as5047d.as5047d_ptz_angle_last = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
    g_ptz.hori_angle_last   = g_ptz.hori_angle_actual;
    return 1;
}

static char ptz_hori_get_angle_a()
{
  float right_diff_x1 = 0;
  float right_diff_x2 = 0;
  float x1 = 0, x2 = 0, x3 = 0;
  float hori_angle[PTZ_HORI_ANGLE_READ_NUM] = {0};
  char  i = 0, j = 0, k = 0, m = 0;
  float tem = 0;
  float tem1 = 0;
  float tem2 = 0;
  float left_diff_x1 = 0;
  float left_diff_x2 = 0;
  unsigned int tem3 = 0;
  float tem4 = 0;
  static float hori_cycle_angle_right;
  static float hori_cycle_angle_left;
  char flag;
  int  cycle_count_save = 0;
  
 
  
  g_ptz.hori_as5047d.time_end = g_ptz.hori_as5047d.time_now;
  g_ptz.hori_as5047d.time_now = OSTimeGet();
  g_ptz.hori_as5047d.time = time_ticks(g_ptz.hori_as5047d.time_now, g_ptz.hori_as5047d.time_end);
  
#ifdef CLK_50M
       if(g_ptz.hori_as5047d.time < 2)
       {
         g_ptz.hori_as5047d.time = 2;
       }
#else
       if(g_ptz.hori_as5047d.time < 1)
       {
         g_ptz.hori_as5047d.time = 1;
       }
#endif
       
#ifdef PTZ_HORI_GET_ANGLE_MANY
    for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM; i++)
    {
       hori_angle[i] = as5047d_hori_get_angle_a();
       if(hori_angle[i] < 0 || //防止读出的角度为负值
          hori_angle[i] > 360.0||//防止读出的数值超过360
          isnan(hori_angle[i]) == 1)//防止读出的不是一个数
       {//以上错误的数据都该舍弃
         i --;
       }
    }
    
    //由于磁编码器存在0位，防止采集到的数据差距接近360度，对采集到的数据进行优化处理
    //例如0.2、0.1、359.9、359.8这种数据就需要处理
    flag = 0;
    for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM - 1; i++)
    {
      if(fabs(hori_angle[i] - hori_angle[i+1]) > 350.0)
      {
        flag = 1;//说明磁编码器处于0位附近，需要进行数据优化处理
        break;
      }
    }
    if(flag == 1)
    {
      for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM; i++)
      {
        if(hori_angle[i] < 5.0)
        {
          hori_angle[i] = hori_angle[i] + 360.0;
        }
      }
    }
    
    //对采集到的角度值排序
    for(j = 0; j < PTZ_HORI_ANGLE_READ_NUM-1; j++)//采样值由小到大排列
    {
       for(k = 0; k < PTZ_HORI_ANGLE_READ_NUM-j-1; k++)
       {
           if(hori_angle[k] > hori_angle[k+1])
           {
              tem             = hori_angle[k];
              hori_angle[k]   = hori_angle[k+1];
              hori_angle[k+1] = tem;
           }
        }
     }
    tem = 0;
    for(uint8_t i = 1; i < PTZ_HORI_ANGLE_READ_NUM - 1; i++)
    {
      tem = tem + hori_angle[i];//去掉一个最大值和一个最小值
    }
    tem = tem / ((float)(PTZ_HORI_ANGLE_READ_NUM - 2));
    
    if(tem > 360.0)
    {
      tem = tem - 360.0;
    }
    
    g_ptz.hori_as5047d.as5047d_angle_actual = tem;
#endif
    
#ifdef PTZ_HORI_GET_ANGLE_SINGLE
    for(m = 0; m < 5; m++)
    {
      
      tem = as5047d_hori_get_angle_a();
      
      if(tem < 0          || //防止读出的角度为负值
         tem > 360.0      ||//防止读出的数值超过360
         isnan(tem) == 1  ||//防止读出的不是一个数
         ((fabs(tem - g_ptz.hori_as5047d.as5047d_angle_last) > PTZ_HORI_ANGLE_DIFF_A) && (fabs(tem - g_ptz.hori_as5047d.as5047d_angle_last) < PTZ_HORI_ANGLE_DIFF_B)))
      {//以上错误的数据都该舍弃,从新读取
       asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");
      }
      else
      {
        break;
      }
    }
    g_ptz.hori_as5047d.as5047d_angle_actual = tem;
    
    
//     tem = as5047d_hori_get_angle_a();
//    
//    if(tem < 0 || //防止读出的角度为负值
//       tem > 360.0||//防止读出的数值超过360
//       isnan(tem) == 1)//防止读出的不是一个数
//       {//以上错误的数据都该舍弃,从新读取
//         return 0;
//       }
//    g_ptz.hori_as5047d.as5047d_angle_actual = tem;
//    
#endif
    
        
    
    if(g_ptz.hori_as5047d.as5047d_data_reset == 1)
    {
      g_ptz.hori_as5047d.as5047d_cycle_count = 0;
      g_ptz.hori_as5047d.as5047d_cycle_num = 0;
      g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
      g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0;
      g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
      g_ptz.hori_as5047d.as5047d_data_reset = 0;
      tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
      hori_cycle_angle_right = tem1 + 350.0;
      hori_cycle_angle_left = tem1 + 10.0;
    }
    
    
    x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 
              g_ptz.hori_as5047d.as5047d_angle_last);
    
    x2 = (g_ptz.hori_as5047d.as5047d_angle_actual - 0) +
         (360.0 - g_ptz.hori_as5047d.as5047d_angle_last);
    
    x3 = (g_ptz.hori_as5047d.as5047d_angle_last - 0) +
         (360.0 - g_ptz.hori_as5047d.as5047d_angle_actual);
           
    //首先判断磁编码器角度判断磁编码器转向
    //磁编码器正转角度增大       
    if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_angle_last
       && x1 < x3)
    {
      g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_FWD;
    }
    
    if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_angle_last
       && x2 < x1)
    {
      g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_FWD;
    } 
    //磁编码器反转角度减小      
    if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_angle_last
       && x3 < x1)
    {
      g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_REV;
    }
    
    if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_angle_last
       && x1 < x2)
    {
      g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_REV;
    } 
    
    if(x1 == x2 || x1 == x3)
    {
      if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
      {
        g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_REV;
      }
      if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
      {
        g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_FWD;
      }
    }
    
    
    
    
    
     
    
    //反转，云台角度减小，磁编码角度减小
    if(g_ptz.hori_as5047d.as5047d_dir == AS5047D_DIR_REV)
    {
#ifdef PTZ_PHOTOELECTRIC_SWITCH
      //挡片更新角度
      if(g_ptz.hori_as5047d.as5047d_state == 1)
      {
//        if(g_ptz.hori_ps_sw3_right_fall >= 1)
//        {
//          if(PS_HORI_SW3_READ == PS_COVER)
//          {
//            g_ptz.hori_as5047d.as5047d_cycle_count = 0;
//            g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
//            g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0;
//            g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
//            g_ptz.hori_ps_sw3_right_fall = 0;
//          }
//        }
        if(g_ptz.hori_ps_sw3_left_rise >= 1)
        {
#ifdef PTZ_SW3_LEFT_RISE_UPDATE

            if(g_ptz.hori_angle_erro_switch == 0)
          {//误差消除开关关闭，使能左转上升沿角度更新，，打开则失能
            ptz_hori_ps_delay_ms();
            if(PS_HORI_SW3_READ == PS_NO_COVER)
            {
              g_ptz.hori_as5047d.as5047d_cycle_count =  g_ptz.hori_as5047d.as5047d_cycle_num;
              g_ptz.hori_as5047d.as5047d_ptz_angle_actual =  g_ptz.hori_as5047d.as5047d_ptz_angle_max;
              g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
              g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
                g_ptz.hori_as5047d.as5047d_angle_actual - g_ptz.hori_as5047d.as5047d_remain_angle;
              if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
              {
                g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
                  g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
              }
              g_ptz.hori_angle_state = 1;
              g_ptz.hori_ps_sw3_left_rise = 0;
              ptz_printf("\n hori_ps_sw3_left_rise angle max update!! \r\n",strlen((const char *)"\n hori_ps_sw3_left_rise angle max update!! \r\n"));
            }
          }
#endif
          g_ptz.hori_ps_sw3_left_rise = 0;
        }
        
        if(g_ptz.hori_ps_sw3_right_fall >= 1)
        {
          g_ptz.hori_ps_sw3_right_fall = 0;
        }
        if(g_ptz.hori_ps_sw3_right_rise >= 1)
        {
          g_ptz.hori_ps_sw3_right_rise = 0;
        }
        if(g_ptz.hori_ps_sw3_left_fall >= 1)
        {
          g_ptz.hori_ps_sw3_left_fall = 0;
        }
      }
      
#endif
           
      tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
      hori_cycle_angle_right = tem1 + 350.0;
      hori_cycle_angle_left = tem1 + 10.0;
      //根据计算方法b，计算云台当前角度
      
      //判断磁编码器是否转完一圈
      
      //首先计算当前角度度和初始角度之间的角度距离
      left_diff_x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 
                          g_ptz.hori_as5047d.as5047d_ptz_init_angle);
      
      left_diff_x2 = fabs(360.0 - g_ptz.hori_as5047d.as5047d_angle_actual) + 
                          fabs(g_ptz.hori_as5047d.as5047d_ptz_init_angle - 0);
      
      if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_ptz_init_angle
         && left_diff_x1 <= PTZ_LEFT_CYCLE_C
         && g_ptz.hori_as5047d.as5047d_ptz_angle_actual < hori_cycle_angle_left)
      {
        g_ptz.hori_as5047d.as5047d_cycle_count --;//转动圈数减少1
      }
      
      if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_init_angle
         && left_diff_x2 <= PTZ_LEFT_CYCLE_C
         && g_ptz.hori_as5047d.as5047d_ptz_angle_actual < hori_cycle_angle_left)
      {
        g_ptz.hori_as5047d.as5047d_cycle_count --;//转动圈数减少1
      }
    }

    //正转，云台角度增大，磁编码角度增大
    if(g_ptz.hori_as5047d.as5047d_dir == AS5047D_DIR_FWD)
    {
#ifdef PTZ_PHOTOELECTRIC_SWITCH
      //挡片更新角度
      if(g_ptz.hori_as5047d.as5047d_state == 1)
      {
//        if(g_ptz.hori_ps_sw3_left_rise >= 1)
//        {
//          if(PS_HORI_SW3_READ == PS_NO_COVER)
//          {
//            g_ptz.hori_as5047d.as5047d_cycle_count =  g_ptz.hori_as5047d.as5047d_cycle_num;
//            g_ptz.hori_as5047d.as5047d_ptz_angle_actual =  g_ptz.hori_as5047d.as5047d_ptz_angle_max;
//            g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
//            g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
//              g_ptz.hori_as5047d.as5047d_angle_actual - g_ptz.hori_as5047d.as5047d_remain_angle;
//            if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
//            {
//              g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
//                g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
//            }
//            g_ptz.hori_ps_sw3_left_rise = 0;
//          }
//        }
        if(g_ptz.hori_ps_sw3_right_fall >= 1)
        {
          ptz_hori_ps_delay_ms();
          if(PS_HORI_SW3_READ == PS_COVER)
          {
            g_ptz.hori_as5047d.as5047d_cycle_count = 0;
            g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
            g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0;
            g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
            g_ptz.hori_ps_sw3_right_fall = 0;
            g_ptz.hori_angle_state = 1;
//            term_printf(" hori_ps_sw3_right_fall  update angle \r\n");
            ptz_printf("\n hori_ps_sw3_right_fall angle min update!! \r\n",strlen((const char *)"\n hori_ps_sw3_right_fall angle min update!! \r\n"));
          }
          g_ptz.hori_ps_sw3_right_fall = 0;
        }
        
        if(g_ptz.hori_ps_sw3_left_rise >= 1)
        {
          g_ptz.hori_ps_sw3_left_rise = 0;
        }
        if(g_ptz.hori_ps_sw3_right_rise >= 1)
        {
          g_ptz.hori_ps_sw3_right_rise = 0;
        }
        if(g_ptz.hori_ps_sw3_left_fall >= 1)
        {
          g_ptz.hori_ps_sw3_left_fall = 0;
        }
      }
      
#endif
      tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
      hori_cycle_angle_right = tem1 + 350.0;
      hori_cycle_angle_left = tem1 + 10.0;
      
      //根据计算方法b，计算云台当前角度
      
      //判断磁编码器是否转完一圈
      
      //首先计算当前角度度和初始角度之间的角度距离
      right_diff_x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 
                          g_ptz.hori_as5047d.as5047d_ptz_init_angle);
      
      right_diff_x2 = fabs(360.0 - g_ptz.hori_as5047d.as5047d_ptz_init_angle) + 
                          fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 0);
      
      if(g_ptz.hori_as5047d.as5047d_angle_actual >= g_ptz.hori_as5047d.as5047d_ptz_init_angle
         && right_diff_x1 <= PTZ_RIGHT_CYCLE_C
         && g_ptz.hori_as5047d.as5047d_ptz_angle_actual > hori_cycle_angle_right)
      {
        g_ptz.hori_as5047d.as5047d_cycle_count ++;//转动圈数增加1
      }
     
      if(g_ptz.hori_as5047d.as5047d_angle_actual <= g_ptz.hori_as5047d.as5047d_ptz_init_angle
         && right_diff_x2 <= PTZ_RIGHT_CYCLE_C
         && g_ptz.hori_as5047d.as5047d_ptz_angle_actual > hori_cycle_angle_right)
      {
        g_ptz.hori_as5047d.as5047d_cycle_count ++;//转动圈数增加1
      }
    }
   
    //保存当前磁编码器圈数
    cycle_count_save = g_ptz.hori_as5047d.as5047d_cycle_count;

    for(i = 0; i < 4; i++)
    {//这个循环的目的是为了防止磁编码器圈数计算错误，恢复正常的磁编码圈数   
        //计算当前角度  
        if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_init_angle)
        {
          g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual =
            (g_ptz.hori_as5047d.as5047d_angle_actual - 
              g_ptz.hori_as5047d.as5047d_ptz_init_angle) +
                g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
        }
        if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_ptz_init_angle)
        {
          g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual =
            fabs(360.0 - g_ptz.hori_as5047d.as5047d_ptz_init_angle) +
              fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 0) +
                g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
        }
        if(g_ptz.hori_as5047d.as5047d_angle_actual == g_ptz.hori_as5047d.as5047d_ptz_init_angle)
        {
          g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
        }

        //对磁编码器角度进行滤波
        if((fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last) 
            < PTZ_HORI_AS5047D_ANGLE_WF_A) ||
           (fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last) 
            > PTZ_HORI_AS5047D_ANGLE_WF_B))
        {
          g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
          g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
          break;
        }
        else
        {
              //根据判断，恢复实际的磁编码器圈数
              switch(i)
              {
                case 0:
                  g_ptz.hori_as5047d.as5047d_cycle_count = cycle_count_save - 1;//转动圈数减少1
                  g_ptz.hori_jia ++;
                  break;
                    
                case 1:
                  g_ptz.hori_as5047d.as5047d_cycle_count = cycle_count_save + 1;//转动圈数增加1
                  g_ptz.hori_jian ++;
                  break;

                case 2: //磁编码器实际圈数恢复失败
                  g_ptz.hori_as5047d.as5047d_cycle_count = cycle_count_save;
                  g_ptz.hori_cuowu ++;
                  break; 
                  
                case 3:
                  g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
                  g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
                  break;
              }
        }
    }
   
       
    //防止角度范围超过限度
    if(g_ptz.hori_as5047d.as5047d_state == 1)
    {
      if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_angle_max)
      {
        tem2 = fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_max);
        
        g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_max;
        
        g_ptz.hori_as5047d.as5047d_cycle_count = (int)(g_ptz.hori_as5047d.as5047d_ptz_angle_actual / 360.0);
        
        g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual - tem2;
        if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
        {
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
        }
      }
      
      if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual < 0)
      {
        tem2 = fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_actual - 0);
        
        g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_actual + g_ptz.hori_as5047d.as5047d_ptz_angle_max;
        
        g_ptz.hori_as5047d.as5047d_cycle_count = (int)(g_ptz.hori_as5047d.as5047d_ptz_angle_actual / 360.0);
        
        g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual - g_ptz.hori_as5047d.as5047d_remain_angle + tem2;
        if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
        {
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
            g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
        }
        if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
        {
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
            g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
        }
      }
      if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual ==  g_ptz.hori_as5047d.as5047d_ptz_angle_max
         || g_ptz.hori_as5047d.as5047d_ptz_angle_actual == 0)
      {
        if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
        {
          g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
          g_ptz.hori_as5047d.as5047d_cycle_count = 0;
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
        }
        else//左
        {
          g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
          g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
          g_ptz.hori_as5047d.as5047d_angle_actual - g_ptz.hori_as5047d.as5047d_remain_angle;
          if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
          {
            g_ptz.hori_as5047d.as5047d_ptz_init_angle = 
              g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
          }
        }
      }
      
     
      
      
      //对云台角度进行四舍五入,保留小数点后3位小数
      tem3 = (unsigned int)((g_ptz.hori_as5047d.as5047d_ptz_angle_actual * 
        g_ptz.hori_as5047d.as5047d_ptz_angle_K) * 1000.0 + 0.5);
      //计算云台角度
      tem4 = tem3 / 1000.0;
      
      //误差消除开关打开
      if(g_ptz.hori_angle_erro_switch == 1)
      {
        tem4 = tem4 + (g_ptz.offset_angle.offset_anle / 50.0);//消除误差,,,,使用误差消除时需要关闭光电开关左转上升沿角度更新
      }
      
      g_ptz.hori_angle_actual_a = tem4;
      
      if(g_ptz.offset_angle.hori_offset_switch == 2)
      {
        tem4 = tem4 - g_ptz.offset_angle.hori_zero_offset_angle;
//        if(tem4 < 0)
//        {
//          tem4 = tem4 + 360.0;
//        }
      }
      if(tem4 == 360.0)///////解决既显示360又显示角度0的问题
      {
        tem4 = 0;
      }
      if(tem4 < 0)
      {
        tem4 = tem4 + 360.0;
      }
      if(tem4>360.0)
      {
        tem4 = tem4 - 360.0;
      }
      g_ptz.hori_angle_actual = tem4;
      
      
//      g_ptz.hori_angle_actual_a = tem4;
////      //误差消除开关打开
////      if(g_ptz.hori_angle_erro_switch == 1)
////      {//注：误差消除开关打开时，需要关闭重复定位功能
////        tem4 = tem4 + g_ptz.offset_angle.offset_anle;//消除误差,,,,使用误差消除时需要关闭光电开关左转上升沿角度更新
////      }
//      
//      if(g_ptz.offset_angle.hori_offset_switch == 2)
//      {
//        tem4 = tem4 - g_ptz.offset_angle.hori_zero_offset_angle;
//        if(tem4 < 0)
//        {
//          tem4 = tem4 + 360.0;
//        }
//      }
//      if(tem4 == 360.0)///////解决既显示360又显示角度0的问题
//      {
//        tem4 = 0;
//      }
//      g_ptz.hori_angle_actual = tem4;
    }
    
    //保存当前磁编码器的值以及云台角度值
    g_ptz.hori_as5047d.as5047d_angle_last = g_ptz.hori_as5047d.as5047d_angle_actual;
    g_ptz.hori_as5047d.as5047d_ptz_angle_last = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
    g_ptz.hori_angle_last   = g_ptz.hori_angle_actual;

    return 1;
}

#endif


/// 计算云台垂直角度
#ifdef PTZ_VERT_ANGLE_AS5047D_ANGLE_SYN
static char ptz_vert_get_angle()
{
  float up_diff_x1 = 0;
  float up_diff_x2 = 0;
  
  float vert_angle[PTZ_VERT_ANGLE_READ_NUM] = {0};
  char  i = 0, j = 0, k = 0;
  float tem = 0;
  float tem1 = 0;
  static float vert_cycle_angle_up;
  static float vert_cycle_angle_down;
  long int tem3 = 0;
  float down_diff_x1 = 0;
  float down_diff_x2 = 0;  
  float tem4 = 0;
  char flag;
//  static float vert_angle_actual;
//  static float vert_angle_last;
#ifdef PTZ_VERT_GET_ANGLE_MANY
    for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM; i++)
    {
       vert_angle[i] = as5047d_vert_get_angle_a();
       
//       test_angle[test_angle_count] = vert_angle[i];
//       test_angle_count ++;
//       if(test_angle_count >= 500)
//       {
//         test_angle_count = 0;
//       }
       
       if(vert_angle[i] < 0 || //防止读出的角度为负值
          vert_angle[i] > 360.0||//防止读出的数值超过360
          isnan(vert_angle[i]) == 1)//防止读出的不是一个数
       {//以上错误的数据都该舍弃
         i --;
       }
    }
    
    //由于磁编码器存在0位，防止采集到的数据差距接近360度，对采集到的数据进行优化处理
    //例如0.2、0.1、359.9、359.8这种数据就需要处理
    flag = 0;
    for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM - 1; i++)
    {
      if(fabs(vert_angle[i] - vert_angle[i+1]) > 350.0)
      {
        flag = 1;//说明磁编码器处于0位附近，需要进行数据优化处理
        break;
      }
    }
    if(flag == 1)
    {
      for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM; i++)
      {
        if(vert_angle[i] < 5.0)
        {
          vert_angle[i] = vert_angle[i] + 360.0;
        }
      }
    }
    
    //对采集到的角度值排序
    for(j = 0; j < PTZ_VERT_ANGLE_READ_NUM-1; j++)//采样值由小到大排列
    {
       for(k = 0; k < PTZ_VERT_ANGLE_READ_NUM-j-1; k++)
       {
           if(vert_angle[k] > vert_angle[k+1])
           {
              tem             = vert_angle[k];
              vert_angle[k]   = vert_angle[k+1];
              vert_angle[k+1] = tem;
           }
        }
     }
    tem = 0;
    for(uint8_t i = 1; i < PTZ_VERT_ANGLE_READ_NUM - 1; i++)
    {
      tem = tem + vert_angle[i];//去掉一个最大值和一个最小值
    }
    tem = tem / ((float)(PTZ_VERT_ANGLE_READ_NUM - 2));
    
    if(tem > 360.0)
    {
      tem = tem - 360.0;
    }
    
    g_ptz.vert_as5047d.as5047d_angle_actual = tem;
#endif
    
#ifdef PTZ_VERT_GET_ANGLE_SINGLE
    tem = as5047d_vert_get_angle_a();
    
    if(tem < 0 || //防止读出的角度为负值
       tem > 360.0||//防止读出的数值超过360
       isnan(tem) == 1)//防止读出的不是一个数
       {//以上错误的数据都该舍弃,从新读取
         return 0;
       }
    g_ptz.vert_as5047d.as5047d_angle_actual = tem;
#endif
    
    if(g_ptz.vert_as5047d.as5047d_data_reset == 1)
    {
      g_ptz.vert_as5047d.as5047d_cycle_count = 0;
      g_ptz.vert_as5047d.as5047d_cycle_num = 0;
      g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
      g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
      g_ptz.vert_as5047d.as5047d_data_reset = 0;
      g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
      tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
      vert_cycle_angle_up = tem1 + 350.0;
      vert_cycle_angle_down = tem1 + 10.0;
    }
    
    //反转，云台角度减小，磁编码角度减小
    if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
    {
#ifdef PTZ_PHOTOELECTRIC_SWITCH
      //挡片更新角度
      if(g_ptz.vert_as5047d.as5047d_state == 1)
      {
        if(g_ptz.vert_ps_sw1_up_rise > 0)
        {
          g_ptz.vert_ps_sw1_up_rise = 0;
        }
        if(g_ptz.vert_ps_sw1_down_fall > 0)
        {
#ifdef PTZ_SW1_DOWN_FALL_UPDATE
          ptz_vert_ps_delay_ms();
          if(PS_VERT_SW1_READ == PS_COVER)
          {
            g_ptz.vert_as5047d.as5047d_cycle_count = 0;
            g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
            g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
            g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
            g_ptz.vert_angle_state = 1;
            g_ptz.vert_ps_sw1_down_fall = 0; 
          }
#endif
          g_ptz.vert_ps_sw1_down_fall = 0; 
        }
        
//        if(g_ptz.vert_ps_sw2_down_rise > 0)
//        {
//          g_ptz.vert_as5047d.as5047d_cycle_count =  g_ptz.vert_as5047d.as5047d_cycle_num;
//          g_ptz.vert_as5047d.as5047d_ptz_angle_actual =  g_ptz.vert_as5047d.as5047d_ptz_angle_max;
//          g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
//          g_ptz.vert_as5047d.as5047d_ptz_init_angle = 
//          g_ptz.vert_as5047d.as5047d_angle_actual - g_ptz.vert_as5047d.as5047d_remain_angle;
//          if(g_ptz.vert_as5047d.as5047d_ptz_init_angle < 0)
//          {
//            g_ptz.vert_as5047d.as5047d_ptz_init_angle = 
//              g_ptz.vert_as5047d.as5047d_ptz_init_angle + 360.0;
//          }
//          g_ptz.vert_ps_sw2_down_rise = 0;
//        }
        
        if(g_ptz.vert_ps_sw2_down_rise > 0)
        {
          g_ptz.vert_ps_sw2_down_rise = 0;
        }
        
        if(g_ptz.vert_ps_sw2_up_fall > 0)
        {
          g_ptz.vert_ps_sw2_up_fall = 0;
        }
      }
      
#endif
      
      tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
      vert_cycle_angle_up = tem1 + 350.0;
      vert_cycle_angle_down = tem1 + 10.0;
      //根据计算方法b，计算云台当前角度
      
      //判断磁编码器是否转完一圈
      
      //首先计算当前角度度和初始角度之间的角度距离
      down_diff_x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 
                          g_ptz.vert_as5047d.as5047d_ptz_init_angle);
      
      down_diff_x2 = fabs(360.0 - g_ptz.vert_as5047d.as5047d_angle_actual) + 
                          fabs(g_ptz.vert_as5047d.as5047d_ptz_init_angle - 0);
      
      if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_ptz_init_angle
         && down_diff_x1 <= PTZ_DOWN_CYCLE_C
         && g_ptz.vert_as5047d.as5047d_ptz_angle_actual < vert_cycle_angle_down)
      {
        g_ptz.vert_as5047d.as5047d_cycle_count --;//转动圈数减少1
      }
      if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_ptz_init_angle
         && down_diff_x2 <= PTZ_DOWN_CYCLE_C
         && g_ptz.vert_as5047d.as5047d_ptz_angle_actual < vert_cycle_angle_down)
      {
        g_ptz.vert_as5047d.as5047d_cycle_count --;//转动圈数减少1
      }

    }
    
    
    //正转，云台角度增大，磁编码角度增大
    if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
    {
#ifdef PTZ_PHOTOELECTRIC_SWITCH
      //挡片更新角度
      if(g_ptz.vert_as5047d.as5047d_state == 1)
      {
//        if(g_ptz.vert_ps_sw1_up_rise > 0)
//        {
//          g_ptz.vert_as5047d.as5047d_cycle_count = 0;
//          g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
//          g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
//          g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
//
//          g_ptz.vert_ps_sw1_up_rise = 0;
//        }
        
        if(g_ptz.vert_ps_sw1_up_rise > 0)
        {
#ifdef PTZ_SW1_UP_RISE_UPDATE
          ptz_vert_ps_delay_ms();
          if(PS_VERT_SW1_READ == PS_NO_COVER)
          {
            g_ptz.vert_as5047d.as5047d_cycle_count = 0;
            g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
            g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
            g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
            g_ptz.vert_angle_state = 1;
            g_ptz.vert_ps_sw1_up_rise = 0; 
          }
#endif
          g_ptz.vert_ps_sw1_up_rise = 0;
        }
        
        if( g_ptz.vert_ps_sw1_down_fall > 0)
        {
          g_ptz.vert_ps_sw1_down_fall = 0;
        }
        
        if(g_ptz.vert_ps_sw2_down_rise > 0)
        {
          g_ptz.vert_ps_sw2_down_rise = 0;
        }
        
        if(g_ptz.vert_ps_sw2_up_fall > 0)
        {
          ptz_vert_ps_delay_ms();
          if(PS_VERT_SW2_READ == PS_COVER)
          {
            g_ptz.vert_as5047d.as5047d_cycle_count =  g_ptz.vert_as5047d.as5047d_cycle_num;
            g_ptz.vert_as5047d.as5047d_ptz_angle_actual =  g_ptz.vert_as5047d.as5047d_ptz_angle_max;
            g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
            g_ptz.vert_as5047d.as5047d_ptz_init_angle = 
            g_ptz.vert_as5047d.as5047d_angle_actual - g_ptz.vert_as5047d.as5047d_remain_angle;
            if(g_ptz.vert_as5047d.as5047d_ptz_init_angle < 0)
            {
              g_ptz.vert_as5047d.as5047d_ptz_init_angle = 
                g_ptz.vert_as5047d.as5047d_ptz_init_angle + 360.0;
            }
            g_ptz.vert_angle_state = 1;
            g_ptz.vert_ps_sw2_up_fall = 0;
          }
          g_ptz.vert_ps_sw2_up_fall = 0;
        }
      }
      
#endif
      
      tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
      vert_cycle_angle_up = tem1 + 350.0;
      vert_cycle_angle_down = tem1 + 10.0;
      //根据计算方法b，计算云台当前角度
      
      //判断磁编码器是否转完一圈
      
      //首先计算当前角度度和初始角度之间的角度距离
      up_diff_x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 
                          g_ptz.vert_as5047d.as5047d_ptz_init_angle);
      
      up_diff_x2 = fabs(360.0 - g_ptz.vert_as5047d.as5047d_ptz_init_angle) + 
                          fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 0);
      
      if(g_ptz.vert_as5047d.as5047d_angle_actual >= g_ptz.vert_as5047d.as5047d_ptz_init_angle
         && up_diff_x1 <= PTZ_UP_CYCLE_C
         && g_ptz.vert_as5047d.as5047d_ptz_angle_actual > vert_cycle_angle_up)
      {
        g_ptz.vert_as5047d.as5047d_cycle_count ++;//转动圈数增加1
      }
      if(g_ptz.vert_as5047d.as5047d_angle_actual <= g_ptz.vert_as5047d.as5047d_ptz_init_angle
         && up_diff_x2 <= PTZ_UP_CYCLE_C
         && g_ptz.vert_as5047d.as5047d_ptz_angle_actual > vert_cycle_angle_up)
      {
        g_ptz.vert_as5047d.as5047d_cycle_count ++;//转动圈数增加1
      }
    }
   
    
    //计算当前角度  
    if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_ptz_init_angle)
    {
      g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual =
        (g_ptz.vert_as5047d.as5047d_angle_actual - 
          g_ptz.vert_as5047d.as5047d_ptz_init_angle) +
            g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;

    }
    if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_ptz_init_angle)
    {
      g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual =
        fabs(360.0 - g_ptz.vert_as5047d.as5047d_ptz_init_angle) +
          fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 0) +
            g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
    }
    if(g_ptz.vert_as5047d.as5047d_angle_actual == g_ptz.vert_as5047d.as5047d_ptz_init_angle)
    {
      g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
    }
    /**/
    //对算出的磁编码器总角度进行滤波处理
    if(fabs(g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last) 
       < PTZ_VERT_AS5047D_ANGLE_WF_A)
    {
      g_ptz.vert_as5047d.as5047d_ptz_angle_actual = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
      g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
    }
    

    if(g_ptz.vert_as5047d.as5047d_state == 1)
    {
      //对云台角度进行四舍五入,保留小数点后3位小数
      tem3 = (long int)((g_ptz.vert_as5047d.as5047d_ptz_angle_K * 
              (g_ptz.vert_as5047d.as5047d_ptz_angle_actual -
                g_ptz.vert_as5047d.as5047d_ptz_angle_max / 2.0)) * 1000.0 + 0.5);
      
      tem4 = tem3 / 1000.0;
      g_ptz.vert_angle_actual_a = tem4;
      if(g_ptz.offset_angle.vert_offset_switch == 2)
      {
        tem4 = tem4 - g_ptz.offset_angle.vert_zero_offset_angle;
      }
      g_ptz.vert_angle_actual = tem4;
      
    }
    
    //保存当前磁编码器的值以及云台角度值
    g_ptz.vert_as5047d.as5047d_angle_last = g_ptz.vert_as5047d.as5047d_angle_actual;
    g_ptz.vert_as5047d.as5047d_ptz_angle_last = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
    g_ptz.vert_angle_last   = g_ptz.vert_angle_actual;
    return 1;
}

static char ptz_vert_get_angle_a()
{
  float up_diff_x1 = 0;
  float up_diff_x2 = 0;
  float x1 = 0, x2 = 0, x3 = 0;
  float vert_angle[PTZ_VERT_ANGLE_READ_NUM] = {0};
  char  i = 0, j = 0, k = 0, m = 0;
  float tem = 0;
  float tem1 = 0;
  static float vert_cycle_angle_up;
  static float vert_cycle_angle_down;
  long int tem3 = 0;
  float down_diff_x1 = 0;
  float down_diff_x2 = 0; 
  float tem4 = 0;
  char flag;
  int cycle_count_save = 0;
//  static float vert_angle_actual;
//  static float vert_angle_last;
  
  g_ptz.vert_as5047d.time_end = g_ptz.vert_as5047d.time_now;
  g_ptz.vert_as5047d.time_now = OSTimeGet();
  g_ptz.vert_as5047d.time = time_ticks(g_ptz.vert_as5047d.time_now, g_ptz.vert_as5047d.time_end);
  
#ifdef CLK_50M
       if(g_ptz.vert_as5047d.time < 2)
       {
         g_ptz.vert_as5047d.time = 2;
       }
#else
       if(g_ptz.vert_as5047d.time < 1)
       {
         g_ptz.vert_as5047d.time = 1;
       }
#endif
  
  
#ifdef PTZ_VERT_GET_ANGLE_MANY
    for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM; i++)
    {
       vert_angle[i] = as5047d_vert_get_angle_a();
       
//       test_angle[test_angle_count] = vert_angle[i];
//       test_angle_count ++;
//       if(test_angle_count >= 500)
//       {
//         test_angle_count = 0;
//       }
       
       if(vert_angle[i] < 0 || //防止读出的角度为负值
          vert_angle[i] > 360.0||//防止读出的数值超过360
          isnan(vert_angle[i]) == 1)//防止读出的不是一个数
       {//以上错误的数据都该舍弃
         i --;
       }
    }
    
    //由于磁编码器存在0位，防止采集到的数据差距接近360度，对采集到的数据进行优化处理
    //例如0.2、0.1、359.9、359.8这种数据就需要处理
    flag = 0;
    for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM - 1; i++)
    {
      if(fabs(vert_angle[i] - vert_angle[i+1]) > 350.0)
      {
        flag = 1;//说明磁编码器处于0位附近，需要进行数据优化处理
        break;
      }
    }
    if(flag == 1)
    {
      for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM; i++)
      {
        if(vert_angle[i] < 5.0)
        {
          vert_angle[i] = vert_angle[i] + 360.0;
        }
      }
    }
    
    //对采集到的角度值排序
    for(j = 0; j < PTZ_VERT_ANGLE_READ_NUM-1; j++)//采样值由小到大排列
    {
       for(k = 0; k < PTZ_VERT_ANGLE_READ_NUM-j-1; k++)
       {
           if(vert_angle[k] > vert_angle[k+1])
           {
              tem             = vert_angle[k];
              vert_angle[k]   = vert_angle[k+1];
              vert_angle[k+1] = tem;
           }
        }
     }
    tem = 0;
    for(uint8_t i = 1; i < PTZ_VERT_ANGLE_READ_NUM - 1; i++)
    {
      tem = tem + vert_angle[i];//去掉一个最大值和一个最小值
    }
    tem = tem / ((float)(PTZ_VERT_ANGLE_READ_NUM - 2));
    
    if(tem > 360.0)
    {
      tem = tem - 360.0;
    }
    
    g_ptz.vert_as5047d.as5047d_angle_actual = tem;
#endif
    
#ifdef PTZ_VERT_GET_ANGLE_SINGLE
//    tem = as5047d_vert_get_angle_a();
//    
//    if(tem < 0 || //防止读出的角度为负值
//       tem > 360.0||//防止读出的数值超过360
//       isnan(tem) == 1)//防止读出的不是一个数
//       {//以上错误的数据都该舍弃,从新读取
//         return 0;
//       }
//    g_ptz.vert_as5047d.as5047d_angle_actual = tem;
    
    for(m = 0; m < 5; m++)
    {
      
      tem = as5047d_vert_get_angle_a();
      
      if(tem < 0          || //防止读出的角度为负值
         tem > 360.0      ||//防止读出的数值超过360
         isnan(tem) == 1  ||
         ((fabs(tem - g_ptz.vert_as5047d.as5047d_angle_last) > PTZ_VERT_ANGLE_DIFF_A) && (fabs(tem - g_ptz.vert_as5047d.as5047d_angle_last) < PTZ_VERT_ANGLE_DIFF_B)))//防止读出的不是一个数
      {//以上错误的数据都该舍弃,从新读取
       asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");
      }
      else
      {
        break;
      }
    }
    g_ptz.vert_as5047d.as5047d_angle_actual = tem;
#endif
    
    if(g_ptz.vert_as5047d.as5047d_data_reset == 1)
    {
      g_ptz.vert_as5047d.as5047d_cycle_count = 0;
      g_ptz.vert_as5047d.as5047d_cycle_num = 0;
      g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
      g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
      g_ptz.vert_as5047d.as5047d_data_reset = 0;
      g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
      tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
      vert_cycle_angle_up = tem1 + 350.0;
      vert_cycle_angle_down = tem1 + 10.0;
    }
    
    
    x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 
              g_ptz.vert_as5047d.as5047d_angle_last);
    
    x2 = (g_ptz.vert_as5047d.as5047d_angle_actual - 0) +
         (360.0 - g_ptz.vert_as5047d.as5047d_angle_last);
    
    x3 = (g_ptz.vert_as5047d.as5047d_angle_last - 0) +
         (360.0 - g_ptz.vert_as5047d.as5047d_angle_actual);
           
    //首先判断磁编码器角度判断磁编码器转向
    //磁编码器正转角度增大       
    if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_angle_last
       && x1 < x3)
    {
      g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_FWD;
    }
    
    if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_angle_last
       && x2 < x1)
    {
      g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_FWD;
    } 
    //磁编码器反转角度减小      
    if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_angle_last
       && x3 < x1)
    {
      g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_REV;
    }
    
    if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_angle_last
       && x1 < x2)
    {
      g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_REV;
    } 
    
    if(x1 == x2 || x1 == x3)
    {
      if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
      {
        g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_REV;
      }
      if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
      {
        g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_FWD;
      }
    }
    
    
    //反转，云台角度减小，磁编码角度减小
    if(g_ptz.vert_as5047d.as5047d_dir == AS5047D_DIR_REV)
    {
#ifdef PTZ_PHOTOELECTRIC_SWITCH
      //挡片更新角度
      if(g_ptz.vert_as5047d.as5047d_state == 1)
      {
        if(g_ptz.vert_ps_sw1_up_rise > 0)
        {
          g_ptz.vert_ps_sw1_up_rise = 0;
        }
        if( g_ptz.vert_ps_sw1_down_fall > 0)
        {
#ifdef PTZ_SW1_DOWN_FALL_UPDATE
          ptz_vert_ps_delay_ms();
          if(PS_VERT_SW1_READ == PS_COVER)
          {
            g_ptz.vert_as5047d.as5047d_cycle_count = 0;
            g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
            g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
            g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
            g_ptz.vert_angle_state = 1;
            g_ptz.vert_ps_sw1_down_fall = 0; 
            ptz_printf("\n vert_ps_sw1_down_fall angle min update!! \r\n",strlen((const char *)"\n vert_ps_sw1_down_fall angle min update!! \r\n"));
          }
#endif
          g_ptz.vert_ps_sw1_down_fall = 0; 
        }
        
//        if(g_ptz.vert_ps_sw2_down_rise > 0)
//        {
//          g_ptz.vert_as5047d.as5047d_cycle_count =  g_ptz.vert_as5047d.as5047d_cycle_num;
//          g_ptz.vert_as5047d.as5047d_ptz_angle_actual =  g_ptz.vert_as5047d.as5047d_ptz_angle_max;
//          g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
//          g_ptz.vert_as5047d.as5047d_ptz_init_angle = 
//          g_ptz.vert_as5047d.as5047d_angle_actual - g_ptz.vert_as5047d.as5047d_remain_angle;
//          if(g_ptz.vert_as5047d.as5047d_ptz_init_angle < 0)
//          {
//            g_ptz.vert_as5047d.as5047d_ptz_init_angle = 
//              g_ptz.vert_as5047d.as5047d_ptz_init_angle + 360.0;
//          }
//          g_ptz.vert_ps_sw2_down_rise = 0;
//        }
        
        if(g_ptz.vert_ps_sw2_down_rise > 0)
        {
          g_ptz.vert_ps_sw2_down_rise = 0;
        }
        
        if(g_ptz.vert_ps_sw2_up_fall > 0)
        {
          g_ptz.vert_ps_sw2_up_fall = 0;
        }
      }
      
#endif
      
      tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
      vert_cycle_angle_up = tem1 + 350.0;
      vert_cycle_angle_down = tem1 + 10.0;
      //根据计算方法b，计算云台当前角度
      
      //判断磁编码器是否转完一圈
      
      //首先计算当前角度度和初始角度之间的角度距离
      down_diff_x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 
                          g_ptz.vert_as5047d.as5047d_ptz_init_angle);
      
      down_diff_x2 = fabs(360.0 - g_ptz.vert_as5047d.as5047d_angle_actual) + 
                          fabs(g_ptz.vert_as5047d.as5047d_ptz_init_angle - 0);
      
      if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_ptz_init_angle
         && down_diff_x1 <= PTZ_DOWN_CYCLE_C
         && g_ptz.vert_as5047d.as5047d_ptz_angle_actual < vert_cycle_angle_down)
      {
        g_ptz.vert_as5047d.as5047d_cycle_count --;//转动圈数减少1
      }
      if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_ptz_init_angle
         && down_diff_x2 <= PTZ_DOWN_CYCLE_C
         && g_ptz.vert_as5047d.as5047d_ptz_angle_actual < vert_cycle_angle_down)
      {
        g_ptz.vert_as5047d.as5047d_cycle_count --;//转动圈数减少1
      }

    }
    
    
    //正转，云台角度增大，磁编码角度增大
    if(g_ptz.vert_as5047d.as5047d_dir == AS5047D_DIR_FWD)
    {
#ifdef PTZ_PHOTOELECTRIC_SWITCH
      //挡片更新角度
      if(g_ptz.vert_as5047d.as5047d_state == 1)
      {
        if(g_ptz.vert_ps_sw1_up_rise > 0)
        {
#ifdef PTZ_SW1_UP_RISE_UPDATE
          ptz_vert_ps_delay_ms();
          if(PS_VERT_SW1_READ == PS_NO_COVER)
          {
            g_ptz.vert_as5047d.as5047d_cycle_count = 0;
            g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
            g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
            g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
            g_ptz.vert_angle_state = 1;
            g_ptz.vert_ps_sw1_up_rise = 0; 
            ptz_printf("\n vert_ps_sw1_up_rise angle min update!! \r\n",strlen((const char *)"\n vert_ps_sw1_up_rise angle min update!! \r\n"));
          }
#endif
          g_ptz.vert_ps_sw1_up_rise = 0;
        }
        
        if( g_ptz.vert_ps_sw1_down_fall > 0)
        {
          g_ptz.vert_ps_sw1_down_fall = 0;
        }
        
        if(g_ptz.vert_ps_sw2_down_rise > 0)
        {
          g_ptz.vert_ps_sw2_down_rise = 0;
        }
        
        if(g_ptz.vert_ps_sw2_up_fall > 0)
        {
          ptz_vert_ps_delay_ms();
          if(PS_VERT_SW2_READ == PS_COVER)
          {
            g_ptz.vert_as5047d.as5047d_cycle_count =  g_ptz.vert_as5047d.as5047d_cycle_num;
            g_ptz.vert_as5047d.as5047d_ptz_angle_actual =  g_ptz.vert_as5047d.as5047d_ptz_angle_max;
            g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
            g_ptz.vert_as5047d.as5047d_ptz_init_angle = 
            g_ptz.vert_as5047d.as5047d_angle_actual - g_ptz.vert_as5047d.as5047d_remain_angle;
            if(g_ptz.vert_as5047d.as5047d_ptz_init_angle < 0)
            {
              g_ptz.vert_as5047d.as5047d_ptz_init_angle = 
                g_ptz.vert_as5047d.as5047d_ptz_init_angle + 360.0;
            }
            g_ptz.vert_angle_state = 1;
            g_ptz.vert_ps_sw2_up_fall = 0;
            ptz_printf("\n vert_ps_sw2_up_fall angle max update!! \r\n",strlen((const char *)"\n vert_ps_sw2_up_fall angle max update!! \r\n"));
          }
          g_ptz.vert_ps_sw2_up_fall = 0;
        }
      }
      
#endif
      
      tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
      vert_cycle_angle_up = tem1 + 350.0;
      vert_cycle_angle_down = tem1 + 10.0;
      //根据计算方法b，计算云台当前角度
      
      //判断磁编码器是否转完一圈
      
      //首先计算当前角度度和初始角度之间的角度距离
      up_diff_x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 
                          g_ptz.vert_as5047d.as5047d_ptz_init_angle);
      
      up_diff_x2 = fabs(360.0 - g_ptz.vert_as5047d.as5047d_ptz_init_angle) + 
                          fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 0);
      
      if(g_ptz.vert_as5047d.as5047d_angle_actual >= g_ptz.vert_as5047d.as5047d_ptz_init_angle
         && up_diff_x1 <= PTZ_UP_CYCLE_C
         && g_ptz.vert_as5047d.as5047d_ptz_angle_actual > vert_cycle_angle_up)
      {
        g_ptz.vert_as5047d.as5047d_cycle_count ++;//转动圈数增加1
      }
      if(g_ptz.vert_as5047d.as5047d_angle_actual <= g_ptz.vert_as5047d.as5047d_ptz_init_angle
         && up_diff_x2 <= PTZ_UP_CYCLE_C
         && g_ptz.vert_as5047d.as5047d_ptz_angle_actual > vert_cycle_angle_up)
      {
        g_ptz.vert_as5047d.as5047d_cycle_count ++;//转动圈数增加1
      }
    }
   
    //保存当前磁编码器圈数
    cycle_count_save = g_ptz.vert_as5047d.as5047d_cycle_count;
    
    for(i = 0; i < 4; i++)
    {//这个循环的目的是为了防止磁编码器圈数计算错误，恢复正常的磁编码圈数 
        //计算当前角度  
        if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_ptz_init_angle)
        {
          g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual =
            (g_ptz.vert_as5047d.as5047d_angle_actual - 
              g_ptz.vert_as5047d.as5047d_ptz_init_angle) +
                g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
        }
        if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_ptz_init_angle)
        {
          g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual =
            fabs(360.0 - g_ptz.vert_as5047d.as5047d_ptz_init_angle) +
              fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 0) +
                g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
        }
        if(g_ptz.vert_as5047d.as5047d_angle_actual == g_ptz.vert_as5047d.as5047d_ptz_init_angle)
        {
          g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
        }
        /**/
        //对算出的磁编码器总角度进行滤波处理
        if(fabs(g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last) 
           < PTZ_VERT_AS5047D_ANGLE_WF_A)
        {
          g_ptz.vert_as5047d.as5047d_ptz_angle_actual = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
          g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
          break;
        }
        else
        {
          //根据判断，恢复实际的磁编码器圈数
          switch(i)
          {
            case 0:
              g_ptz.vert_as5047d.as5047d_cycle_count = cycle_count_save;
              g_ptz.vert_as5047d.as5047d_cycle_count --;//转动圈数减少1
              break;
                
            case 1:
              g_ptz.vert_as5047d.as5047d_cycle_count = cycle_count_save;
              g_ptz.vert_as5047d.as5047d_cycle_count ++;//转动圈数增加1
              break;

            case 2: //磁编码器实际圈数恢复失败
              g_ptz.vert_as5047d.as5047d_cycle_count = cycle_count_save;
              break;  
              
            case 3:
              g_ptz.vert_as5047d.as5047d_ptz_angle_actual = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
              g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
              break;
              
          }
        }
    }

    
    
    if(g_ptz.vert_as5047d.as5047d_state == 1)
    {
      //对云台角度进行四舍五入,保留小数点后3位小数
      tem3 = (long int)((g_ptz.vert_as5047d.as5047d_ptz_angle_K * 
              (g_ptz.vert_as5047d.as5047d_ptz_angle_actual -
                g_ptz.vert_as5047d.as5047d_ptz_angle_max / 2.0)) * 1000.0 + 0.5);
      
      
      tem4 = tem3 / 1000.0;
      g_ptz.vert_angle_actual_a = tem4;
      if(g_ptz.offset_angle.vert_offset_switch == 2)
      {
        tem4 = tem4 - g_ptz.offset_angle.vert_zero_offset_angle;
      }
      g_ptz.vert_angle_actual = tem4;
      
      
    }
    
    //保存当前磁编码器的值以及云台角度值
    g_ptz.vert_as5047d.as5047d_angle_last = g_ptz.vert_as5047d.as5047d_angle_actual;
    g_ptz.vert_as5047d.as5047d_ptz_angle_last = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
    g_ptz.vert_angle_last   = g_ptz.vert_angle_actual;
    return 1;
}

#endif

static void ptz_get_angle_task()
{
  while(1)
  {
    ptz_hori_get_angle_a();
    OSTimeDlyHMSM(0u, 0u, 0u, 1u);
    ptz_vert_get_angle_a();
    OSTimeDlyHMSM(0u, 0u, 0u, 1u);
  }
}


void ptz_get_angle_init()
{
  g_ptz.hori_as5047d.as5047d_angle_actual = as5047d_hori_get_angle_a();
  g_ptz.hori_as5047d.as5047d_angle_actual = as5047d_hori_get_angle_a();
  g_ptz.hori_as5047d.as5047d_angle_last   = g_ptz.hori_as5047d.as5047d_angle_actual;
  g_ptz.vert_as5047d.as5047d_angle_actual = as5047d_vert_get_angle_a();
  g_ptz.vert_as5047d.as5047d_angle_actual = as5047d_vert_get_angle_a();
  g_ptz.vert_as5047d.as5047d_angle_last   = g_ptz.vert_as5047d.as5047d_angle_actual;
  
}



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

}

void init_angle_module()
{
//  if(strstr((const char*)system_info.error_compensation, "on"))
//  {//注：误差消除开关打开时，需要关闭水平重复定位功能
////    g_ptz.hori_angle_erro_switch = 1;//水平误差消除打开
////    g_ptz.hori_repeat_locate_switch = 0;//重复定位打开，重型云台关闭
//    
//    g_ptz.hori_repeat_locate_switch = 1;//重复定位打开，重型云台关闭
//    g_ptz.vert_repeat_locate_switch = 1;//重复定位打开
//  }else{
////    g_ptz.hori_angle_erro_switch = 0;//水平误差消除关闭
//    
//    g_ptz.hori_repeat_locate_switch = 0;//重复定位打开，重型云台关闭
//    g_ptz.vert_repeat_locate_switch = 0;//重复定位打开
//  }
  
  
  if(strstr((const char*)system_info.error_compensation, "on"))
  {//注：误差消除开关打开时，需要关闭水平重复定位功能
    g_ptz.hori_angle_erro_switch = 1;//水平误差消除打开
    g_ptz.hori_repeat_locate_switch = 0;//重复定位打开，重型云台关闭
  }else{
    g_ptz.hori_angle_erro_switch = 0;//水平误差消除关闭
  }
  ptz_zero_offset_angle_read();//读取角度偏移量
  as5047d_init();
  g_ptz.hori_as5047d.time_end = 0;
  g_ptz.hori_as5047d.time_now = 0;
  g_ptz.hori_as5047d.time = 0;
  g_ptz.vert_as5047d.time_end = 0;
  g_ptz.vert_as5047d.time_now = 0;
  g_ptz.vert_as5047d.time = 0;
  ptz_get_angle_init();
  creat_task_get_angle();
}


























