#include "as5047d.h"
#include  <includes.h>
#include <stdint.h>
#include <stdio.h>
#include <stdbool.h>

#ifdef AS5047

static BSP_OS_SEM ptz_hori_get_angle_mutex;//共享资源锁
static BSP_OS_SEM ptz_vert_get_angle_mutex;

static void as5047d_delay_nop(int as5047d_delay_time);
static unsigned int as5047d_Parity_bit_even(unsigned int package);
/// AS5047D短延时函数
///
/// 用于AS5047D的SPI通信短延时
/// @param delay_time 短延时时间长短    
/// @return 无 
/// @par   修改日志
///        杨鹏程于2017-09-06创建
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初始化
///
/// 初始化AS5047D
/// @param  无   
/// @return 无 
/// @par   修改日志
///        杨鹏程于2017-09-06创建
///        LH修改于2022.0525
void as5047d_init()
{
  //总线时钟使能
  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;
  
  
  
  //垂直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数据包奇偶校验(偶校验)
///
/// 计算数据包的奇偶校验(偶校验)
/// @param  package 需要计算的数据包   
/// @return 返回奇偶校验后的数据包 
/// @par   修改日志
///        杨鹏程于2017-09-06创建
static unsigned int as5047d_Parity_bit_even(unsigned int package)
{
  unsigned int temp = 0;
  char temp1 = 0;
  ///判断数据每一位是0还是1
  for(char i = 0; i < 15; i++)
  {
    temp = package & (1 << i);
    if(temp != 0)
    {
      ///如果位数值为1则相加
      temp1++;
    }
    temp = 0;
  }
  ///temp1如果为偶数
  if(temp1 % 2 == 0)
  {
    ///将奇偶效验位置为0
    package = package & 0X7FFF; //0111 1111 1111 1111
  }
  ///temp1如果为奇数
  else
  {
    ///将奇偶效验位置为1 
    package = package | 0X8000;//1000 0000 0000 0000
  }
  return package;
}
 


/// 设定数据包read/write的类型
///
/// 设定数据包read/write的类型
/// @param  package 需要设定的数据包   
/// @return 设定后的数据包 
/// @par   修改日志
///        杨鹏程于2017-09-07创建
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;  
}

/// 判断接收到的数据
///
/// 判断接收到的数据
/// @param  package 待判断的数据
/// @param  无
/// @return 返回判断结果
/// @par   修改日志
///        杨鹏程于2017-09-08创建
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;
  //首先判断数据包的偶校验码是否正确
  //效验码错误
  if(package != as5047d_Parity_bit_even(package))
  {
    temp1.parity_error_package = 1;
    temp1.Operation_Result = 0;
  }
  //效验码正确
  else
  {
    //判断返回数据包是否是回复的指令出错内容
    temp = package & AS5047D_JUDGE_RECEIVE_DATA;
    //回复指令出错内容
    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;
  //首先判断数据包的偶校验码是否正确
  //效验码错误
  if(package != as5047d_Parity_bit_even(package))
  {
    temp1.parity_error_package = 1;
    temp1.Operation_Result = 0;
  }
  //效验码正确
  else
  {
    //判断返回数据包是否是回复的指令出错内容
    temp = package & AS5047D_JUDGE_RECEIVE_DATA;
    //回复指令出错内容
    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;
}

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




/// 水平发送数据
///
/// 向as5047d发送数据
/// @param  无 
/// @return 返回接收到的数据
/// @par   修改日志
///        杨鹏程于2017-09-08创建
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;
  //返回的上一个指令回复的数据
  return temp2;
}

/// 水平接收数据
///
/// 接收as5047d返回的数据
/// @param  无 
/// @return 返回接收到的数据
/// @par   修改日志
///        杨鹏程于2017-09-07创建
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;
}


/// 水平发送读指令
///
/// 发送读取as5047d数据指令
/// @param  add 寄存器地址   
/// @return 发送过程中接收的数据 111 1111 1111 1111
/// @par   修改日志
///        杨鹏程于2017-09-07创建
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);
   //发送指令
  temp = as5047d_hori_send_package(add);
  //返回的上一个指令回复的数据
  return temp;
}


