#include "as5047d.h" #include #include #include #include #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 * 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