增加TL720D陀螺仪适配及IV显示

RBcore/TL720D.c

@@ -0,0 +1,146 @@
+/******************************************************************************
+
+                  版权所有 (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);
+}

RBcore/drv_interface.c

@@ -254,6 +254,7 @@ extern void decode_PV(const char *buf, uint32_t _iSize);
 extern void ground_manger_init(void);
 extern void Custom_Task(void *argument);
 void SendIV_Init(void);
+void TL720D_Init(void);
 
 void Drv_InterfaceInit(void)
 {
@@ -303,4 +304,6 @@ void Drv_InterfaceInit(void)
 		.priority = (osPriority_t) osPriorityRealtime1,
 	};
 	(void)osThreadNew(Read_PV, NULL, &Read_PV_attributes);
+
+	TL720D_Init();
 }

RBcore/include/BHBF.h

@@ -95,6 +95,7 @@ typedef enum {
 	CUSTOM_GET_PV,				 // 获取PV
 	CUSTOM_SET_IV,				 // 设置IV
 	CUSTOM_GET_MK32,		     // 获取MK32
+	CUSTOM_GET_TL720D_ROLL,		 // 获取陀螺仪横滚角
 	CUSTOM_CMD_PAINTGUN,         // 控制喷枪
 	CUSTOM_CMD_END = 0x0300,
 } Motor_Cmd_e;

project/paint_robot_new/paint_robot_new.c

@@ -67,6 +67,14 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 
     switch (pstMsg->m_uiMsgID)
     {
+    	case CUSTOM_GET_TL720D_ROLL:
+    	{
+    		if (pstMsg->m_uiDataLen >= sizeof(int32_t))
+			{
+				RD_MEMCPY(&g_stIV.CurrentAngle, pstMsg->m_aucData, sizeof(int32_t));
+			}
+    		break;
+    	}
 		case CUSTOM_GET_PV:
 		{
 			if (pstMsg->m_uiDataLen >= sizeof(g_stPV))