|
|
@ -57,6 +57,8 @@ static IV_struct_define g_stIV; |
|
|
* 宏定义 * |
|
|
* 宏定义 * |
|
|
*----------------------------------------------*/ |
|
|
*----------------------------------------------*/ |
|
|
|
|
|
|
|
|
|
|
|
#define IV_SEND_TIME 500 // IV上报周期(单位毫秒)
|
|
|
|
|
|
|
|
|
static void Custom_ModuleHandler(const Msg_t *pstMsg) |
|
|
static void Custom_ModuleHandler(const Msg_t *pstMsg) |
|
|
{ |
|
|
{ |
|
|
if (NULL == pstMsg) |
|
|
if (NULL == pstMsg) |
|
|
@ -89,6 +91,15 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) |
|
|
RD_MEMCPY(&g_stMK32, pstMsg->m_aucData, sizeof(g_stMK32)); |
|
|
RD_MEMCPY(&g_stMK32, pstMsg->m_aucData, sizeof(g_stMK32)); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
int iSpeedSelection = 3 * (g_stMK32.CH11_RD1 + 1000) / 200; |
|
|
|
|
|
int iVehicleSpeed = 1; |
|
|
|
|
|
if (iSpeedSelection > 0) |
|
|
|
|
|
{ |
|
|
|
|
|
iVehicleSpeed = g_stCV.Speed_m_per_min*iSpeedSelection/30; |
|
|
|
|
|
} |
|
|
|
|
|
g_stIV.RobotMoveSpeed = iVehicleSpeed; |
|
|
|
|
|
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_GET_VEHICLE_SPEED, (void *)&iVehicleSpeed, sizeof(int)); |
|
|
|
|
|
|
|
|
if (abs(g_stMK32.CH2_LY_V) <= g_stCV.Joy_Sticker_Value_Allowance && abs(g_stMK32.CH3_LY_H) <= g_stCV.Joy_Sticker_Value_Allowance) |
|
|
if (abs(g_stMK32.CH2_LY_V) <= g_stCV.Joy_Sticker_Value_Allowance && abs(g_stMK32.CH3_LY_H) <= g_stCV.Joy_Sticker_Value_Allowance) |
|
|
{ |
|
|
{ |
|
|
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); |
|
|
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); |
|
|
@ -151,10 +162,17 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) |
|
|
void Custom_Task(void *argument) |
|
|
void Custom_Task(void *argument) |
|
|
{ |
|
|
{ |
|
|
g_uiCustomModuleID = MsgCenter_Register(MODULE_NAME_CUSTOM, Custom_ModuleHandler); |
|
|
g_uiCustomModuleID = MsgCenter_Register(MODULE_NAME_CUSTOM, Custom_ModuleHandler); |
|
|
|
|
|
uint32_t uiLastSendTick = Rd_GetTime(); |
|
|
while(1) |
|
|
while(1) |
|
|
{ |
|
|
{ |
|
|
MsgCenter_ProcessWait(g_uiCustomModuleID, 2); |
|
|
MsgCenter_ProcessWait(g_uiCustomModuleID, 2); |
|
|
|
|
|
|
|
|
|
|
|
uint32_t uiNowTick = Rd_GetTime(); |
|
|
|
|
|
if ((int32_t)(uiNowTick - uiLastSendTick) >= IV_SEND_TIME) |
|
|
|
|
|
{ |
|
|
|
|
|
uiLastSendTick = uiNowTick; |
|
|
MsgCenter_SendTo(MODULE_NAME_SENDIV, CUSTOM_SET_IV, (void *)&g_stIV, sizeof(g_stIV)); |
|
|
MsgCenter_SendTo(MODULE_NAME_SENDIV, CUSTOM_SET_IV, (void *)&g_stIV, sizeof(g_stIV)); |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|