/// 水平读取as5047d寄存器中的值
///
/// 读取as5047d寄存器中的值
/// @param  add 寄存器地址 
/// @return 返回读取到的寄存器中的值
/// @par   修改日志
///        杨鹏程于2017-09-07创建
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);
  
  //发送读指令
  as5047d_hori_read_command(add);
  
  AS5047D_HORI_CSN_DISABLE;
  as5047d_delay_nop(4);
  AS5047D_HORI_CSN_ENABLE;
  as5047d_delay_nop(4);
  
  //读取上一条指令获得的数据
  temp = as5047d_hori_nop_command();
  
  AS5047D_HORI_CSN_DISABLE;
  return temp;
}


/// 水平发送写指令
///
/// 向as5047d发送写指令
/// @param  add 寄存器地址   
/// @return 发送过程中接收的数据 
/// @par   修改日志
///        杨鹏程于2017-09-08创建
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);
  //发送指令
  temp = as5047d_hori_send_package(add);
  //返回的上一个指令回复的数据
  return temp;
}

/// 水平发送NOP指令
///
/// 向as5047d发送NOP指令
/// @param  无
/// @return 发送过程中接收的数据 
/// @par   修改日志
///        杨鹏程于2017-09-08创建
unsigned int as5047d_hori_nop_command()
{
  unsigned int temp = 0;
  temp = as5047d_hori_send_package(AS5047D_NOP);
  //返回的上一个指令回复的数据
  return temp;
}

/// 水平清除错误标记指令
///
/// 向as5047d发送清除错误标记指令CLEAR ERROR FLAG command
/// @param  无
/// @return 1清除成功，0清除失败
/// @par    修改日志
///        杨鹏程于2017-09-08创建
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;
}

/// 水平发送写指令并读取返回的数据
///
/// 向as5047d发送写指令并读取返回的数据
/// @param  add 寄存器地址 
/// @param  data 需要写入的数据
/// @return 发送过程中接收的数据 
/// @par   修改日志
///        杨鹏程于2017-09-08创建
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);
    
    //发送写入指令
    as5047d_hori_write_command(add);
    //从新写入新的数据，并返回寄存器中旧的数据
    add = as5047d_command_type(data,AS5047D_WRITE_DATA);
    add = as5047d_Parity_bit_even(data);
    as5047d_hori_send_package(data);
    //接收寄存器中的新数据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
    {
      //判断写入的数据和最新返回的寄存器数据是否相同
      if((data & 0X3FFF) == (temp.package & 0X3FFF))
      {
        temp.Operation_Result = 1;
        break;
      }
      else
      {
        continue;
      }
    }
  }
  return temp;
}

/// 读取云台水平角度
///
/// 读取云台水平角度
/// @param  无
/// @param  无
/// @return 返回读取结果。-1读取失败，其他值读取成功
/// @par   修改日志
///        杨鹏程于2017-09-27创建
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));
  //读取角度数据
  temp = as5047d_hori_read_data(AS5047D_ANGLE);
  //判断接收到的数据是否正常
  temp1 = as5047d_hori_judge_receive_package(temp);
  //数据正确
  if(temp1.Operation_Result == 1)
  {
    angle = (temp & 0x3FFF) * 0.02197265625;//0.02197265625 = 360.0 / 16384.0
    angle1 = (unsigned int)(angle * 1000.0);//只保留小数点后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;
}


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



/// 垂直发送数据
///
/// 向as5047d发送数据
/// @param  无 
/// @return 返回接收到的数据
/// @par   修改日志
///        杨鹏程于2017-09-08创建
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;
  //返回的上一个指令回复的数据
  return temp2;
}

/// 垂直接收数据
///
/// 接收as5047d返回的数据
/// @param  无 
/// @return 返回接收到的数据
/// @par   修改日志
///        杨鹏程于2017-09-07创建
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;
}


/// 垂直发送读指令
///
/// 发送读取as5047d数据指令
/// @param  add 寄存器地址   
/// @return 发送过程中接收的数据 111 1111 1111 1111
/// @par   修改日志
///        杨鹏程于2017-09-07创建
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);
   //发送指令
  temp = as5047d_vert_send_package(add);
  //返回的上一个指令回复的数据
  return temp;
}


/// 垂直读取as5047d寄存器中的值
///
/// 读取as5047d寄存器中的值
/// @param  add 寄存器地址 
/// @return 返回读取到的寄存器中的值
/// @par   修改日志
///        杨鹏程于2017-09-07创建
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);
  
  //发送读指令
  as5047d_vert_read_command(add);
  
  AS5047D_VERT_CSN_DISABLE;
  as5047d_delay_nop(4);
  AS5047D_VERT_CSN_ENABLE;
  as5047d_delay_nop(4);
  
  //读取上一条指令获得的数据
  temp = as5047d_vert_nop_command();
  
  AS5047D_VERT_CSN_DISABLE;
  return temp;
}

