MW22-02A/BSP/Driver/as5047d/as5047d.c

966 lines
22 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

#include "as5047d.h"
#include <includes.h>
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;
}
/********************************************************************************/
/********************************************************************************/
/********************************************************************************/