You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
154 lines
4.1 KiB
154 lines
4.1 KiB
/******************************************************************************
|
|
|
|
版权所有 (C), 2018-2099, Radkil
|
|
|
|
******************************************************************************
|
|
文 件 名 : TL720D.c
|
|
版 本 号 : 初稿
|
|
作 者 : radkil
|
|
生成日期 : 2026年9月8日
|
|
最近修改 :
|
|
功能描述 : 机器人本体陀螺仪解析
|
|
|
|
修改历史 :
|
|
1.日 期 : 2026年9月8日
|
|
作 者 : radkil
|
|
修改内容 : 创建文件
|
|
|
|
******************************************************************************/
|
|
#include "msp_TL720D.pb.h"
|
|
#include "BHBF.h"
|
|
#include "msg_center.h"
|
|
#include "rd_time.h"
|
|
|
|
/*----------------------------------------------*
|
|
* 外部变量说明 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 外部函数原型说明 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 内部函数原型说明 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 全局变量 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 模块级变量 *
|
|
*----------------------------------------------*/
|
|
static MSP_TL720DParameters g_stTL720D = {0};
|
|
|
|
/*----------------------------------------------*
|
|
* 常量定义 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 宏定义 *
|
|
*----------------------------------------------*/
|
|
|
|
/* 数值计算
|
|
* data: 数据地址
|
|
* return: 单位: 0.01
|
|
* */
|
|
static int16_t getDeci(uint8_t *data)
|
|
{
|
|
char isNegative = 0;
|
|
|
|
if (*data >> 4)
|
|
{
|
|
isNegative = 1;
|
|
} else
|
|
{
|
|
isNegative = 0;
|
|
}
|
|
|
|
int16_t data_value = 0;
|
|
data_value = ((*data) & 0x0f) * 10000;
|
|
data++;
|
|
data_value += (*data >> 4) * 1000;
|
|
data_value += ((*data) & 0x0f) * 100;
|
|
data++;
|
|
|
|
int16_t xiaoshu = 0;
|
|
xiaoshu = (*data >> 4) * 10;
|
|
xiaoshu += (*data) & 0x0f;
|
|
if (isNegative)
|
|
{
|
|
return -(data_value + xiaoshu);
|
|
} else
|
|
{
|
|
return (data_value + xiaoshu);
|
|
}
|
|
}
|
|
|
|
static int check_TL720D(char *_pBuffer, uint32_t _iSize)
|
|
{
|
|
uint8_t check_sum = 0;
|
|
for(uint8_t i = 1; i < 31; i++)
|
|
{
|
|
check_sum += (uint8_t)_pBuffer[i];
|
|
}
|
|
|
|
if(check_sum != _pBuffer[31])
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
if (_pBuffer[0] != 0x68) return -1;
|
|
if (_pBuffer[1] != 0x1f) return -1;
|
|
if (_pBuffer[2] != 0x00) return -1;
|
|
if (_pBuffer[3] != 0x84) return -1;
|
|
|
|
return 32;
|
|
}
|
|
|
|
static void decode_TL720D(const char *buf, uint32_t _iSize)
|
|
{
|
|
static uint32_t uiLastSendTick = 0;
|
|
|
|
g_stTL720D.RF_Angle_Roll = getDeci((uint8_t *)&buf[4]);
|
|
g_stTL720D.RF_Angle_Pitch = getDeci((uint8_t *)&buf[7]);
|
|
g_stTL720D.RF_Angle_Yaw = getDeci((uint8_t *)&buf[10]);
|
|
g_stTL720D.RF_Acc_X = getDeci((uint8_t *)&buf[13]);
|
|
g_stTL720D.RF_Acc_Y = getDeci((uint8_t *)&buf[16]);
|
|
g_stTL720D.RF_Acc_Z = getDeci((uint8_t *)&buf[19]);
|
|
g_stTL720D.RF_Gro_X = getDeci((uint8_t *)&buf[22]);
|
|
g_stTL720D.RF_Gro_Y = getDeci((uint8_t *)&buf[25]);
|
|
g_stTL720D.RF_Gro_Z = getDeci((uint8_t *)&buf[28]);
|
|
|
|
uint32_t uiNowTick = Rd_GetTime();
|
|
if ((int32_t)(uiNowTick - uiLastSendTick) >= 50)
|
|
{
|
|
uiLastSendTick = uiNowTick;
|
|
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_GET_TL720D_ROLL, (void *)&g_stTL720D.RF_Angle_Roll, sizeof(int32_t));
|
|
}
|
|
}
|
|
|
|
void Read_TL720D(void *argument)
|
|
{
|
|
char pcBuffer[64] = {0};
|
|
|
|
while(1)
|
|
{
|
|
rd_ComRead(g_ptrs485_1, pcBuffer, 64);
|
|
Rd_Delay(1);
|
|
}
|
|
}
|
|
|
|
void TL720D_Init(void)
|
|
{
|
|
TUartUserData *pt4851UserData = UART_userdata_init(1, -1, 512);
|
|
g_ptrs485_1 = rd_ComCreate(check_TL720D, decode_TL720D, RS485_1_Send, pt4851UserData->m_buf_size, pt4851UserData);
|
|
UART_IT_init(g_ptrs485_1);
|
|
|
|
const osThreadAttr_t Read_TL720D_attributes = {
|
|
.name = "Read_TL720D",
|
|
.stack_size = 2048,
|
|
.priority = (osPriority_t) osPriorityRealtime1,
|
|
};
|
|
(void)osThreadNew(Read_TL720D, NULL, &Read_TL720D_attributes);
|
|
}
|
|
|