paint_robot_new【编译通过】增加电机状态接收

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