MW22-01A/BSP/Driver/as5047d/as5047d.c

#include "as5047d.h"
#include  <includes.h>

static BSP_OS_SEM ptz_hori_get_angle_mutex;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><D4B4>
static BSP_OS_SEM ptz_vert_get_angle_mutex;

#ifdef AS5047

static void as5047d_delay_nop(int as5047d_delay_time);
static unsigned int as5047d_Parity_bit_even(unsigned int package);
/// AS5047D<37><44><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>
///
/// <20><><EFBFBD><EFBFBD>AS5047D<37><44>SPIͨ<49>Ŷ<EFBFBD><C5B6><EFBFBD>ʱ
/// @param delay_time <20><><EFBFBD><EFBFBD>ʱʱ<CAB1>䳤<EFBFBD><E4B3A4>    
/// @return <20><> 
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-06<30><36><EFBFBD><EFBFBD>
static void as5047d_delay_nop(int delay_time)
{
    for(int i = 0; i < delay_time; i ++)
    {
      asm("nop");
      asm("nop");
      asm("nop");
      asm("nop");
    }  
}

/// AS5047D<37><44>ʼ<EFBFBD><CABC>
///
/// <20><>ʼ<EFBFBD><CABC>AS5047D
/// @param  <20><>   
/// @return <20><> 
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-06<30><36><EFBFBD><EFBFBD>
///        LH<4C>޸<EFBFBD><DEB8><EFBFBD>2022.0525
void as5047d_init()
{
  //<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>ʹ<EFBFBD><CAB9>
  rcu_periph_clock_enable(RCU_GPIOA);
  rcu_periph_clock_enable(RCU_GPIOC);
  rcu_periph_clock_enable(RCU_GPIOD);
  //ˮƽPC10--CLK,PC12--MOSI
  gpio_mode_set(GPIOC, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_10| GPIO_PIN_12);
  gpio_output_options_set(GPIOC, GPIO_OTYPE_PP, GPIO_OSPEED_25MHZ, GPIO_PIN_10| GPIO_PIN_12);
  //PA15--CS
  gpio_mode_set(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_15);
  gpio_output_options_set(GPIOA, GPIO_OTYPE_PP, GPIO_OSPEED_25MHZ, GPIO_PIN_15);
  //PC11--MISO
  gpio_mode_set(GPIOC, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_11);
  
  AS5047D_HORI_CSN_DISABLE;
  AS5047D_HORI_CLK_LOW;
  AS5047D_HORI_MOSI_LOW;
  
  
  
  //<2F><>ֱPD0--CS,PD1--CLK,PD3--MOSI
  gpio_mode_set(GPIOD, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_0| GPIO_PIN_1| GPIO_PIN_3);
  gpio_output_options_set(GPIOD, GPIO_OTYPE_PP, GPIO_OSPEED_25MHZ, GPIO_PIN_0| GPIO_PIN_1| GPIO_PIN_3);
  //PD2--MISO
  gpio_mode_set(GPIOD, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_2);
  
  AS5047D_VERT_CSN_DISABLE;
  AS5047D_VERT_CLK_LOW;
  AS5047D_VERT_MOSI_LOW;
}

/// AS5047D<37><44><EFBFBD>ݰ<EFBFBD><DDB0><EFBFBD>żУ<C5BC><D0A3>(żУ<C5BC><D0A3>)
///
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD><DDB0><EFBFBD><EFBFBD><EFBFBD>żУ<C5BC><D0A3>(żУ<C5BC><D0A3>)
/// @param  package <20><>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD>   
/// @return <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>żУ<C5BC><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD> 
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-06<30><36><EFBFBD><EFBFBD>
static unsigned int as5047d_Parity_bit_even(unsigned int package)
{
  unsigned int temp = 0;
  char temp1 = 0;
  ///<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD>ÿһλ<D2BB><CEBB>0<EFBFBD><30><EFBFBD><EFBFBD>1
  for(char i = 0; i < 15; i++)
  {
    temp = package & (1 << i);
    if(temp != 0)
    {
      ///<2F><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>ֵΪ1<CEAA><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
      temp1++;
    }
    temp = 0;
  }
  ///temp1<70><31><EFBFBD><EFBFBD>Ϊż<CEAA><C5BC>
  if(temp1 % 2 == 0)
  {
    ///<2F><><EFBFBD><EFBFBD>żЧ<C5BC><D0A7>λ<EFBFBD><CEBB>Ϊ0
    package = package & 0X7FFF; //0111 1111 1111 1111
  }
  ///temp1<70><31><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
  else
  {
    ///<2F><><EFBFBD><EFBFBD>żЧ<C5BC><D0A7>λ<EFBFBD><CEBB>Ϊ1 
    package = package | 0X8000;//1000 0000 0000 0000
  }
  return package;
}
 


/// <20>趨<EFBFBD><E8B6A8><EFBFBD>ݰ<EFBFBD>read/write<74><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
///
/// <20>趨<EFBFBD><E8B6A8><EFBFBD>ݰ<EFBFBD>read/write<74><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
/// @param  package <20><>Ҫ<EFBFBD>趨<EFBFBD><E8B6A8><EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD>   
/// @return <20>趨<EFBFBD><E8B6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD> 
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-07<30><37><EFBFBD><EFBFBD>
unsigned int as5047d_command_type(unsigned int package, unsigned int type)
{
  switch(type)
  {
    case AS5047D_WRITE_COMMAND:
    case AS5047D_WRITE_DATA:
      package = package & AS5047D_WRITE_COMMAND;
      break;

    case AS5047D_READ_COMMAND:
      package = package | AS5047D_READ_COMMAND;
      break;

    default :
      return 0;
  }
  return package;  
}

