966 lines
22 KiB
C
966 lines
22 KiB
C
#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;
|
||
}
|
||
|
||
/********************************************************************************/
|
||
/********************************************************************************/
|
||
/********************************************************************************/
|
||
|