/// 垂直发送写指令
///
/// 向as5047d发送写指令
/// @param  add 寄存器地址   
/// @return 发送过程中接收的数据 
/// @par   修改日志
///        杨鹏程于2017-09-08创建
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);
  //发送指令
  temp = as5047d_vert_send_package(add);
  //返回的上一个指令回复的数据
  return temp;
}

/// 垂直发送NOP指令
///
/// 向as5047d发送NOP指令
/// @param  无
/// @return 发送过程中接收的数据 
/// @par   修改日志
///        杨鹏程于2017-09-08创建
unsigned int as5047d_vert_nop_command()
{
  unsigned int temp = 0;
  temp = as5047d_vert_send_package(AS5047D_NOP);
  //返回的上一个指令回复的数据
  return temp;
}

/// 垂直清除错误标记指令
///
/// 向as5047d发送清除错误标记指令CLEAR ERROR FLAG command
/// @param  无
/// @return 1清除成功，0清除失败
/// @par    修改日志
///        杨鹏程于2017-09-08创建
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;
}

/// 垂直发送写指令并读取返回的数据
///
/// 向as5047d发送写指令并读取返回的数据
/// @param  add 寄存器地址 
/// @param  data 需要写入的数据
/// @return 发送过程中接收的数据 
/// @par   修改日志
///        杨鹏程于2017-09-08创建
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);
    
    //发送写入指令
    as5047d_vert_write_command(add);
    //从新写入新的数据，并返回寄存器中旧的数据
    add = as5047d_command_type(data,AS5047D_WRITE_DATA);
    add = as5047d_Parity_bit_even(data);
    as5047d_vert_send_package(data);
    //接收寄存器中的新数据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
    {
      //判断写入的数据和最新返回的寄存器数据是否相同
      if((data & 0X3FFF) == (temp.package & 0X3FFF))
      {
        temp.Operation_Result = 1;
        break;
      }
      else
      {
        continue;
      }
    }
  }
  return temp;
}

/// 读取云台垂直角度
///
/// 读取云台垂直角度
/// @param  无
/// @param  无
/// @return 返回读取结果。-1读取失败，其他值读取成功
/// @par   修改日志
///        杨鹏程于2017-09-27创建
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));
  //读取角度数据
  temp = as5047d_vert_read_data(AS5047D_ANGLE);
  //判断接收到的数据是否正常
  temp1 = as5047d_vert_judge_receive_package(temp);
  //数据正确
  if(temp1.Operation_Result == 1)
  {
    angle = (temp & 0x3FFF) * 0.02197265625;//0.02197265625 = 360.0 / 16384.0
    angle1 = (unsigned int)(angle * 1000.0);//只保留小数点后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;
}



/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
//直接通过时钟读取角度
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;
}

//修改读取角度的方式，提升角度读取速度
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);
  
  //发送读指令
  temp = as5047d_hori_clk_read_angle();
  
  AS5047D_HORI_CSN_DISABLE;
  return temp;
}

//新的判断方式
As5047DJudgeReceivePackage as5047d_hori_judge_receive_package_a(unsigned int package)
{
  As5047DJudgeReceivePackage temp1;
  memset(&temp1,0,sizeof(temp1));
  
  temp1.package = package;
  temp1.Operation_Result = 1;
  //首先判断数据包的偶校验码是否正确
  //效验码错误
  if(package != as5047d_Parity_bit_even(package))
  {
    temp1.parity_error_package = 1;
    temp1.Operation_Result = 0;
  }
  return temp1;
}



//新的读取角度方式
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));
  //读取角度数据
  temp = as5047d_hori_read_data_a();
  //判断接收到的数据是否正常
  temp1 = as5047d_hori_judge_receive_package_a(temp);
  //数据正确
  if(temp1.Operation_Result == 1)
  {
    angle = (temp & 0x3FFF) * 0.02197265625;//0.02197265625 = 360.0 / 16384.0
    angle1 = (unsigned int)(angle * 1000.0);//只保留小数点后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;
}

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

//直接通过时钟读取角度
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;
}

//修改读取角度的方式，提升角度读取速度
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);
  
  //发送读指令
  temp = as5047d_vert_clk_read_angle();
  
  AS5047D_VERT_CSN_DISABLE;
  return temp;
}