/// <20>жϽ<D0B6><CFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
///
/// <20>жϽ<D0B6><CFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
/// @param  package <20><><EFBFBD>жϵ<D0B6><CFB5><EFBFBD><EFBFBD><EFBFBD>
/// @param  <20><>
/// @return <20><><EFBFBD><EFBFBD><EFBFBD>жϽ<D0B6><CFBD><EFBFBD>
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-08<30><38><EFBFBD><EFBFBD>
As5047DJudgeReceivePackage as5047d_hori_judge_receive_package(unsigned int package)
{
  unsigned int temp = 0;
  As5047DJudgeReceivePackage temp1;
  memset(&temp1,0,sizeof(temp1));
  
  temp1.package = package;
  temp1.Operation_Result = 1;
  //<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ݰ<EFBFBD><DDB0><EFBFBD>żУ<C5BC><D0A3><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ȷ
  //Ч<><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  if(package != as5047d_Parity_bit_even(package))
  {
    temp1.parity_error_package = 1;
    temp1.Operation_Result = 0;
  }
  //Ч<><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ
  else
  {
    //<2F>жϷ<D0B6><CFB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD><DDB0>Ƿ<EFBFBD><C7B7>ǻظ<C7BB><D8B8><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    temp = package & AS5047D_JUDGE_RECEIVE_DATA;
    //<2F>ظ<EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    if(temp != 0)
    {
//      temp1.error_flag_command = 1;
//      temp1.framing_error_command   = package & 0X0001;
//      temp1.command_invalid_command = package & (0X0001 << 1);
//      temp1.parity_error_command    = package & (0X0001 << 2);
      as5047d_hori_cef_command();
      
    }
  }
  return temp1;
}


As5047DJudgeReceivePackage as5047d_vert_judge_receive_package(unsigned int package)
{
  unsigned int temp = 0;
  As5047DJudgeReceivePackage temp1;
  memset(&temp1,0,sizeof(temp1));
  
  temp1.package = package;
  temp1.Operation_Result = 1;
  //<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ݰ<EFBFBD><DDB0><EFBFBD>żУ<C5BC><D0A3><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ȷ
  //Ч<><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  if(package != as5047d_Parity_bit_even(package))
  {
    temp1.parity_error_package = 1;
    temp1.Operation_Result = 0;
  }
  //Ч<><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ
  else
  {
    //<2F>жϷ<D0B6><CFB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD><DDB0>Ƿ<EFBFBD><C7B7>ǻظ<C7BB><D8B8><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    temp = package & AS5047D_JUDGE_RECEIVE_DATA;
    //<2F>ظ<EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    if(temp != 0)
    {
//      temp1.error_flag_command = 1;
//      temp1.framing_error_command   = package & 0X0001;
//      temp1.command_invalid_command = package & (0X0001 << 1);
//      temp1.parity_error_command    = package & (0X0001 << 2);
      as5047d_vert_cef_command();
      
    }
  }
  return temp1;
}

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




/// ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
///
/// <20><>as5047d<37><64><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
/// @param  <20><> 
/// @return <20><><EFBFBD>ؽ<EFBFBD><D8BD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-08<30><38><EFBFBD><EFBFBD>
unsigned int as5047d_hori_send_package(unsigned int package)
{
  unsigned int temp = 0;
  unsigned int temp1 = 0;
  unsigned int temp2 = 0;
  AS5047D_HORI_CLK_LOW;
  AS5047D_HORI_MOSI_LOW;
  as5047d_delay_nop(2);
  for(int i = 15; i >= 0; i--)
  {
    AS5047D_HORI_CLK_LOW;
    as5047d_delay_nop(2);
    temp = package & (1 << i);
    if(temp != 0)
    {
      AS5047D_HORI_MOSI_HIGH;
    }
    else
    {
      AS5047D_HORI_MOSI_LOW;
    }
    as5047d_delay_nop(2);
    AS5047D_HORI_CLK_HIGH;
    as5047d_delay_nop(2);
    
    temp1 = AS5047D_HORI_MISO_READ;
    temp2 = temp2 | (temp1 << i);
    
    temp1 = 0;
    temp  = 0;
  }
  AS5047D_HORI_CLK_LOW;
  AS5047D_HORI_MOSI_LOW;
  //<2F><><EFBFBD>ص<EFBFBD><D8B5><EFBFBD>һ<EFBFBD><D2BB>ָ<EFBFBD><D6B8><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  return temp2;
}

/// ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
///
/// <20><><EFBFBD><EFBFBD>as5047d<37><64><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
/// @param  <20><> 
/// @return <20><><EFBFBD>ؽ<EFBFBD><D8BD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-07<30><37><EFBFBD><EFBFBD>
unsigned int as5047d_hori_receive_package()
{
  unsigned int temp = 0;
  unsigned int temp1 = 0;
  AS5047D_HORI_CLK_LOW;
  as5047d_delay_nop(2);
  for(int j = 15; j >= 0; j--)
  {
    AS5047D_HORI_CLK_HIGH;
    as5047d_delay_nop(2);
    temp = AS5047D_HORI_MISO_READ;
    temp1 = temp1 | (temp << j);
    AS5047D_HORI_CLK_LOW;
    as5047d_delay_nop(2);
    temp = 0;
  }
  AS5047D_HORI_CLK_LOW;
  return temp1;
}


/// ˮƽ<CBAE><C6BD><EFBFBD>Ͷ<EFBFBD>ָ<EFBFBD><D6B8>
///
/// <20><><EFBFBD>Ͷ<EFBFBD>ȡas5047d<37><64><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
/// @param  add <20>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD>ַ   
/// @return <20><><EFBFBD>͹<EFBFBD><CDB9><EFBFBD><EFBFBD>н<EFBFBD><D0BD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD> 111 1111 1111 1111
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-07<30><37><EFBFBD><EFBFBD>
unsigned int as5047d_hori_read_command(unsigned int add)
{
  unsigned int temp = 0;
  add = as5047d_command_type(add,AS5047D_READ_COMMAND);
  add = as5047d_Parity_bit_even(add);
   //<2F><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
  temp = as5047d_hori_send_package(add);
  //<2F><><EFBFBD>ص<EFBFBD><D8B5><EFBFBD>һ<EFBFBD><D2BB>ָ<EFBFBD><D6B8><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  return temp;
}


