81 lines
2.0 KiB
C
81 lines
2.0 KiB
C
#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;
|
||
}
|