#include "read_spi.h"

float angle_encoder;
volatile int32_t multi_turn_angle = 0;
volatile uint16_t last_angle = 0;


// 计算奇偶函数
uint16_t Parity_bit_Calculate(uint16_t data_2_cal)
{
	uint16_t parity_bit_value=0;
	while(data_2_cal != 0)
	{
		parity_bit_value ^= data_2_cal; 
		data_2_cal >>=1;
	}
	return (parity_bit_value & 0x1); 
}
// SPI发送读取函数
uint16_t SPI_ReadWrite_OneByte(uint16_t _txdata)
{

	//AS5407P_CS_L	cs拉低
  AS5407P_CS_L();
	uint16_t rxdata;
	if(HAL_SPI_TransmitReceive(&hspi2,(uint8_t *)&_txdata,(uint8_t *)&rxdata,1,1000) !=HAL_OK)
		rxdata=0;
  //AS5407P_CS_H		//cs拉高
  AS5407P_CS_H();
	return rxdata;
}
uint16_t AS5047_read(uint16_t add)
{
	uint16_t data;
	add |= 0x4000;	//读指令 bit14 置1
	if(Parity_bit_Calculate(add)==1) add=add|0x8000; //如果前15位 1的个数位偶数，则Bit15 置1
	SPI_ReadWrite_OneByte(add);		//发送一条指令，不管读回的数据
		data=SPI_ReadWrite_OneByte(NOP|0x4000); //发送一条空指令，读取上一次指令返回的数据
	data &=0x3fff;
	return data;
}

// void Update_MultiTurn_Angle(void)
// {
//     uint16_t current_angle = AS5047_read(ANGLEUNC);
//     int16_t angle_diff = current_angle - last_angle;

//     // 检测角度跳变
//     if (angle_diff > 8192) // 角度从低到高跳变
//     {
//         multi_turn_angle -= (16384 - angle_diff);
//     }
//     else if (angle_diff < -8192) // 角度从高到低跳变
//     {
//         multi_turn_angle += (16384 + angle_diff);
//     }
//     else
//     {
//         multi_turn_angle += angle_diff;
//     }

//     last_angle = current_angle;
// }
void Update_MultiTurn_Angle(void)
{
    uint16_t current_angle = AS5047_read(ANGLEUNC);
    int16_t angle_diff = current_angle - last_angle;

    // 检测角度跳变
    if (angle_diff > 8192) // 角度从低到高跳变
    {
        multi_turn_angle -= 1;
    }
    else if (angle_diff < -8192) // 角度从高到低跳变
    {
        multi_turn_angle += 1;
    }

    last_angle = current_angle;
}