/// ˮƽ<CBAE><C6BD>ȡas5047d<37>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD>е<EFBFBD>ֵ
///
/// <20><>ȡas5047d<37>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD>е<EFBFBD>ֵ
/// @param  add <20>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD>ַ 
/// @return <20><><EFBFBD>ض<EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD>ļĴ<C4BC><C4B4><EFBFBD><EFBFBD>е<EFBFBD>ֵ
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-07<30><37><EFBFBD><EFBFBD>
unsigned int as5047d_hori_read_data(unsigned int add)
{
  
  unsigned int temp = 0;
 
  AS5047D_HORI_CSN_DISABLE;
  as5047d_delay_nop(4);
  AS5047D_HORI_CSN_ENABLE;
  as5047d_delay_nop(4);
  
  //<2F><><EFBFBD>Ͷ<EFBFBD>ָ<EFBFBD><D6B8>
  as5047d_hori_read_command(add);
  
  AS5047D_HORI_CSN_DISABLE;
  as5047d_delay_nop(4);
  AS5047D_HORI_CSN_ENABLE;
  as5047d_delay_nop(4);
  
  //<2F><>ȡ<EFBFBD><C8A1>һ<EFBFBD><D2BB>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD>
  temp = as5047d_hori_nop_command();
  
  AS5047D_HORI_CSN_DISABLE;
  return temp;
}


/// ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD>дָ<D0B4><D6B8>
///
/// <20><>as5047d<37><64><EFBFBD><EFBFBD>дָ<D0B4><D6B8>
/// @param  add <20>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD>ַ   
/// @return <20><><EFBFBD>͹<EFBFBD><CDB9><EFBFBD><EFBFBD>н<EFBFBD><D0BD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD> 
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-08<30><38><EFBFBD><EFBFBD>
unsigned int as5047d_hori_write_command(unsigned int add)
{
  unsigned int temp = 0;
  add = as5047d_command_type(add,AS5047D_WRITE_COMMAND);
  add = as5047d_Parity_bit_even(add);
  //<2F><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
  temp = as5047d_hori_send_package(add);
  //<2F><><EFBFBD>ص<EFBFBD><D8B5><EFBFBD>һ<EFBFBD><D2BB>ָ<EFBFBD><D6B8><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  return temp;
}

/// ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD>NOPָ<50><D6B8>
///
/// <20><>as5047d<37><64><EFBFBD><EFBFBD>NOPָ<50><D6B8>
/// @param  <20><>
/// @return <20><><EFBFBD>͹<EFBFBD><CDB9><EFBFBD><EFBFBD>н<EFBFBD><D0BD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD> 
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-08<30><38><EFBFBD><EFBFBD>
unsigned int as5047d_hori_nop_command()
{
  unsigned int temp = 0;
  temp = as5047d_hori_send_package(AS5047D_NOP);
  //<2F><><EFBFBD>ص<EFBFBD><D8B5><EFBFBD>һ<EFBFBD><D2BB>ָ<EFBFBD><D6B8><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  return temp;
}

/// ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
///
/// <20><>as5047d<37><64><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>CLEAR ERROR FLAG command
/// @param  <20><>
/// @return 1<><31><EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD><C9B9><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7>
/// @par    <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-08<30><38><EFBFBD><EFBFBD>
unsigned int as5047d_hori_cef_command()
{
  unsigned int temp  = 0;
  unsigned int temp1 = 0;
  
  AS5047D_HORI_CSN_DISABLE;
  as5047d_delay_nop(4);
  AS5047D_HORI_CSN_ENABLE;
  as5047d_delay_nop(4);
  temp = as5047d_Parity_bit_even(AS5047D_CLEAR_ERROR_FLAG);
  as5047d_hori_send_package(temp);
  
  AS5047D_HORI_CSN_DISABLE;
  as5047d_delay_nop(4);
  AS5047D_HORI_CSN_ENABLE;
  as5047d_delay_nop(4);
  temp1 = as5047d_hori_nop_command();
  
  AS5047D_HORI_CSN_DISABLE;
  as5047d_delay_nop(4);
  AS5047D_HORI_CSN_ENABLE;
  as5047d_delay_nop(4);
  temp1 = as5047d_hori_nop_command();
  
  AS5047D_HORI_CSN_DISABLE;
  return temp1;
}

/// ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD>дָ<D0B4><EFBFBD><EEB2A2>ȡ<EFBFBD><C8A1><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
///
/// <20><>as5047d<37><64><EFBFBD><EFBFBD>дָ<D0B4><EFBFBD><EEB2A2>ȡ<EFBFBD><C8A1><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
/// @param  add <20>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD>ַ 
/// @param  data <20><>Ҫд<D2AA><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
/// @return <20><><EFBFBD>͹<EFBFBD><CDB9><EFBFBD><EFBFBD>н<EFBFBD><D0BD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD> 
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-08<30><38><EFBFBD><EFBFBD>
As5047DJudgeReceivePackage as5047d_hori_write_data(unsigned int add,unsigned int data)
{
  As5047DJudgeReceivePackage temp;
  int i;
  for(i = 0; i < AS5047D_WRITE_NUM; i++)
  {
    AS5047D_HORI_CSN_DISABLE;
    as5047d_delay_nop(20);
    AS5047D_HORI_CSN_ENABLE;
    as5047d_delay_nop(20);
    
    //<2F><><EFBFBD><EFBFBD>д<EFBFBD><D0B4>ָ<EFBFBD><D6B8>
    as5047d_hori_write_command(add);
    //<2F><><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؼĴ<D8BC><C4B4><EFBFBD><EFBFBD>оɵ<D0BE><C9B5><EFBFBD><EFBFBD><EFBFBD>
    add = as5047d_command_type(data,AS5047D_WRITE_DATA);
    add = as5047d_Parity_bit_even(data);
    as5047d_hori_send_package(data);
    //<2F><><EFBFBD>ռĴ<D5BC><C4B4><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0X3FFF 0011 1111 1111 1111
    temp.package = as5047d_hori_nop_command();
    temp = as5047d_hori_judge_receive_package(temp.package);
    
    as5047d_delay_nop(2);
    AS5047D_HORI_CSN_DISABLE;
    
    if(temp.Operation_Result != 1)
    {
      as5047d_hori_cef_command();
      continue;
    }
    else
    {
      //<2F>ж<EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݺ<EFBFBD><DDBA><EFBFBD><EFBFBD>·<EFBFBD><C2B7>صļĴ<C4BC><C4B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ͬ
      if((data & 0X3FFF) == (temp.package & 0X3FFF))
      {
        temp.Operation_Result = 1;
        break;
      }
      else
      {
        continue;
      }
    }
  }
  return temp;
}