//新的判断方式
As5047DJudgeReceivePackage as5047d_vert_judge_receive_package_a(unsigned int package)
{
  As5047DJudgeReceivePackage temp1;
  memset(&temp1,0,sizeof(temp1));
  
  temp1.package = package;
  temp1.Operation_Result = 1;
  //首先判断数据包的偶校验码是否正确
  //效验码错误
  if(package != as5047d_Parity_bit_even(package))
  {
    temp1.parity_error_package = 1;
    temp1.Operation_Result = 0;
  }
  return temp1;
}



//新的读取角度方式
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));
  //读取角度数据
  temp = as5047d_vert_read_data_a();
  //判断接收到的数据是否正常
  temp1 = as5047d_vert_judge_receive_package_a(temp);
  //数据正确
  if(temp1.Operation_Result == 1)
  {
    angle = (temp & 0x3FFF) * 0.02197265625;//0.02197265625 = 360.0 / 16384.0
    angle1 = (unsigned int)(angle * 1000.0);//只保留小数点后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

//TMR3109 操作码定义
#define TMR3109_OPCODE_WRITE_EEPROM       0x01      //001 写EEPROM
#define TMR3109_OPCODE_WRITE_REGISTER     0x05      //101 写寄存器
#define TMR3109_OPCODE_READ_REGISTER      0x06      //110 读寄存器
#define TMR3109_OPCODE_CHANGE_MODE        0x07      //111 模式切换
#define TMR3109_OPCODE_READ_ANGLE         0x03      //011 读角度

//TMR3109设备结构体
typedef struct{
    uint8_t device_id;          //设备地址
    uint32_t spi_periph;        //SPI外设地址
    uint32_t cs_pin;            //片选引脚
    uint32_t cs_gpio_periph;    //片选引脚外设地址
}TMR3109_Device;

//角度数据结构体
typedef struct{
    uint32_t angle_raw;         //23位原始角度值
    float angle_degree;         //角度值，单位：度
    uint8_t crc;                //CRC校验码
    uint8_t error;              //错误码
    bool is_valid;              //是否有效
}TMR3109_AngleData;

//TMR3109设备定义
TMR3109_Device tmr3109_dev1 = {
    .device_id = 1,
    .spi_periph = SPI2,
    .cs_pin = GPIO_PIN_15,
    .cs_gpio_periph = GPIOA,
};

TMR3109_Device tmr3109_dev2 = {
    .device_id = 2,
    .spi_periph = SPI4,
    .cs_pin = GPIO_PIN_11,
    .cs_gpio_periph = GPIOE,
};

TMR3109_AngleData hori_angle_data = {0};
TMR3109_AngleData vert_angle_data = {0};


//TMR3109初始化
void TMR3109_Init(void)
{
    //初始化SPI2（U33编码器）
    rcu_periph_clock_enable(RCU_GPIOA);
    rcu_periph_clock_enable(RCU_GPIOC);
    rcu_periph_clock_enable(RCU_SPI2);

    //配置SPI2引脚
    gpio_af_set(GPIOC, GPIO_AF_6, GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12);     //PC10,11,12复用为SPI2
    gpio_mode_set(GPIOC, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12);
    gpio_output_options_set(GPIOC, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_10 | /*GPIO_PIN_11 | */GPIO_PIN_12);      //注意一下MISO是输入模式，这里可能会有问题

    //配置CS引脚
    gpio_mode_set(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_15);        
    gpio_output_options_set(GPIOA, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_15);    
    gpio_bit_set(GPIOA, GPIO_PIN_15);                                                 //默认CS引脚高电平                                            

    //配置SPI2参数（不超过8MHz，暂定5MHz左右）
    spi_parameter_struct spi_init_struct;
    spi_struct_para_init(&spi_init_struct);
    spi_init_struct.trans_mode = SPI_TRANSMODE_FULLDUPLEX;
    spi_init_struct.device_mode = SPI_MASTER;
    spi_init_struct.frame_size = SPI_FRAMESIZE_16BIT;                //16位帧长
    spi_init_struct.clock_polarity_phase = SPI_CK_PL_LOW_PH_2EDGE;   //时钟极性为低，相位为第二边缘(下降沿)
    spi_init_struct.nss = SPI_NSS_SOFT;                              //软件控制
    spi_init_struct.prescale = SPI_PSC_16;                           //时钟预分频系数为16(5MHz左右)
    spi_init_struct.endian = SPI_ENDIAN_MSB;                         //高位先出
    spi_init(SPI2, &spi_init_struct);
    spi_enable(SPI2);

    //初始化SPI4（U32编码器）
    rcu_periph_clock_enable(RCU_GPIOE);
    rcu_periph_clock_enable(RCU_SPI4);

    //配置SPI4引脚
    gpio_af_set(GPIOE, GPIO_AF_6, GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14); //PE12,13,14复用为SPI4
    gpio_mode_set(GPIOE, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14);
    gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14);      //注意一下MISO是输入模式，这里可能会有问题

    //配置CS引脚
    gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_11);        
    gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_11);    
    gpio_bit_set(GPIOE, GPIO_PIN_11);                                                 //默认CS引脚高电平

    //配置SPI4参数（不超过8MHz，暂定5MHz左右）
    spi_parameter_struct spi_init_struct2;
    spi_struct_para_init(&spi_init_struct2);
    spi_init_struct2.trans_mode = SPI_TRANSMODE_FULLDUPLEX;
    spi_init_struct2.device_mode = SPI_MASTER;
    spi_init_struct2.frame_size = SPI_FRAMESIZE_16BIT;                //16位帧长
    spi_init_struct2.clock_polarity_phase = SPI_CK_PL_LOW_PH_2EDGE;   //时钟极性为低，相位为第二边缘(下降沿)
    spi_init_struct2.nss = SPI_NSS_SOFT;                              //软件控制
    spi_init_struct2.prescale = SPI_PSC_16;                           //时钟预分频系数为16(5MHz左右)
    spi_init_struct2.endian = SPI_ENDIAN_MSB;                         //高位先出
    spi_init(SPI4, &spi_init_struct2);
    spi_enable(SPI4);

    //初始化后延迟
}

