From ac2124de571bdd228e147cefbefe70dc3a0dd5b0 Mon Sep 17 00:00:00 2001
From: Lizongdi <1210855344@qq.com>
Date: Tue, 14 Jul 2026 15:10:17 +0800
Subject: [PATCH] =?UTF-8?q?paint=5Frobot=5Fnew=E3=80=90=E7=BC=96=E8=AF=91?=
 =?UTF-8?q?=E9=80=9A=E8=BF=87=E3=80=91=E5=A2=9E=E5=8A=A0=E7=94=B5=E6=9C=BA?=
 =?UTF-8?q?=E7=8A=B6=E6=80=81=E6=8E=A5=E6=94=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 RBcore/drv_interface.c | 52 ++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 52 insertions(+)

diff --git a/RBcore/drv_interface.c b/RBcore/drv_interface.c
index 6ef0af4..21596e7 100644
--- a/RBcore/drv_interface.c
+++ b/RBcore/drv_interface.c
@@ -256,6 +256,52 @@ void SendAll_Task(void *argument)
 }
 
 extern void MotorInit(void);
+extern void MotorMgr_ParseResponse(uint8_t ucMotorID, const uint8_t *pucData, uint32_t uiLen);
+
+// 雷赛电机CAN响应检查函数
+// 雷赛响应ID范围：0x581 - 0x5FF (0x580 + MotorID)
+static int check_LeiSaiMotor(char *_pBuffer, uint32_t _iSize)
+{
+    if (NULL == _pBuffer || _iSize < 4)
+    {
+        return 0;
+    }
+
+    // 从buffer中提取CAN ID（前4字节）
+    uint32_t uiFrameID = (uint32_t)(_pBuffer[0] & 0xFF) 
+                        | ((_pBuffer[1] & 0xFF) << 8) 
+                        | ((_pBuffer[2] & 0xFF) << 16) 
+                        | ((_pBuffer[3] & 0xFF) << 24);
+
+    // 雷赛响应ID范围：0x581 - 0x5FF
+    if (uiFrameID >= 0x581 && uiFrameID <= 0x5FF)
+    {
+        return 8 + 4; // 数据长度8 + CAN ID长度4
+    }
+
+    return 0;
+}
+
+// 雷赛电机CAN响应解码函数
+static void decode_LeiSaiMotor(const char *_pBuffer, uint32_t _iSize)
+{
+    if (NULL == _pBuffer || _iSize < 12) // 4字节CAN ID + 8字节数据
+    {
+        return;
+    }
+
+    // 从buffer中提取CAN ID
+    uint32_t uiFrameID = (uint32_t)(_pBuffer[0] & 0xFF) 
+                        | ((_pBuffer[1] & 0xFF) << 8) 
+                        | ((_pBuffer[2] & 0xFF) << 16) 
+                        | ((_pBuffer[3] & 0xFF) << 24);
+
+    // 提取电机ID (0x580 + MotorID)
+    uint8_t ucMotorID = (uint8_t)(uiFrameID - 0x580);
+
+    // 调用电机管理模块解析响应
+    MotorMgr_ParseResponse(ucMotorID, (const uint8_t *)&_pBuffer[4], 8);
+}
 
 void Drv_InterfaceInit(void)
 {
@@ -274,5 +320,11 @@ void Drv_InterfaceInit(void)
     (void)osThreadNew(SendAll_Task, NULL, &send_task_attributes);
 
     MsgCenter_Init();
+
+    // 初始化FDCAN1，使用雷赛电机的回调
+    TCANUserData *ptCANUserData = CAN_userdata_init(&hfdcan1, 8, 0);
+    g_ptFDCAN1 = rd_ComCreate(check_LeiSaiMotor, decode_LeiSaiMotor, FDCAN1_Send, CONFIG_UART_BUFFER_SIZE, ptCANUserData);
+    CAN_IT_init(g_ptFDCAN1);
+
     MotorInit();
 }