/// <20><>ȡ<EFBFBD><C8A1>̨ˮƽ<CBAE>Ƕ<EFBFBD>
///
/// <20><>ȡ<EFBFBD><C8A1>̨ˮƽ<CBAE>Ƕ<EFBFBD>
/// @param  <20><>
/// @param  <20><>
/// @return <20><><EFBFBD>ض<EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>-1<><31>ȡʧ<C8A1>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>ȡ<EFBFBD>ɹ<EFBFBD>
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-27<32><37><EFBFBD><EFBFBD>
float as5047d_hori_get_angle()
{
  BSP_OS_SemWait(&ptz_hori_get_angle_mutex, 0u);
  float angle = 0;
  unsigned int angle1 = 0;
  unsigned int temp = 0;
  As5047DJudgeReceivePackage temp1;
  memset(&temp1, 0, sizeof(temp1));
  //<2F><>ȡ<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
  temp = as5047d_hori_read_data(AS5047D_ANGLE);
  //<2F>жϽ<D0B6><CFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>
  temp1 = as5047d_hori_judge_receive_package(temp);
  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ
  if(temp1.Operation_Result == 1)
  {
    angle = (temp & 0x3FFF) * 0.02197265625;//0.02197265625 = 360.0 / 16384.0
    angle1 = (unsigned int)(angle * 1000.0);//ֻ<><D6BB><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3λ
    angle  = (float)(angle1 / 1000.0);
    BSP_OS_SemPost(&ptz_hori_get_angle_mutex);
    return angle;
  }
  BSP_OS_SemPost(&ptz_hori_get_angle_mutex);
  return -1;
}


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



/// <20><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
///
/// <20><>as5047d<37><64><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
/// @param  <20><> 
/// @return <20><><EFBFBD>ؽ<EFBFBD><D8BD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-08<30><38><EFBFBD><EFBFBD>
unsigned int as5047d_vert_send_package(unsigned int package)
{
  unsigned int temp = 0;
  unsigned int temp1 = 0;
  unsigned int temp2 = 0;
  AS5047D_VERT_CLK_LOW;
  AS5047D_VERT_MOSI_LOW;
  as5047d_delay_nop(2);
  for(int i = 15; i >= 0; i--)
  {
    AS5047D_VERT_CLK_LOW;
    as5047d_delay_nop(2);
    temp = package & (1 << i);
    if(temp != 0)
    {
      AS5047D_VERT_MOSI_HIGH;
    }
    else
    {
      AS5047D_VERT_MOSI_LOW;
    }
    as5047d_delay_nop(2);
    AS5047D_VERT_CLK_HIGH;
    as5047d_delay_nop(2);
    
    temp1 = AS5047D_VERT_MISO_READ;
    temp2 = temp2 | (temp1 << i);
    
    temp1 = 0;
    temp  = 0;
  }
  AS5047D_VERT_CLK_LOW;
  AS5047D_VERT_MOSI_LOW;
  //<2F><><EFBFBD>ص<EFBFBD><D8B5><EFBFBD>һ<EFBFBD><D2BB>ָ<EFBFBD><D6B8><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  return temp2;
}

/// <20><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
///
/// <20><><EFBFBD><EFBFBD>as5047d<37><64><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
/// @param  <20><> 
/// @return <20><><EFBFBD>ؽ<EFBFBD><D8BD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-07<30><37><EFBFBD><EFBFBD>
unsigned int as5047d_vert_receive_package()
{
  unsigned int temp = 0;
  unsigned int temp1 = 0;
  AS5047D_VERT_CLK_LOW;
  as5047d_delay_nop(2);
  for(int j = 15; j >= 0; j--)
  {
    AS5047D_VERT_CLK_HIGH;
    as5047d_delay_nop(2);
    temp = AS5047D_VERT_MISO_READ;
    temp1 = temp1 | (temp << j);
    AS5047D_VERT_CLK_LOW;
    as5047d_delay_nop(2);
    temp = 0;
  }
  AS5047D_VERT_CLK_LOW;
  return temp1;
}


/// <20><>ֱ<EFBFBD><D6B1><EFBFBD>Ͷ<EFBFBD>ָ<EFBFBD><D6B8>
///
/// <20><><EFBFBD>Ͷ<EFBFBD>ȡas5047d<37><64><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
/// @param  add <20>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD>ַ   
/// @return <20><><EFBFBD>͹<EFBFBD><CDB9><EFBFBD><EFBFBD>н<EFBFBD><D0BD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD> 111 1111 1111 1111
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-07<30><37><EFBFBD><EFBFBD>
unsigned int as5047d_vert_read_command(unsigned int add)
{
  unsigned int temp = 0;
  add = as5047d_command_type(add,AS5047D_READ_COMMAND);
  add = as5047d_Parity_bit_even(add);
   //<2F><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
  temp = as5047d_vert_send_package(add);
  //<2F><><EFBFBD>ص<EFBFBD><D8B5><EFBFBD>һ<EFBFBD><D2BB>ָ<EFBFBD><D6B8><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  return temp;
}


/// <20><>ֱ<EFBFBD><D6B1>ȡas5047d<37>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD>е<EFBFBD>ֵ
///
/// <20><>ȡas5047d<37>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD>е<EFBFBD>ֵ
/// @param  add <20>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD>ַ 
/// @return <20><><EFBFBD>ض<EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD>ļĴ<C4BC><C4B4><EFBFBD><EFBFBD>е<EFBFBD>ֵ
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-07<30><37><EFBFBD><EFBFBD>
unsigned int as5047d_vert_read_data(unsigned int add)
{
  
  unsigned int temp = 0;
 
  AS5047D_VERT_CSN_DISABLE;
  as5047d_delay_nop(4);
  AS5047D_VERT_CSN_ENABLE;
  as5047d_delay_nop(4);
  
  //<2F><><EFBFBD>Ͷ<EFBFBD>ָ<EFBFBD><D6B8>
  as5047d_vert_read_command(add);
  
  AS5047D_VERT_CSN_DISABLE;
  as5047d_delay_nop(4);
  AS5047D_VERT_CSN_ENABLE;
  as5047d_delay_nop(4);
  
  //<2F><>ȡ<EFBFBD><C8A1>һ<EFBFBD><D2BB>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD>
  temp = as5047d_vert_nop_command();
  
  AS5047D_VERT_CSN_DISABLE;
  return temp;
}

