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