void as5047d_init()
{
    TMR3109_Init();
}


// SPI发送接收32位数据(GD32的SPI只支持8位或16位传输，所以需要分两次发送)
static uint32_t spi_transfer_32bit(uint32_t spi_periph, uint32_t data)
{
    uint16_t temp_high, temp_low;
    uint32_t received_data = 0;
    uint32_t start_tick;
    const uint32_t timeout_ticks = 10; // 10ms超时
    
    // 提取要发送的32位数据的高16位和低16位
    temp_high = (uint16_t)(data >> 16);     // 高16位
    temp_low  = (uint16_t)(data & 0xFFFF);  // 低16位

    // 1. 先发送高16位，并读取高16位响应
    start_tick = OSTimeGet();
    while(RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TBE));    
    {
       //超时判断
       if (OSTimeGet() - start_tick > timeout_ticks) {
           // 根据SPI外设判断使用哪个片选引脚
           if (spi_periph == SPI2) {
               gpio_bit_set(GPIOA, GPIO_PIN_15); // tmr3109_dev1 CS拉高
           } else if (spi_periph == SPI4) {
               gpio_bit_set(GPIOE, GPIO_PIN_11); // tmr3109_dev2 CS拉高
           }
           return 0xFFFFFFFF;
       }
    }
    spi_i2s_data_transmit(spi_periph, temp_high);
    
    start_tick = OSTimeGet();
    while(RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE));
    {
       if (OSTimeGet() - start_tick > timeout_ticks) {
           if (spi_periph == SPI2) {
               gpio_bit_set(GPIOA, GPIO_PIN_15);
           } else if (spi_periph == SPI4) {
               gpio_bit_set(GPIOE, GPIO_PIN_11);
           }
           return 0xFFFFFFFF;
       }
    }
    received_data = (uint32_t)spi_i2s_data_receive(spi_periph) << 16; // 存储到高16位

    // 2. 紧接着发送低16位，并读取低16位响应
    start_tick = OSTimeGet();
    while(RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TBE));
    {
       if (OSTimeGet() - start_tick > timeout_ticks) {
           if (spi_periph == SPI2) {
               gpio_bit_set(GPIOA, GPIO_PIN_15);
           } else if (spi_periph == SPI4) {
               gpio_bit_set(GPIOE, GPIO_PIN_11);
           }
           return 0xFFFFFFFF;
       }
    }
    spi_i2s_data_transmit(spi_periph, temp_low);
    
    start_tick = OSTimeGet();
    while(RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE));
    {
       if (OSTimeGet() - start_tick > timeout_ticks) {
           if (spi_periph == SPI2) {
               gpio_bit_set(GPIOA, GPIO_PIN_15);
           } else if (spi_periph == SPI4) {
               gpio_bit_set(GPIOE, GPIO_PIN_11);
           }
           return 0xFFFFFFFF;
       }
    }
    received_data |= spi_i2s_data_receive(spi_periph); // 存储到低16位

    return received_data;
}