/// <20><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>дָ<D0B4><D6B8>
///
/// <20><>as5047d<37><64><EFBFBD><EFBFBD>дָ<D0B4><D6B8>
/// @param  add <20>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD>ַ   
/// @return <20><><EFBFBD>͹<EFBFBD><CDB9><EFBFBD><EFBFBD>н<EFBFBD><D0BD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD> 
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-08<30><38><EFBFBD><EFBFBD>
unsigned int as5047d_vert_write_command(unsigned int add)
{
  unsigned int temp = 0;
  add = as5047d_command_type(add,AS5047D_WRITE_COMMAND);
  add = as5047d_Parity_bit_even(add);
  //<2F><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
  temp = as5047d_vert_send_package(add);
  //<2F><><EFBFBD>ص<EFBFBD><D8B5><EFBFBD>һ<EFBFBD><D2BB>ָ<EFBFBD><D6B8><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  return temp;
}

/// <20><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>NOPָ<50><D6B8>
///
/// <20><>as5047d<37><64><EFBFBD><EFBFBD>NOPָ<50><D6B8>
/// @param  <20><>
/// @return <20><><EFBFBD>͹<EFBFBD><CDB9><EFBFBD><EFBFBD>н<EFBFBD><D0BD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD> 
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-08<30><38><EFBFBD><EFBFBD>
unsigned int as5047d_vert_nop_command()
{
  unsigned int temp = 0;
  temp = as5047d_vert_send_package(AS5047D_NOP);
  //<2F><><EFBFBD>ص<EFBFBD><D8B5><EFBFBD>һ<EFBFBD><D2BB>ָ<EFBFBD><D6B8><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  return temp;
}

/// <20><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
///
/// <20><>as5047d<37><64><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>CLEAR ERROR FLAG command
/// @param  <20><>
/// @return 1<><31><EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD><C9B9><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7>
/// @par    <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-08<30><38><EFBFBD><EFBFBD>
unsigned int as5047d_vert_cef_command()
{
  unsigned int temp  = 0;
  unsigned int temp1 = 0;
  
  AS5047D_VERT_CSN_DISABLE;
  as5047d_delay_nop(4);
  AS5047D_VERT_CSN_ENABLE;
  as5047d_delay_nop(4);
  temp = as5047d_Parity_bit_even(AS5047D_CLEAR_ERROR_FLAG);
  as5047d_vert_send_package(temp);
  
  AS5047D_VERT_CSN_DISABLE;
  as5047d_delay_nop(4);
  AS5047D_VERT_CSN_ENABLE;
  as5047d_delay_nop(4);
  temp1 = as5047d_vert_nop_command();
  
  AS5047D_VERT_CSN_DISABLE;
  as5047d_delay_nop(4);
  AS5047D_VERT_CSN_ENABLE;
  as5047d_delay_nop(4);
  temp1 = as5047d_vert_nop_command();
  
  AS5047D_VERT_CSN_DISABLE;
  return temp1;
}

/// <20><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>дָ<D0B4><EFBFBD><EEB2A2>ȡ<EFBFBD><C8A1><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
///
/// <20><>as5047d<37><64><EFBFBD><EFBFBD>дָ<D0B4><EFBFBD><EEB2A2>ȡ<EFBFBD><C8A1><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
/// @param  add <20>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD>ַ 
/// @param  data <20><>Ҫд<D2AA><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
/// @return <20><><EFBFBD>͹<EFBFBD><CDB9><EFBFBD><EFBFBD>н<EFBFBD><D0BD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD> 
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-08<30><38><EFBFBD><EFBFBD>
As5047DJudgeReceivePackage as5047d_vert_write_data(unsigned int add,unsigned int data)
{
  As5047DJudgeReceivePackage temp;
  int i;
  for(i = 0; i < AS5047D_WRITE_NUM; i++)
  {
    AS5047D_VERT_CSN_DISABLE;
    as5047d_delay_nop(20);
    AS5047D_VERT_CSN_ENABLE;
    as5047d_delay_nop(20);
    
    //<2F><><EFBFBD><EFBFBD>д<EFBFBD><D0B4>ָ<EFBFBD><D6B8>
    as5047d_vert_write_command(add);
    //<2F><><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؼĴ<D8BC><C4B4><EFBFBD><EFBFBD>оɵ<D0BE><C9B5><EFBFBD><EFBFBD><EFBFBD>
    add = as5047d_command_type(data,AS5047D_WRITE_DATA);
    add = as5047d_Parity_bit_even(data);
    as5047d_vert_send_package(data);
    //<2F><><EFBFBD>ռĴ<D5BC><C4B4><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0X3FFF 0011 1111 1111 1111
    temp.package = as5047d_vert_nop_command();
    temp = as5047d_vert_judge_receive_package(temp.package);
    
    as5047d_delay_nop(2);
    AS5047D_VERT_CSN_DISABLE;
    
    if(temp.Operation_Result != 1)
    {
      as5047d_vert_cef_command();
      continue;
    }
    else
    {
      //<2F>ж<EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݺ<EFBFBD><DDBA><EFBFBD><EFBFBD>·<EFBFBD><C2B7>صļĴ<C4BC><C4B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ͬ
      if((data & 0X3FFF) == (temp.package & 0X3FFF))
      {
        temp.Operation_Result = 1;
        break;
      }
      else
      {
        continue;
      }
    }
  }
  return temp;
}

