1289 lines
33 KiB
C
1289 lines
33 KiB
C
#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};
|
||
|
||
//暂定微秒级延迟函数(基于SysTick)(后期删除，所有延迟函数更换为统一的函数)
|
||
void delay_us(uint32_t us)
|
||
{
|
||
uint32_t start, now, delta, reload, us_tick;
|
||
start = SysTick->VAL;
|
||
reload = SysTick->LOAD;
|
||
us_tick = SystemCoreClock / 1000000UL;
|
||
|
||
do {
|
||
now = SysTick->VAL;
|
||
delta = start > now? start - now : reload + start - now;
|
||
} while (delta < us_tick * us);
|
||
}
|
||
|
||
//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
|