From e994856340564cc7f994fcf8c2b66ff05fff067a Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Tue, 8 Sep 2026 17:15:32 +0800 Subject: [PATCH] =?UTF-8?q?=E6=94=B9=E4=B8=BA=E5=9C=A8LeiSai=5FParseRespon?= =?UTF-8?q?se=E4=B8=AD=E7=9B=B4=E6=8E=A5=E5=90=91custom=E5=8F=91=E6=B6=88?= =?UTF-8?q?=E6=81=AF=EF=BC=8C=E5=A2=9E=E5=8A=A0=E9=80=9F=E5=BA=A6=E5=8F=8D?= =?UTF-8?q?=E5=90=91=E8=BD=AC=E6=8D=A2=E5=87=BD=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- motor/CMakeLists.txt | 6 +++++ motor/include/motor_manager.h | 1 + motor/include/motor_protocol.h | 3 +++ motor/motor_manager.c | 17 +++++++++++++++ motor/msp_motor_leisai.c | 40 ++++++++++++++++++++++++++++++++-- 5 files changed, 65 insertions(+), 2 deletions(-) diff --git a/motor/CMakeLists.txt b/motor/CMakeLists.txt index e275e8d..d904896 100644 --- a/motor/CMakeLists.txt +++ b/motor/CMakeLists.txt @@ -13,4 +13,10 @@ if(TARGET peripheral) target_link_libraries(${TARGET_NAME} PUBLIC peripheral) else() message(FATAL_ERROR "[${TARGET_NAME}] Dependency 'peripheral' not found.") +endif() + +if(TARGET RBcore) + target_link_libraries(${TARGET_NAME} PUBLIC RBcore) +else() + message(FATAL_ERROR "[${TARGET_NAME}] Dependency 'RBcore' not found.") endif() \ No newline at end of file diff --git a/motor/include/motor_manager.h b/motor/include/motor_manager.h index e0a9976..afa4588 100644 --- a/motor/include/motor_manager.h +++ b/motor/include/motor_manager.h @@ -163,6 +163,7 @@ void MotorMgr_ParseResponse(uint8_t ucMotorID, const uint8_t *pucData, uint32_t * @brief 速度单位转换 */ int32_t MotorMgr_SpeedConvert(Motor_Instance_t *pstMotor, int32_t iSpeedUserUnit); +int32_t MotorMgr_SpeedConvertex(Motor_Instance_t *pstMotor, int32_t iSpeedCount); /** * @brief 获取电机状态 diff --git a/motor/include/motor_protocol.h b/motor/include/motor_protocol.h index 436ef45..a60b4d0 100644 --- a/motor/include/motor_protocol.h +++ b/motor/include/motor_protocol.h @@ -115,6 +115,9 @@ typedef struct { // 速度单位转换：用户单位 -> 脉冲/秒 int32_t (*pfSpeedConvert)(struct Motor_Instance *pstMotor, int32_t iSpeedUserUnit); + + // 速度单位转换：脉冲/秒 -> 用户单位 + int32_t (*pfSpeedConvertex)(struct Motor_Instance *pstMotor, int32_t iSpeedCount); } Motor_Protocol_t; // 电机实例结构体 diff --git a/motor/motor_manager.c b/motor/motor_manager.c index f15c453..0312055 100644 --- a/motor/motor_manager.c +++ b/motor/motor_manager.c @@ -468,6 +468,23 @@ int32_t MotorMgr_SpeedConvert(Motor_Instance_t *pstMotor, int32_t iSpeedUserUnit return 0; } +/***************************************************************************** + 函 数 名 : MotorMgr_SpeedConvert + 功能描述 : 速度单位转换 + 输入参数 : Motor_Instance_t *pstMotor 电机实例 + int32_t iSpeedCount 脉冲/秒 + 输出参数 : 无 + 返 回 值 : int32_t 用户速度单位 +*****************************************************************************/ +int32_t MotorMgr_SpeedConvertex(Motor_Instance_t *pstMotor, int32_t iSpeedCount) +{ + if (pstMotor->m_pstProtocol && pstMotor->m_pstProtocol->pfSpeedConvertex) + { + return pstMotor->m_pstProtocol->pfSpeedConvertex(pstMotor, iSpeedCount); + } + return 0; +} + /***************************************************************************** 函 数 名 : MotorMgr_GetState 功能描述 : 获取电机状态 diff --git a/motor/msp_motor_leisai.c b/motor/msp_motor_leisai.c index d0f18d8..6e3bc29 100644 --- a/motor/msp_motor_leisai.c +++ b/motor/msp_motor_leisai.c @@ -19,6 +19,7 @@ #include "msp_motor_leisai.h" #include "rd_time.h" +#include "BHBF.h" /*----------------------------------------------* * 外部变量说明 * @@ -52,6 +53,7 @@ static int LeiSai_SetTargetPosition(Motor_Instance_t *pstMotor, int32_t iTargetP static int LeiSai_SetDO(Motor_Instance_t *pstMotor, uint8_t ucDoIndex, uint8_t ucState); static int LeiSai_ParseResponse(Motor_Instance_t *pstMotor, const uint8_t *pucData, uint32_t uiLen); static int32_t LeiSai_SpeedConvert(Motor_Instance_t *pstMotor, int32_t iSpeedE01Mmin); +static int32_t LeiSai_SpeedConvertex(Motor_Instance_t *pstMotor, int32_t iSpeedCount); /*----------------------------------------------* * 全局变量 * @@ -76,6 +78,7 @@ const Motor_Protocol_t g_stLeiSaiProtocol = { .pfSetDO = LeiSai_SetDO, .pfParseResponse = LeiSai_ParseResponse, .pfSpeedConvert = LeiSai_SpeedConvert, + .pfSpeedConvertex = LeiSai_SpeedConvertex, }; /*----------------------------------------------* @@ -393,12 +396,29 @@ static int LeiSai_ParseResponse(Motor_Instance_t *pstMotor, const uint8_t *pucDa { case LEISAI_OD_FAULT_CODE: { - pstMotor->m_eState = MOTOR_STATE_ERROR; + uint32_t uiFaultCode[2] = {0}; + uiFaultCode[1] = (uint32_t)(pucData[4] + | (pucData[5] << 8) + | (pucData[6] << 16) + | (pucData[7] << 24)); + uiFaultCode[0] = pstMotor->m_stConfig.m_ucMotorID; + if (uiFaultCode[1] != 0) + { + pstMotor->m_eState = MOTOR_STATE_ERROR; + } + MsgCenter_SendTo(MODULE_NAME_CUSTOM, MOTOR_CMD_GET_FAULT_CODE, uiFaultCode, sizeof(uiFaultCode)); break; } case LEISAI_OD_VELOCITY_ACTUAL: { - + int32_t iRealVelocity[2] = {0}; + iRealVelocity[0] = (int32_t)(pucData[4] + | (pucData[5] << 8) + | (pucData[6] << 16) + | (pucData[7] << 24)); + iRealVelocity[1] = MotorMgr_SpeedConvertex(pstMotor, iRealVelocity[0]); + iRealVelocity[0] = pstMotor->m_stConfig.m_ucMotorID; + MsgCenter_SendTo(MODULE_NAME_CUSTOM, MOTOR_CMD_GET_SPEED, iRealVelocity, sizeof(iRealVelocity)); break; } default: @@ -427,3 +447,19 @@ static int32_t LeiSai_SpeedConvert(Motor_Instance_t *pstMotor, int32_t iSpeedE01 return (int32_t)dPulsePerSec; } + +/***************************************************************************** + 函 数 名 : LeiSai_SpeedConvertex + 功能描述 : 速度单位转换：脉冲/秒 -> m/min +*****************************************************************************/ +static int32_t LeiSai_SpeedConvertex(Motor_Instance_t *pstMotor, int32_t iSpeedCount) +{ + if (NULL == pstMotor) + { + return 0; + } + + double dPulsePerSec = iSpeedCount*60.0/pstMotor->m_stConfig.m_uiPulsePerRound/pstMotor->m_stConfig.m_uiReductionRatio* 3.14 * pstMotor->m_stConfig.m_fWheelDiameter; + + return (int32_t)dPulsePerSec; +}