/// <20><>ȡ<EFBFBD><C8A1>̨<EFBFBD><CCA8>ֱ<EFBFBD>Ƕ<EFBFBD>
///
/// <20><>ȡ<EFBFBD><C8A1>̨<EFBFBD><CCA8>ֱ<EFBFBD>Ƕ<EFBFBD>
/// @param  <20><>
/// @param  <20><>
/// @return <20><><EFBFBD>ض<EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>-1<><31>ȡʧ<C8A1>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>ȡ<EFBFBD>ɹ<EFBFBD>
/// @par   <20>޸<EFBFBD><DEB8><EFBFBD>־
///        <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2017-09-27<32><37><EFBFBD><EFBFBD>
float as5047d_vert_get_angle()
{
  BSP_OS_SemWait(&ptz_vert_get_angle_mutex, 0u);
  float angle = 0;
  unsigned int angle1 = 0;
  unsigned int temp = 0;
  As5047DJudgeReceivePackage temp1;
  memset(&temp1, 0, sizeof(temp1));
  //<2F><>ȡ<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
  temp = as5047d_vert_read_data(AS5047D_ANGLE);
  //<2F>жϽ<D0B6><CFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>
  temp1 = as5047d_vert_judge_receive_package(temp);
  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ
  if(temp1.Operation_Result == 1)
  {
    angle = (temp & 0x3FFF) * 0.02197265625;//0.02197265625 = 360.0 / 16384.0
    angle1 = (unsigned int)(angle * 1000.0);//ֻ<><D6BB><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3λ
    angle  = (float)(angle1 / 1000.0);
    BSP_OS_SemPost(&ptz_vert_get_angle_mutex);
    return angle;
  }
  BSP_OS_SemPost(&ptz_vert_get_angle_mutex);
  return -1;
}



/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
//ֱ<><D6B1>ͨ<EFBFBD><CDA8>ʱ<EFBFBD>Ӷ<EFBFBD>ȡ<EFBFBD>Ƕ<EFBFBD>
unsigned int as5047d_hori_clk_read_angle()
{
  unsigned int temp1 = 0;
  unsigned int temp2 = 0;
  AS5047D_HORI_CLK_LOW;
  AS5047D_HORI_MOSI_HIGH;
  as5047d_delay_nop(2);
  for(int i = 15; i >= 0; i--)
  {
    AS5047D_HORI_CLK_LOW;
    as5047d_delay_nop(2);
    AS5047D_HORI_CLK_HIGH;
    as5047d_delay_nop(2);
    
    temp1 = AS5047D_HORI_MISO_READ;
    temp2 = temp2 | (temp1 << i);
    
    temp1 = 0;
  }
  AS5047D_HORI_CLK_LOW;
  return temp2;
}

//<2F>޸Ķ<DEB8>ȡ<EFBFBD>Ƕȵķ<C8B5>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕȶ<C7B6>ȡ<EFBFBD>ٶ<EFBFBD>
unsigned int as5047d_hori_read_data_a()
{
  
  unsigned int temp = 0;
 
  AS5047D_HORI_CSN_DISABLE;
  as5047d_delay_nop(4);
  AS5047D_HORI_CSN_ENABLE;
  as5047d_delay_nop(4);
  
  //<2F><><EFBFBD>Ͷ<EFBFBD>ָ<EFBFBD><D6B8>
  temp = as5047d_hori_clk_read_angle();
  
  AS5047D_HORI_CSN_DISABLE;
  return temp;
}

//<2F>µ<EFBFBD><C2B5>жϷ<D0B6>ʽ
As5047DJudgeReceivePackage as5047d_hori_judge_receive_package_a(unsigned int package)
{
  As5047DJudgeReceivePackage temp1;
  memset(&temp1,0,sizeof(temp1));
  
  temp1.package = package;
  temp1.Operation_Result = 1;
  //<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ݰ<EFBFBD><DDB0><EFBFBD>żУ<C5BC><D0A3><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ȷ
  //Ч<><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  if(package != as5047d_Parity_bit_even(package))
  {
    temp1.parity_error_package = 1;
    temp1.Operation_Result = 0;
  }
  return temp1;
}



//<2F>µĶ<C2B5>ȡ<EFBFBD>Ƕȷ<C7B6>ʽ
float as5047d_hori_get_angle_a()
{
  BSP_OS_SemWait(&ptz_hori_get_angle_mutex, 0u);
  float angle = 0;
  unsigned int angle1 = 0;
  unsigned int temp = 0;
  As5047DJudgeReceivePackage temp1;
  memset(&temp1, 0, sizeof(temp1));
  //<2F><>ȡ<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
  temp = as5047d_hori_read_data_a();
  //<2F>жϽ<D0B6><CFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>
  temp1 = as5047d_hori_judge_receive_package_a(temp);
  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ
  if(temp1.Operation_Result == 1)
  {
    angle = (temp & 0x3FFF) * 0.02197265625;//0.02197265625 = 360.0 / 16384.0
    angle1 = (unsigned int)(angle * 1000.0);//ֻ<><D6BB><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3λ
    angle  = (float)(angle1 / 1000.0);
    BSP_OS_SemPost(&ptz_hori_get_angle_mutex);
    return angle;
  }
  BSP_OS_SemPost(&ptz_hori_get_angle_mutex);
  return -1;
}

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

//ֱ<><D6B1>ͨ<EFBFBD><CDA8>ʱ<EFBFBD>Ӷ<EFBFBD>ȡ<EFBFBD>Ƕ<EFBFBD>
unsigned int as5047d_vert_clk_read_angle()
{
  unsigned int temp1 = 0;
  unsigned int temp2 = 0;
  AS5047D_VERT_CLK_LOW;
  AS5047D_VERT_MOSI_HIGH;
  as5047d_delay_nop(2);
  for(int i = 15; i >= 0; i--)
  {
    AS5047D_VERT_CLK_LOW;
    as5047d_delay_nop(2);
    AS5047D_VERT_CLK_HIGH;
    as5047d_delay_nop(2);
    
    temp1 = AS5047D_VERT_MISO_READ;
    temp2 = temp2 | (temp1 << i);
    
    temp1 = 0;
  }
  AS5047D_VERT_CLK_LOW;
  return temp2;
}

//<2F>޸Ķ<DEB8>ȡ<EFBFBD>Ƕȵķ<C8B5>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕȶ<C7B6>ȡ<EFBFBD>ٶ<EFBFBD>
unsigned int as5047d_vert_read_data_a()
{
  
  unsigned int temp = 0;
 
  AS5047D_VERT_CSN_DISABLE;
  as5047d_delay_nop(4);
  AS5047D_VERT_CSN_ENABLE;
  as5047d_delay_nop(4);
  
  //<2F><><EFBFBD>Ͷ<EFBFBD>ָ<EFBFBD><D6B8>
  temp = as5047d_vert_clk_read_angle();
  
  AS5047D_VERT_CSN_DISABLE;
  return temp;
}

