From 12a9ec90111ae936b3009f9055143eec19e28e49 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Wed, 4 Mar 2026 18:16:02 +0800 Subject: [PATCH] fix protucal --- .../include/rm_serial_driver/protocol.hpp | 19 ++--- .../include/rm_serial_driver/新协议.txt | 39 ---------- src/rm_serial_driver/src/rm_serial_driver.cpp | 75 +++++++------------ 3 files changed, 36 insertions(+), 97 deletions(-) delete mode 100644 src/rm_serial_driver/include/rm_serial_driver/新协议.txt diff --git a/src/rm_serial_driver/include/rm_serial_driver/protocol.hpp b/src/rm_serial_driver/include/rm_serial_driver/protocol.hpp index 80f0e1a..1a3329e 100644 --- a/src/rm_serial_driver/include/rm_serial_driver/protocol.hpp +++ b/src/rm_serial_driver/include/rm_serial_driver/protocol.hpp @@ -2,18 +2,14 @@ #include -// 帧头定义 -#define FRAME_HEADER_0 'M' // 0x4D -#define FRAME_HEADER_1 'R' // 0x52 - -// 数据包 ID（已废弃，使用帧头识别） -#define ID_MCU (0xC4) -#define ID_REF (0xA8) +// 数据包 ID 定义 +#define ID_MCU (0xC4) // MCU 数据包 +#define ID_REF (0xA8) // 裁判系统数据包 +#define ID_AI (0xB5) // AI 控制数据包 // MCU 数据结构（MCU -> 上位机） typedef struct __attribute__((packed)) { - uint8_t head[2]; // 帧头 'M' 'R' uint8_t mode; // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符 float q[4]; // 四元数 wxyz 顺序 float yaw; // 偏航角 @@ -26,11 +22,12 @@ typedef struct __attribute__((packed)) typedef struct __attribute__((packed)) { + uint8_t id; // 数据包 ID: 0xC4 DataMCU_t data; uint16_t crc16; } PackageMCU_t; -// 裁判系统数据结构（保持不变） +// 裁判系统数据结构 typedef struct __attribute__((packed)) { uint16_t remain_hp; // 剩余血量 @@ -40,7 +37,7 @@ typedef struct __attribute__((packed)) typedef struct __attribute__((packed)) { - uint8_t id; + uint8_t id; // 数据包 ID: 0xA8 DataReferee_t data; uint16_t crc16; } PackageReferee_t; @@ -48,7 +45,6 @@ typedef struct __attribute__((packed)) // AI 控制数据结构（上位机 -> MCU） typedef struct __attribute__((packed)) { - uint8_t head[2]; // 帧头 'M' 'R' uint8_t mode; // 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火 float yaw; // 目标偏航角 float yaw_vel; // 偏航角速度 @@ -64,6 +60,7 @@ typedef struct __attribute__((packed)) typedef struct __attribute__((packed)) { + uint8_t id; // 数据包 ID: 0xB5 DataAI_t data; uint16_t crc16; } PackageAI_t; diff --git a/src/rm_serial_driver/include/rm_serial_driver/新协议.txt b/src/rm_serial_driver/include/rm_serial_driver/新协议.txt deleted file mode 100644 index c0c9ac0..0000000 --- a/src/rm_serial_driver/include/rm_serial_driver/新协议.txt +++ /dev/null @@ -1,39 +0,0 @@ -DataMCU_t - - uint8_t head[2] = {'M', 'R'}; - uint8_t mode; // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符 - float q[4]; // wxyz顺序 - float yaw; - float yaw_vel; - float pitch; - float pitch_vel; - float bullet_speed; - uint16_t bullet_count; // 子弹累计发送次数 - - -DataReferee_t - - uint16_t remain_hp; /* 剩余血量 */ - uint8_t game_progress : 4; /* 比赛进度 */ - uint16_t stage_remain_time; /* 比赛剩余时间 */ - - - -DataAI_t - - uint8_t head[2] = {'M', 'R'}; - uint8_t mode; // 0: 不控制, 1: 控制云台但不开火，2: 控制云台且开火 - float yaw; - float yaw_vel; - float yaw_acc; - float pitch; - float pitch_vel; - float pitch_acc; - - float vx; - float vy; - float wz; - - uint8_t 预留 - uint16_t crc16; - diff --git a/src/rm_serial_driver/src/rm_serial_driver.cpp b/src/rm_serial_driver/src/rm_serial_driver.cpp index 340597a..5e329dc 100644 --- a/src/rm_serial_driver/src/rm_serial_driver.cpp +++ b/src/rm_serial_driver/src/rm_serial_driver.cpp @@ -134,7 +134,7 @@ namespace rm_serial_driver RCLCPP_ERROR(get_logger(), "关闭串口异常: %s", e.what()); } } - }s + } /** * @brief AI 数据回调函数：接收 AI 控制命令并发送给单片机 @@ -152,9 +152,8 @@ namespace rm_serial_driver { PackageAI_t package; - // 设置帧头 - package.data.head[0] = FRAME_HEADER_0; // 'M' - package.data.head[1] = FRAME_HEADER_1; // 'R' + // 设置 ID + package.id = ID_AI; // 0xB5 // 填充数据 package.data.mode = msg->mode; @@ -169,10 +168,10 @@ namespace rm_serial_driver package.data.wz = msg->wz; package.data.reserved = msg->reserved; - // 计算 CRC（从帧头开始） + // 计算 CRC（从 ID 之后开始，不包括 CRC） package.crc16 = crc16::CRC16_Calc( - reinterpret_cast(&package.data), - sizeof(DataAI_t), + reinterpret_cast(&package) + 1, + sizeof(PackageAI_t) - 1 - sizeof(uint16_t), CRC16_INIT); size_t bytes_written = serial_port_->write( @@ -204,15 +203,15 @@ namespace rm_serial_driver /** * @brief 获取期望的数据包长度 - * @param id 数据包标识（帧头第一个字节） + * @param id 数据包 ID */ size_t RMSerialDriver::get_expected_length(uint8_t id) { switch (id) { - case FRAME_HEADER_0: // 'M' - MCU 数据包 + case ID_MCU: // 0xC4 - MCU 数据包 return sizeof(PackageMCU_t); - case ID_REF: // 0xA8 - 裁判系统数据包（保持兼容） + case ID_REF: // 0xA8 - 裁判系统数据包 return sizeof(PackageReferee_t); default: return 0; @@ -224,24 +223,22 @@ namespace rm_serial_driver */ void RMSerialDriver::process_packet(uint8_t id, const char *data, size_t packet_size) { - if (id == FRAME_HEADER_0) // 'M' - MCU 数据包 + if (id == ID_MCU) // 0xC4 - MCU 数据包 { PackageMCU_t mcu; std::memcpy(&mcu, data, packet_size); - // 验证帧头 - if (mcu.data.head[0] != FRAME_HEADER_0 || mcu.data.head[1] != FRAME_HEADER_1) + // 验证 ID + if (mcu.id != ID_MCU) { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, - "MCU 数据帧头错误: 0x%02X%02X (期望: MR)", - mcu.data.head[0], mcu.data.head[1]); + "MCU 数据包 ID 错误: 0x%02X (期望: 0xC4)", mcu.id); return; } - // CRC 校验（从帧头开始，不包括 CRC 本身） - // if (crc16::CRC16_Verify(reinterpret_cast(&mcu.data), - // sizeof(DataMCU_t) + sizeof(uint16_t))) - // { + // CRC 校验（从 ID 之后开始，包含数据，不包括 CRC 本身） + if (crc16::CRC16_Verify(reinterpret_cast(&mcu) + 1, packet_size - 1)) + { rm_msgs::msg::DataMCU msg; // 逐字段赋值 msg.mode = mcu.data.mode; @@ -263,14 +260,14 @@ namespace rm_serial_driver RCLCPP_DEBUG(get_logger(), "接收 MCU 数据: mode=%d, yaw=%.2f, pitch=%.2f, bullet_count=%d", mcu.data.mode, mcu.data.yaw, mcu.data.pitch, mcu.data.bullet_count); - // } - // else - // { - // crc_error_count_++; - // RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, - // "MCU 数据 CRC 校验失败 (总计: %lu)", - // crc_error_count_.load()); - // } + } + else + { + crc_error_count_++; + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, + "MCU 数据 CRC 校验失败 (总计: %lu)", + crc_error_count_.load()); + } } else if (id == ID_REF) // 0xA8 - 裁判系统数据包 { @@ -337,9 +334,9 @@ namespace rm_serial_driver // 解析数据包 while (true) { - // 查找帧头（'M' 或 0xA8） + // 查找帧头（ID_MCU 或 ID_REF） size_t header_pos = serial_buffer_.find_first_of( - std::string() + static_cast(FRAME_HEADER_0) + static_cast(ID_REF)); + std::string() + static_cast(ID_MCU) + static_cast(ID_REF)); if (header_pos == std::string::npos) { @@ -358,30 +355,14 @@ namespace rm_serial_driver if (serial_buffer_.empty()) break; - // 获取帧头第一个字节 + // 获取 ID uint8_t id = static_cast(serial_buffer_[0]); - - // 如果是 'M'，需要验证第二个字节是否为 'R' - if (id == FRAME_HEADER_0) - { - if (serial_buffer_.size() < 2) - break; // 数据不够，等待更多数据 - - if (static_cast(serial_buffer_[1]) != FRAME_HEADER_1) - { - // 第二个字节不是 'R'，删除第一个字节继续查找 - RCLCPP_DEBUG(get_logger(), "帧头不完整，丢弃 'M'"); - serial_buffer_.erase(0, 1); - continue; - } - } - size_t expected_length = get_expected_length(id); if (expected_length == 0) { // 未知 ID，删除该字节 - RCLCPP_DEBUG(get_logger(), "未知帧头: 0x%02X", id); + RCLCPP_DEBUG(get_logger(), "未知 ID: 0x%02X", id); serial_buffer_.erase(0, 1); continue; }