robot/RBcore/TL720D.c

/******************************************************************************

                  版权所有 (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"

/*----------------------------------------------*
 * 外部变量说明                                 *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 外部函数原型说明                             *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 内部函数原型说明                             *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 全局变量                                     *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 模块级变量                                   *
 *----------------------------------------------*/
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)
{
	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]);

	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);
}