//<2F>µ<EFBFBD><C2B5>жϷ<D0B6>ʽ
As5047DJudgeReceivePackage as5047d_vert_judge_receive_package_a(unsigned int package)
{
  As5047DJudgeReceivePackage temp1;
  memset(&temp1,0,sizeof(temp1));
  
  temp1.package = package;
  temp1.Operation_Result = 1;
  //<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ݰ<EFBFBD><DDB0><EFBFBD>żУ<C5BC><D0A3><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ȷ
  //Ч<><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  if(package != as5047d_Parity_bit_even(package))
  {
    temp1.parity_error_package = 1;
    temp1.Operation_Result = 0;
  }
  return temp1;
}

//<2F>µĶ<C2B5>ȡ<EFBFBD>Ƕȷ<C7B6>ʽ
float as5047d_vert_get_angle_a()
{
  BSP_OS_SemWait(&ptz_vert_get_angle_mutex, 0u);
  float angle = 0;
  unsigned int angle1 = 0;
  unsigned int temp = 0;
  As5047DJudgeReceivePackage temp1;
  memset(&temp1, 0, sizeof(temp1));
  //<2F><>ȡ<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
  temp = as5047d_vert_read_data_a();
  //<2F>жϽ<D0B6><CFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>
  temp1 = as5047d_vert_judge_receive_package_a(temp);
  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ
  if(temp1.Operation_Result == 1)
  {
    angle = (temp & 0x3FFF) * 0.02197265625;//0.02197265625 = 360.0 / 16384.0
    angle1 = (unsigned int)(angle * 1000.0);//ֻ<><D6BB><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3λ
    angle  = (float)(angle1 / 1000.0);
    BSP_OS_SemPost(&ptz_vert_get_angle_mutex);
    return angle;
  }
  BSP_OS_SemPost(&ptz_vert_get_angle_mutex);
  return -1;
}

/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
#endif

#ifdef TMR3109

/**
  * @brief  tmr3109<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>
  * @param  
  * @retval
  *
  */
static void tmr3109_init(void)
{
    //<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>ʹ<EFBFBD><CAB9>
    rcu_periph_clock_enable(RCU_GPIOA);
    rcu_periph_clock_enable(RCU_GPIOC);
    rcu_periph_clock_enable(RCU_GPIOD);
    //ˮƽPC10--CLK,PC12--MOSI
    gpio_mode_set(GPIOC, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_10| GPIO_PIN_12);
    gpio_output_options_set(GPIOC, GPIO_OTYPE_PP, GPIO_OSPEED_25MHZ, GPIO_PIN_10| GPIO_PIN_12);
    //PA15--CS
    gpio_mode_set(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_15);
    gpio_output_options_set(GPIOA, GPIO_OTYPE_PP, GPIO_OSPEED_25MHZ, GPIO_PIN_15);
    //PC11--MISO
    gpio_mode_set(GPIOC, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_11);
    
    TMR3109_HORI_CSN_DISABLE;
    TMR3109_HORI_CLK_LOW;
    TMR3109_HORI_MOSI_LOW;
    
    
    
    //<2F><>ֱPD0--CS,PD1--CLK,PD3--MOSI
    gpio_mode_set(GPIOD, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_0| GPIO_PIN_1| GPIO_PIN_3);
    gpio_output_options_set(GPIOD, GPIO_OTYPE_PP, GPIO_OSPEED_25MHZ, GPIO_PIN_0| GPIO_PIN_1| GPIO_PIN_3);
    //PD2--MISO
    gpio_mode_set(GPIOD, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_2);
    
    TMR3109_VERT_CSN_DISABLE;
    TMR3109_VERT_CLK_LOW;
    TMR3109_VERT_MOSI_LOW;
}

/**
  * @brief  tmr3109<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<EFBFBD><EFBFBD><EFBFBD><EFBFBD>tmr3109<EFBFBD><EFBFBD>SPIͨ<EFBFBD>Ŷ<EFBFBD><EFBFBD><EFBFBD>ʱ
  * @param  delay_time <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱʱ<EFBFBD>䳤<EFBFBD><EFBFBD>
  * @retval
  *
  */
static void tmr3109_delay_nop(int delay_time)
{
    for(int i = 0; i < delay_time; i ++)
    {
      asm("nop");
      asm("nop");
      asm("nop");
      asm("nop");
    }  
}

/**
  * @brief  <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽ<EFBFBD><EFBFBD>д<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  * @param  Op_code <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  *         address <EFBFBD><EFBFBD>ַ<EFBFBD><EFBFBD>
  *         idleBit <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  *         dataBit <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
  * @retval <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  *
  */
unsigned int tmr3109_command_type(unsigned char Op_code, unsigned char address, unsigned char idleBit, unsigned short dataBit)
{
    return (unsigned int)((Op_code << 29) | (address << 21) | (idleBit << 16) | dataBit);
}

/**
  * @brief  У<EFBFBD><EFBFBD>tmr3109<EFBFBD>Ƕ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  * @param  data <EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD>ĽǶ<EFBFBD><EFBFBD><EFBFBD>Ϣ
  * @retval crc У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
  *
  */
// <20><><EFBFBD><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD> (x^4 + x^3 + x^2 + 1) -> 0b1101
#define CRC_POLY 0x0D
static unsigned char check_tmr3109_angle(unsigned int data_24bit)
{
    unsigned char crc = 0x03; // <20><>ʼֵ 0011

    uint8_t feedback;
    
    // <20><><EFBFBD><EFBFBD> 24 λ<><CEBB><EFBFBD><EFBFBD> (MSB first)
    for (int i = 23; i >= 0; i--) {
        // <20><>ȡ<EFBFBD><C8A1>ǰλ (<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>ʼ)
        uint8_t bit = (data_24bit >> i) & 0x01;
        
        // <20><><EFBFBD>㷴<EFBFBD><E3B7B4>ֵ = CRC<52><43><EFBFBD><EFBFBD>λ XOR <20><><EFBFBD><EFBFBD>λ
        feedback = (crc >> 3) ^ bit;
        
        // <20><><EFBFBD>ƼĴ<C6BC><C4B4><EFBFBD> (<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ)
        crc = (crc << 1) & 0x0F;
        
        // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ1ʱ<31><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
        if (feedback) {
            crc ^= CRC_POLY; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD> 1101
        }
    }

    return crc;
}