//设置CS引脚
static void set_cs(TMR3109_Device* dev, bool state)
{
    if(state)
    {
        gpio_bit_set(dev->cs_gpio_periph, dev->cs_pin);
    }
    else
    {
        gpio_bit_reset(dev->cs_gpio_periph, dev->cs_pin);
    }
}

/**
 * @brief 计算TMR3109角度数据的CRC4校验码
 * @param angle_data 23位角度数据
 * @return 4位CRC校验码
 */
uint8_t TMR3109_CalculateCRC(uint32_t angle_data_23bit)
{
    // 多项式: x^4 + x^3 + x^2 + 1 = 0b1101 = 0x0D
    const uint8_t polynomial = 0x0D;
    
    // 初始值: 0b0011 = 0x3
    uint8_t crc = 0x3;
    
    // 构建24位数据: 1位0 + 23位角度值
    uint32_t data = angle_data_23bit;  // 低23位是角度值
    // 最高位已经是0（因为angle_data_23bit只有23位）
    
    // 处理24位数据（从最高位开始）
    for (int i = 23; i >= 0; i--) {
        // 检查CRC最高位
        uint8_t crc_msb = (crc >> 3) & 0x1;
        
        // 获取当前数据位
        uint8_t data_bit = (data >> i) & 0x1;
        
        // CRC左移1位，最低位补0
        crc = (crc << 1) & 0x0F;
        
        // 如果CRC最高位与数据位异或结果为1，则与多项式异或
        if (crc_msb ^ data_bit) {
            crc ^= polynomial;
        }
    }
    
    return crc & 0x0F;  // 确保返回4位
}

/**
 * @brief 验证TMR3109角度数据的CRC
 * @param angle_data_23bit 23位角度数据
 * @param received_crc 接收到的4位CRC
 * @return true表示CRC校验通过
 */
bool TMR3109_CheckCRC(uint32_t angle_data_23bit, uint8_t received_crc)
{
    uint8_t calculated_crc = TMR3109_CalculateCRC(angle_data_23bit);
    return (calculated_crc == received_crc);
}

//读角度
bool TMR3109_ReadAngle(TMR3109_Device* dev, TMR3109_AngleData* angle_data)
{
    if(dev == NULL || angle_data == NULL) return false;

    //读角度命令： 操作码（3bit）+地址（8bit）+空闲（5bit）+数据（16bit）
    uint32_t tx_data = (TMR3109_OPCODE_READ_ANGLE << 29)| (0X00 << 21) | (0X00 << 16) | (0X00 << 0) ;

    set_cs(dev, false);    //使能CS
    delay_us(20);          //SPI启动时间 优化点

    //发送32位数据并接收响应
    uint32_t rx_data = spi_transfer_32bit(dev->spi_periph, tx_data);

    set_cs(dev, true);     //拉高片选

    //解析响应数据
    //d27~d5: 23位角度数据，d4~d1: CRC校验码，d0: error位
    angle_data->angle_raw = (rx_data >> 5) & 0X7FFFFF;      //取23位角度数据
    angle_data->crc      = (rx_data >> 1) & 0X0F;           //取4位CRC校验码
    angle_data->error    = rx_data & 0X01;                  //取1位error位

    //计算角度值((∑_{i=0}^{23} Angle<i> * 2^i) / 8388608 * 360)
    angle_data->angle_degree = (angle_data->angle_raw * 360.0f) / 8388608.0f; 

    //数据有效性校验(加入CRC校验)
    bool crc_valid = TMR3109_CheckCRC(angle_data->angle_raw, angle_data->crc);
    // angle_data->is_valid = 0/*(angle_data->error == 0)*/;       //注意这里的error位有问题，手册没找到error位的具体定义
    angle_data->is_valid = crc_valid;

    //CRC错误记录逻辑，后期添加
    // if(!crc_valid) {
    //     //CRC校验失败，记录错误信息
    //     //...
    // }

    return angle_data->is_valid;    
}

float as5047d_hori_get_angle_a()
{
    if (TMR3109_ReadAngle(&tmr3109_dev1, &hori_angle_data)) {
        return hori_angle_data.angle_degree;
    }
    return 0.0f;
}

float as5047d_vert_get_angle_a()
{
    if (TMR3109_ReadAngle(&tmr3109_dev2, &vert_angle_data)) {
        return vert_angle_data.angle_degree;
    }
    return 0.0f;
}

#endif
