diff --git a/RBcore/BHBF.c b/RBcore/BHBF.c index f6ae3c1..20090c3 100644 --- a/RBcore/BHBF.c +++ b/RBcore/BHBF.c @@ -38,6 +38,7 @@ * 全局变量 * *----------------------------------------------*/ static uint32_t g_uiRBcoreModuleID = 0; +static CV_struct_define CV = {0}; /*----------------------------------------------* * 模块级变量 * @@ -91,11 +92,12 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg) void RBcore_Task(void *argument) { - GF_BSP_EEPROM_Init(); + GF_BSP_EEPROM_ReadBytes((uint8_t *)&CV, GF_BSP_EEPROM_CV_struct_define_Start_Address, sizeof(CV_struct_define)); while(1) { // 处理消息 MsgCenter_ProcessWait(g_uiRBcoreModuleID, 2); + MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_GET_CV, (void *)&CV, sizeof(CV)); } } diff --git a/RBcore/client_setting.c b/RBcore/client_setting.c index 170f1e1..8777d33 100644 --- a/RBcore/client_setting.c +++ b/RBcore/client_setting.c @@ -19,7 +19,9 @@ #include "BHBF.h" #include "pb.h" #include "pb_decode.h" +#include "pb_encode.h" #include "bsp_PV.pb.h" +#include "bsp_IV.pb.h" /*----------------------------------------------* * 外部变量说明 * @@ -40,8 +42,11 @@ /*----------------------------------------------* * 模块级变量 * *----------------------------------------------*/ +static uint32_t g_uiIVModuleID = 0; static PV_struct_define decoded_PV_variable = { 0 }; static PV_struct_define decoded_PV = { 0 }; +static IV_struct_define IV = { 0 }; +static char g_IV_buffer[1024] = {0}; /*----------------------------------------------* * 常量定义 * *----------------------------------------------*/ @@ -95,12 +100,57 @@ void decode_PV(const char *_pBuffer, uint32_t _iSize) } } -void Send_IV(void *argument) +static void SendIV_ModuleHandler(const Msg_t *pstMsg) { - while(1) + if (NULL == pstMsg) { + return; + } - Rd_Delay(1); + switch (pstMsg->m_uiMsgID) + { + case CUSTOM_SET_IV: + { + if (pstMsg->m_uiDataLen >= sizeof(IV)) + { + RD_MEMCPY(&IV, pstMsg->m_aucData, sizeof(IV)); + pb_ostream_t IV_o_stream = pb_ostream_from_buffer((pb_byte_t *)&g_IV_buffer[2], sizeof(g_IV_buffer) - 2); + pb_encode(&IV_o_stream, IV_struct_define_fields, &IV); + + g_IV_buffer[0] = 0x55; + g_IV_buffer[1] = 0x55; + + uint16_t crc = Rd_modbusCRC16((const uint8_t *)&(g_IV_buffer[0]), IV_o_stream.bytes_written + 2); + + g_IV_buffer[IV_o_stream.bytes_written + 2] = (crc>>8) &0xff; + g_IV_buffer[IV_o_stream.bytes_written + 3] = crc & 0xff;; + + rd_ComSend(g_ptlpuart, g_IV_buffer, IV_o_stream.bytes_written + 4); + Rd_Delay(1000); + } + break; + } + default: + break; } } +static void Send_IV(void *argument) +{ + while(1) + { + MsgCenter_ProcessWait(g_uiIVModuleID, 2); + } +} + +void SendIV_Init(void) +{ + g_uiIVModuleID = MsgCenter_Register(MODULE_NAME_SENDIV, SendIV_ModuleHandler); + + const osThreadAttr_t Send_PV_attributes = { + .name = MODULE_NAME_SENDIV, + .stack_size = 1024, + .priority = (osPriority_t) osPriorityHigh4, + }; + (void)osThreadNew(Send_IV, NULL, &Send_PV_attributes); +} diff --git a/RBcore/drv_interface.c b/RBcore/drv_interface.c index af47fed..348fdcd 100644 --- a/RBcore/drv_interface.c +++ b/RBcore/drv_interface.c @@ -20,7 +20,7 @@ #include "cmsis_os.h" #include "FreeRTOS.h" - +#include "bsp_EEPROM.h" #include "msg_center.h" /*----------------------------------------------* * 外部变量说明 * @@ -249,14 +249,16 @@ extern int check_MK32(char *_pBuffer, uint32_t _iSize); extern void decode_MK32(const char *buf, uint32_t _iSize); extern int check_PV(char *_pBuffer, uint32_t _iSize); extern void decode_PV(const char *buf, uint32_t _iSize); -extern void Send_IV(void *argument); extern void ground_manger_init(void); extern void Custom_Task(void *argument); +void SendIV_Init(void); void Drv_InterfaceInit(void) { + GF_BSP_EEPROM_Init(); MsgCenter_Init(); + SendIV_Init(); // ground_manger_init(); const osThreadAttr_t Custom_attributes = { @@ -298,11 +300,4 @@ void Drv_InterfaceInit(void) .priority = (osPriority_t) osPriorityHigh6, }; (void)osThreadNew(Read_PV, NULL, &Read_PV_attributes); -// -// const osThreadAttr_t Send_PV_attributes = { -// .name = "Send_PV", -// .stack_size = 1024, -// .priority = (osPriority_t) osPriorityHigh4, -// }; -// (void)osThreadNew(Send_IV, NULL, &Send_PV_attributes); } diff --git a/RBcore/include/BHBF.h b/RBcore/include/BHBF.h index 3c173f1..74519bb 100644 --- a/RBcore/include/BHBF.h +++ b/RBcore/include/BHBF.h @@ -72,6 +72,7 @@ extern "C"{ #define MODULE_NAME_RBCORE "RBcore" #define MODULE_NAME_MOTOR "motor" #define MODULE_NAME_CUSTOM "custom" +#define MODULE_NAME_SENDIV "sendiv" typedef enum { LUA_SHOW_INFO, @@ -92,6 +93,7 @@ typedef enum { RBCORE_CMD_END = 0x0200, CUSTOM_GET_PV, // 获取PV + CUSTOM_GET_CV, // 获取CV CUSTOM_SET_IV, // 设置IV CUSTOM_GET_MK32, // 获取MK32 CUSTOM_CMD_END = 0x0300, diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index 5e19342..b626e1d 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -21,6 +21,8 @@ #include "lua_base.h" #include "Protobuf/PSource/bsp_PV.pb.h" #include "Protobuf/PSource/msp_MK32.pb.h" +#include "Protobuf/PSource/bsp_CV.pb.h" +#include "Protobuf/PSource/bsp_IV.pb.h" /*----------------------------------------------* * 外部变量说明 * @@ -42,8 +44,10 @@ * 模块级变量 * *----------------------------------------------*/ static uint32_t g_uiCustomModuleID = 0; -SP_MSP_MK32_Button g_stMK32; -PV_struct_define g_stPV; +static SP_MSP_MK32_Button g_stMK32; +static PV_struct_define g_stPV; +static CV_struct_define g_stCV; +static IV_struct_define g_stIV; /*----------------------------------------------* * 常量定义 * @@ -52,8 +56,6 @@ PV_struct_define g_stPV; /*----------------------------------------------* * 宏定义 * *----------------------------------------------*/ -#define JOY_STICKER_VALUE_ALLOWANCE 600 //摇杆行程死区 -#define JOY_STICKER_ANGLE_ALLOWANCE 30 //摇杆角度死区 static void Custom_ModuleHandler(const Msg_t *pstMsg) { @@ -64,6 +66,14 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) switch (pstMsg->m_uiMsgID) { + case CUSTOM_GET_CV: + { + if (pstMsg->m_uiDataLen >= sizeof(g_stCV)) + { + RD_MEMCPY(&g_stCV, pstMsg->m_aucData, sizeof(g_stCV)); + } + break; + } case CUSTOM_GET_PV: { if (pstMsg->m_uiDataLen >= sizeof(g_stPV)) @@ -79,7 +89,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) RD_MEMCPY(&g_stMK32, pstMsg->m_aucData, sizeof(g_stMK32)); } - if (abs(g_stMK32.CH2_LY_V) <= JOY_STICKER_VALUE_ALLOWANCE && abs(g_stMK32.CH3_LY_H) <= 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); break; @@ -87,19 +97,19 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) int angle = atan2(g_stMK32.CH2_LY_V, g_stMK32.CH3_LY_H) * 180 / M_PI; - if (abs(angle - 90) <= JOY_STICKER_ANGLE_ALLOWANCE) + if (abs(angle - 90) <= g_stCV.Joy_Sticker_Angle_Allowance) { MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0); } - else if (abs(angle - (-90)) <= JOY_STICKER_ANGLE_ALLOWANCE) + else if (abs(angle - (-90)) <= g_stCV.Joy_Sticker_Angle_Allowance) { MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0); } - else if (abs(angle - 0) <= JOY_STICKER_ANGLE_ALLOWANCE) + else if (abs(angle - 0) <= g_stCV.Joy_Sticker_Angle_Allowance) { MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNRIGHT, NULL, 0); } - else if (abs(angle - 180) <= JOY_STICKER_ANGLE_ALLOWANCE) + else if (abs(angle - 180) <= g_stCV.Joy_Sticker_Angle_Allowance) { MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNLEFT, NULL, 0); } @@ -129,8 +139,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) lua_print("CH14_LT \t%d\n", g_stMK32.CH14_LT); lua_print("CH15_RT \t%d\n", g_stMK32.CH15_RT); lua_print("PV Info\n"); - lua_print("RunMode = %d\nRobotSpeed = %d\nLaneChangeDistance = %d\nVertical_Calibration = %d\nIV_IsRestart_Notified = %d\nTimeStamp = %ld\n", - g_stPV.RunMode, g_stPV.RobotSpeed, g_stPV.LaneChangeDistance, (int)g_stPV.Vertical_Calibration, g_stPV.IV_IsRestart_Notified, g_stPV.TimeStamp); + lua_print("{%d, %d, %d, %d, %d, %d}\n", + g_stPV.RunMode, g_stPV.RobotSpeed, g_stPV.LaneChangeDistance, (int)g_stPV.Vertical_Calibration, g_stPV.IV_IsRestart_Notified, (int)g_stPV.TimeStamp); break; } default: @@ -144,6 +154,7 @@ void Custom_Task(void *argument) while(1) { MsgCenter_ProcessWait(g_uiCustomModuleID, 2); + MsgCenter_SendTo(MODULE_NAME_SENDIV, CUSTOM_SET_IV, (void *)&g_stIV, sizeof(g_stIV)); } }