/**
  * @brief  <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݷ<EFBFBD><EFBFBD>ͷ<EFBFBD><EFBFBD>͵<EFBFBD>ˮƽ<EFBFBD>ű<EFBFBD>
  * @param  package Ҫ<EFBFBD><EFBFBD><EFBFBD>͵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  * @retval <EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  *
  */
unsigned int tmr3109_hori_send_package(unsigned int package)
{
    unsigned int temp = 0;
    unsigned int temp1 = 0;
    unsigned int temp2 = 0;

    TMR3109_HORI_CLK_LOW;
    TMR3109_HORI_MOSI_LOW;

    tmr3109_delay_nop(2);
    for(int i = 31; i >= 0; i--)
    {
        TMR3109_HORI_CLK_LOW;
        tmr3109_delay_nop(2);
        temp = package & (1 << i);
        if(temp != 0) {
            TMR3109_HORI_MOSI_HIGH;
        }
        else {
            TMR3109_HORI_MOSI_LOW;
        }
        tmr3109_delay_nop(2);
        
        TMR3109_HORI_CLK_HIGH;
        tmr3109_delay_nop(2);
        
        temp1 = TMR3109_HORI_MISO_READ;
        temp2 = temp2 | (temp1 << i);
        
        temp1 = 0;
        temp  = 0;
    }

    TMR3109_HORI_CLK_LOW;
    TMR3109_HORI_MOSI_LOW;

    return temp2;
}

static volatile float vert_tmr3109_angle_actual;
static volatile float hori_tmr3109_angle_actual;
/**
  * @brief  <EFBFBD><EFBFBD>ȡˮƽת<EFBFBD>ӵ<EFBFBD>λ<EFBFBD><EFBFBD>
  * @param  
  * @retval λ<EFBFBD><EFBFBD>
  *
  */
float read_horiTmr3109Angle(void)
{
    static float proportion = 360.0f / 8388608.0f;
    static unsigned int temp;
    static float angle;
    // static unsigned int temp1;


    // BSP_OS_SemWait(&ptz_hori_get_angle_mutex, 0u);
    // TMR3109_HORI_CSN_DISABLE;
    // tmr3109_delay_nop(4);
    TMR3109_HORI_CSN_ENABLE;
    tmr3109_delay_nop(20);

    temp = tmr3109_hori_send_package(tmr3109_command_type(0b011, 0, 0, 0));
    
    TMR3109_HORI_CSN_DISABLE;

    if (check_tmr3109_angle((temp >> 5) & 0b11111111111111111111111) == ((temp >> 1) & 0x0F)) {
        //crcУ<63><D0A3><EFBFBD>ɹ<EFBFBD>        
        // pdebug(DEBUG_LEVEL_DEBUG, "crc check success\n");
        angle = (float)((temp >> 5) & 0b11111111111111111111111) * proportion;
        hori_tmr3109_angle_actual = 360.0 - angle;
        // BSP_OS_SemPost(&ptz_hori_get_angle_mutex);
        return hori_tmr3109_angle_actual;
    }

    // BSP_OS_SemPost(&ptz_hori_get_angle_mutex);
    return -1;
}

/**
  * @brief  <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݷ<EFBFBD><EFBFBD>ͷ<EFBFBD><EFBFBD>͵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ű<EFBFBD>
  * @param  package Ҫ<EFBFBD><EFBFBD><EFBFBD>͵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  * @retval <EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
  *
  */
unsigned int tmr3109_vert_send_package(unsigned int package)
{
    unsigned int temp = 0;
    unsigned int temp1 = 0;
    unsigned int temp2 = 0;

    TMR3109_VERT_CLK_LOW;
    TMR3109_VERT_MOSI_LOW;

    tmr3109_delay_nop(2);
    for(int i = 31; i >= 0; i--)
    {
        TMR3109_VERT_CLK_LOW;
        tmr3109_delay_nop(2);
        temp = package & (1 << i);
        if(temp != 0) {
            TMR3109_VERT_MOSI_HIGH;
        }
        else {
            TMR3109_VERT_MOSI_LOW;
        }
        tmr3109_delay_nop(2);
        
        TMR3109_VERT_CLK_HIGH;
        tmr3109_delay_nop(2);
        
        temp1 = TMR3109_VERT_MISO_READ;
        temp2 = temp2 | (temp1 << i);
        
        temp1 = 0;
        temp  = 0;
    }

    TMR3109_VERT_CLK_LOW;
    TMR3109_VERT_MOSI_LOW;

    return temp2;
}

/**
  * @brief  <EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>ӵ<EFBFBD>λ<EFBFBD><EFBFBD>
  * @param  
  * @retval λ<EFBFBD><EFBFBD>
  *
  */
float read_vertTmr3109Angle(void)
{
    static float proportion = 360.0f / 8388608.0f;
    static unsigned int temp;
    static float angle;
    // static unsigned int temp1;

    // BSP_OS_SemWait(&ptz_vert_get_angle_mutex, 0u);
    TMR3109_VERT_CSN_ENABLE;
    tmr3109_delay_nop(20);

    temp = tmr3109_vert_send_package(tmr3109_command_type(0b011, 0, 0, 0));
    
    TMR3109_VERT_CSN_DISABLE;

    if (check_tmr3109_angle((temp >> 5) & 0b11111111111111111111111) == ((temp >> 1) & 0x0F)) {
        //crcУ<63><D0A3><EFBFBD>ɹ<EFBFBD>
        // pdebug(DEBUG_LEVEL_DEBUG, "crc check success\n");
        angle = (float)((temp >> 5) & 0b11111111111111111111111) * proportion;
        vert_tmr3109_angle_actual = (360.0 - angle);

        // BSP_OS_SemPost(&ptz_vert_get_angle_mutex);
        return vert_tmr3109_angle_actual;
    }

    // BSP_OS_SemPost(&ptz_vert_get_angle_mutex);
    return -1;
}

void as5047d_init()
{
    tmr3109_init();
}

float as5047d_hori_get_angle_a()
{
    return read_horiTmr3109Angle();
}

float as5047d_vert_get_angle_a()
{
    return read_vertTmr3109Angle();
}







#endif