robofish/MOVE_AI
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix protucal

Browse Source
This commit is contained in:
Robofish 2026-03-04 18:16:02 +08:00
parent b64dc0740f
commit 12a9ec9011
3 changed files with 36 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/rm_serial_driver/include/rm_serial_driver/protocol.hpp
View File

@ -2,18 +2,14 @@
#include <cstdint> #include <cstdint>
// 帧头定义 // 数据包 ID 定义
#define FRAME_HEADER_0 'M' // 0x4D #define ID_MCU (0xC4) // MCU 数据包
#define FRAME_HEADER_1 'R' // 0x52 #define ID_REF (0xA8) // 裁判系统数据包
#define ID_AI (0xB5) // AI 控制数据包
// 数据包 ID（已废弃，使用帧头识别）
#define ID_MCU (0xC4)
#define ID_REF (0xA8)
// MCU 数据结构（MCU -> 上位机） // MCU 数据结构（MCU -> 上位机）
typedef struct __attribute__((packed)) typedef struct __attribute__((packed))
{ {
uint8_t head[2]; // 帧头 'M' 'R'
uint8_t mode; // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符 uint8_t mode; // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
float q[4]; // 四元数 wxyz 顺序 float q[4]; // 四元数 wxyz 顺序
float yaw; // 偏航角 float yaw; // 偏航角
@ -26,11 +22,12 @@ typedef struct __attribute__((packed))
typedef struct __attribute__((packed)) typedef struct __attribute__((packed))
{ {
uint8_t id; // 数据包 ID: 0xC4
DataMCU_t data; DataMCU_t data;
uint16_t crc16; uint16_t crc16;
} PackageMCU_t; } PackageMCU_t;
// 裁判系统数据结构（保持不变） // 裁判系统数据结构
typedef struct __attribute__((packed)) typedef struct __attribute__((packed))
{ {
uint16_t remain_hp; // 剩余血量 uint16_t remain_hp; // 剩余血量
@ -40,7 +37,7 @@ typedef struct __attribute__((packed))
typedef struct __attribute__((packed)) typedef struct __attribute__((packed))
{ {
uint8_t id; uint8_t id; // 数据包 ID: 0xA8
DataReferee_t data; DataReferee_t data;
uint16_t crc16; uint16_t crc16;
} PackageReferee_t; } PackageReferee_t;
@ -48,7 +45,6 @@ typedef struct __attribute__((packed))
// AI 控制数据结构（上位机 -> MCU） // AI 控制数据结构（上位机 -> MCU）
typedef struct __attribute__((packed)) typedef struct __attribute__((packed))
{ {
uint8_t head[2]; // 帧头 'M' 'R'
uint8_t mode; // 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火 uint8_t mode; // 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
float yaw; // 目标偏航角 float yaw; // 目标偏航角
float yaw_vel; // 偏航角速度 float yaw_vel; // 偏航角速度
@ -64,6 +60,7 @@ typedef struct __attribute__((packed))
typedef struct __attribute__((packed)) typedef struct __attribute__((packed))
{ {
uint8_t id; // 数据包 ID: 0xB5
DataAI_t data; DataAI_t data;
uint16_t crc16; uint16_t crc16;
} PackageAI_t; } PackageAI_t;

39
src/rm_serial_driver/include/rm_serial_driver/新协议.txt
View File

@ -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;

75
src/rm_serial_driver/src/rm_serial_driver.cpp
View File

@ -134,7 +134,7 @@ namespace rm_serial_driver
RCLCPP_ERROR(get_logger(), "关闭串口异常: %s", e.what()); RCLCPP_ERROR(get_logger(), "关闭串口异常: %s", e.what());
} }
} }
}s }
/** /**
* @brief AI 数据回调函数：接收 AI 控制命令并发送给单片机 * @brief AI 数据回调函数：接收 AI 控制命令并发送给单片机
@ -152,9 +152,8 @@ namespace rm_serial_driver
{ {
PackageAI_t package; PackageAI_t package;
// 设置帧头 // 设置 ID
package.data.head[0] = FRAME_HEADER_0; // 'M' package.id = ID_AI; // 0xB5
package.data.head[1] = FRAME_HEADER_1; // 'R'
// 填充数据 // 填充数据
package.data.mode = msg->mode; package.data.mode = msg->mode;
@ -169,10 +168,10 @@ namespace rm_serial_driver
package.data.wz = msg->wz; package.data.wz = msg->wz;
package.data.reserved = msg->reserved; package.data.reserved = msg->reserved;
// 计算 CRC（从帧头开始） // 计算 CRC（从 ID 之后开始，不包括 CRC）
package.crc16 = crc16::CRC16_Calc( package.crc16 = crc16::CRC16_Calc(
reinterpret_cast<const uint8_t *>(&package.data), reinterpret_cast<const uint8_t *>(&package) + 1,
sizeof(DataAI_t), sizeof(PackageAI_t) - 1 - sizeof(uint16_t),
CRC16_INIT); CRC16_INIT);
size_t bytes_written = serial_port_->write( size_t bytes_written = serial_port_->write(
@ -204,15 +203,15 @@ namespace rm_serial_driver
/** /**
* @brief 获取期望的数据包长度 * @brief 获取期望的数据包长度
* @param id 数据包标识（帧头第一个字节） * @param id 数据包 ID
*/ */
size_t RMSerialDriver::get_expected_length(uint8_t id) size_t RMSerialDriver::get_expected_length(uint8_t id)
{ {
switch (id) switch (id)
{ {
case FRAME_HEADER_0: // 'M' - MCU 数据包 case ID_MCU: // 0xC4 - MCU 数据包
return sizeof(PackageMCU_t); return sizeof(PackageMCU_t);
case ID_REF: // 0xA8 - 裁判系统数据包（保持兼容） case ID_REF: // 0xA8 - 裁判系统数据包
return sizeof(PackageReferee_t); return sizeof(PackageReferee_t);
default: default:
return 0; return 0;
@ -224,24 +223,22 @@ namespace rm_serial_driver
*/ */
void RMSerialDriver::process_packet(uint8_t id, const char *data, size_t packet_size) 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; PackageMCU_t mcu;
std::memcpy(&mcu, data, packet_size); std::memcpy(&mcu, data, packet_size);
// 验证帧头 // 验证 ID
if (mcu.data.head[0] != FRAME_HEADER_0 || mcu.data.head[1] != FRAME_HEADER_1) if (mcu.id != ID_MCU)
{ {
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000,
"MCU 数据帧头错误: 0x%02X%02X (期望: MR)", "MCU 数据包 ID 错误: 0x%02X (期望: 0xC4)", mcu.id);
mcu.data.head[0], mcu.data.head[1]);
return; return;
} }
// CRC 校验（从帧头开始，不包括 CRC 本身） // CRC 校验（从 ID 之后开始，包含数据，不包括 CRC 本身）
// if (crc16::CRC16_Verify(reinterpret_cast<uint8_t *>(&mcu.data), if (crc16::CRC16_Verify(reinterpret_cast<uint8_t *>(&mcu) + 1, packet_size - 1))
// sizeof(DataMCU_t) + sizeof(uint16_t))) {
// {
rm_msgs::msg::DataMCU msg; rm_msgs::msg::DataMCU msg;
// 逐字段赋值 // 逐字段赋值
msg.mode = mcu.data.mode; msg.mode = mcu.data.mode;
@ -263,14 +260,14 @@ namespace rm_serial_driver
RCLCPP_DEBUG(get_logger(), RCLCPP_DEBUG(get_logger(),
"接收 MCU 数据: mode=%d, yaw=%.2f, pitch=%.2f, bullet_count=%d", "接收 MCU 数据: mode=%d, yaw=%.2f, pitch=%.2f, bullet_count=%d",
mcu.data.mode, mcu.data.yaw, mcu.data.pitch, mcu.data.bullet_count); mcu.data.mode, mcu.data.yaw, mcu.data.pitch, mcu.data.bullet_count);
// } }
// else else
// { {
// crc_error_count_++; crc_error_count_++;
// RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000,
// "MCU 数据 CRC 校验失败 (总计: %lu)", "MCU 数据 CRC 校验失败 (总计: %lu)",
// crc_error_count_.load()); crc_error_count_.load());
// } }
} }
else if (id == ID_REF) // 0xA8 - 裁判系统数据包 else if (id == ID_REF) // 0xA8 - 裁判系统数据包
{ {
@ -337,9 +334,9 @@ namespace rm_serial_driver
// 解析数据包 // 解析数据包
while (true) while (true)
{ {
// 查找帧头（'M' 或 0xA8） // 查找帧头（ID_MCU 或 ID_REF）
size_t header_pos = serial_buffer_.find_first_of( size_t header_pos = serial_buffer_.find_first_of(
std::string() + static_cast<char>(FRAME_HEADER_0) + static_cast<char>(ID_REF)); std::string() + static_cast<char>(ID_MCU) + static_cast<char>(ID_REF));
if (header_pos == std::string::npos) if (header_pos == std::string::npos)
{ {
@ -358,30 +355,14 @@ namespace rm_serial_driver
if (serial_buffer_.empty()) if (serial_buffer_.empty())
break; break;
// 获取帧头第一个字节 // 获取 ID
uint8_t id = static_cast<uint8_t>(serial_buffer_[0]); uint8_t id = static_cast<uint8_t>(serial_buffer_[0]);
// 如果是 'M'，需要验证第二个字节是否为 'R'
if (id == FRAME_HEADER_0)
{
if (serial_buffer_.size() < 2)
break; // 数据不够，等待更多数据
if (static_cast<uint8_t>(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); size_t expected_length = get_expected_length(id);
if (expected_length == 0) if (expected_length == 0)
{ {
// 未知 ID，删除该字节 // 未知 ID，删除该字节
RCLCPP_DEBUG(get_logger(), "未知帧头: 0x%02X", id); RCLCPP_DEBUG(get_logger(), "未知 ID: 0x%02X", id);
serial_buffer_.erase(0, 1); serial_buffer_.erase(0, 1);
continue; continue;
} }