改为在LeiSai_ParseResponse中直接向custom发消息，增加速度反向转换函数

motor/CMakeLists.txt

@@ -14,3 +14,9 @@ if(TARGET 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()

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 获取电机状态

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;
 
 // 电机实例结构体

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
  功能描述  : 获取电机状态

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,
 };
 
 /*----------------------------------------------*
@@ -392,13 +395,30 @@ static int LeiSai_ParseResponse(Motor_Instance_t *pstMotor, const uint8_t *pucDa
     switch (usFunctionCode)
     {
         case LEISAI_OD_FAULT_CODE:
+        {
+        	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;
+}