diff --git a/RBcore/TL720D.c b/RBcore/TL720D.c new file mode 100644 index 0000000..247c5da --- /dev/null +++ b/RBcore/TL720D.c @@ -0,0 +1,146 @@ +/****************************************************************************** + + 版权所有 (C), 2018-2099, Radkil + + ****************************************************************************** + 文 件 名 : TL720D.c + 版 本 号 : 初稿 + 作 者 : radkil + 生成日期 : 2026年9月8日 + 最近修改 : + 功能描述 : 机器人本体陀螺仪解析 + + 修改历史 : + 1.日 期 : 2026年9月8日 + 作 者 : radkil + 修改内容 : 创建文件 + +******************************************************************************/ +#include "msp_TL720D.pb.h" +#include "BHBF.h" +#include "msg_center.h" + +/*----------------------------------------------* + * 外部变量说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 外部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 内部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 全局变量 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 模块级变量 * + *----------------------------------------------*/ +static MSP_TL720DParameters g_stTL720D = {0}; + +/*----------------------------------------------* + * 常量定义 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 宏定义 * + *----------------------------------------------*/ + +/* 数值计算 + * data: 数据地址 + * return: 单位: 0.01 + * */ +static int16_t getDeci(uint8_t *data) +{ + char isNegative = 0; + + if (*data >> 4) + { + isNegative = 1; + } else + { + isNegative = 0; + } + + int16_t data_value = 0; + data_value = ((*data) & 0x0f) * 10000; + data++; + data_value += (*data >> 4) * 1000; + data_value += ((*data) & 0x0f) * 100; + data++; + + int16_t xiaoshu = 0; + xiaoshu = (*data >> 4) * 10; + xiaoshu += (*data) & 0x0f; + if (isNegative) + { + return -(data_value + xiaoshu); + } else + { + return (data_value + xiaoshu); + } +} + +static int check_TL720D(char *_pBuffer, uint32_t _iSize) +{ + uint8_t check_sum = 0; + for(uint8_t i = 1; i < 31; i++) + { + check_sum += (uint8_t)_pBuffer[i]; + } + + if(check_sum != _pBuffer[31]) + { + return 0; + } + + if (_pBuffer[0] != 0x68) return -1; + if (_pBuffer[1] != 0x1f) return -1; + if (_pBuffer[2] != 0x00) return -1; + if (_pBuffer[3] != 0x84) return -1; + + return 32; +} + +static void decode_TL720D(const char *buf, uint32_t _iSize) +{ + g_stTL720D.RF_Angle_Roll = getDeci((uint8_t *)&buf[4]); + g_stTL720D.RF_Angle_Pitch = getDeci((uint8_t *)&buf[7]); + g_stTL720D.RF_Angle_Yaw = getDeci((uint8_t *)&buf[10]); + g_stTL720D.RF_Acc_X = getDeci((uint8_t *)&buf[13]); + g_stTL720D.RF_Acc_Y = getDeci((uint8_t *)&buf[16]); + g_stTL720D.RF_Acc_Z = getDeci((uint8_t *)&buf[19]); + g_stTL720D.RF_Gro_X = getDeci((uint8_t *)&buf[22]); + g_stTL720D.RF_Gro_Y = getDeci((uint8_t *)&buf[25]); + g_stTL720D.RF_Gro_Z = getDeci((uint8_t *)&buf[28]); + + MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_GET_TL720D_ROLL, (void *)&g_stTL720D.RF_Angle_Roll, sizeof(int32_t)); +} + +void Read_TL720D(void *argument) +{ + char pcBuffer[64] = {0}; + + while(1) + { + rd_ComRead(g_ptrs485_1, pcBuffer, 64); + Rd_Delay(1); + } +} + +void TL720D_Init(void) +{ + TUartUserData *pt4851UserData = UART_userdata_init(1, -1, 512); + g_ptrs485_1 = rd_ComCreate(check_TL720D, decode_TL720D, RS485_1_Send, pt4851UserData->m_buf_size, pt4851UserData); + UART_IT_init(g_ptrs485_1); + + const osThreadAttr_t Read_TL720D_attributes = { + .name = "Read_TL720D", + .stack_size = 2048, + .priority = (osPriority_t) osPriorityRealtime1, + }; + (void)osThreadNew(Read_TL720D, NULL, &Read_TL720D_attributes); +} diff --git a/RBcore/drv_interface.c b/RBcore/drv_interface.c index 1c64b89..e2b414d 100644 --- a/RBcore/drv_interface.c +++ b/RBcore/drv_interface.c @@ -254,6 +254,7 @@ extern void decode_PV(const char *buf, uint32_t _iSize); extern void ground_manger_init(void); extern void Custom_Task(void *argument); void SendIV_Init(void); +void TL720D_Init(void); void Drv_InterfaceInit(void) { @@ -303,4 +304,6 @@ void Drv_InterfaceInit(void) .priority = (osPriority_t) osPriorityRealtime1, }; (void)osThreadNew(Read_PV, NULL, &Read_PV_attributes); + + TL720D_Init(); } diff --git a/RBcore/include/BHBF.h b/RBcore/include/BHBF.h index c97ad1a..13a8814 100644 --- a/RBcore/include/BHBF.h +++ b/RBcore/include/BHBF.h @@ -95,6 +95,7 @@ typedef enum { CUSTOM_GET_PV, // 获取PV CUSTOM_SET_IV, // 设置IV CUSTOM_GET_MK32, // 获取MK32 + CUSTOM_GET_TL720D_ROLL, // 获取陀螺仪横滚角 CUSTOM_CMD_PAINTGUN, // 控制喷枪 CUSTOM_CMD_END = 0x0300, } Motor_Cmd_e; diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index 7bd4188..f229140 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -67,6 +67,14 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) switch (pstMsg->m_uiMsgID) { + case CUSTOM_GET_TL720D_ROLL: + { + if (pstMsg->m_uiDataLen >= sizeof(int32_t)) + { + RD_MEMCPY(&g_stIV.CurrentAngle, pstMsg->m_aucData, sizeof(int32_t)); + } + break; + } case CUSTOM_GET_PV: { if (pstMsg->m_uiDataLen >= sizeof(g_stPV))