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>
// 帧头定义
#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;

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());
}
}
}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<const uint8_t *>(&package.data),
sizeof(DataAI_t),
reinterpret_cast<const uint8_t *>(&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<uint8_t *>(&mcu.data),
// sizeof(DataMCU_t) + sizeof(uint16_t)))
// {
// CRC 校验（从 ID 之后开始，包含数据，不包括 CRC 本身）
if (crc16::CRC16_Verify(reinterpret_cast<uint8_t *>(&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<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)
{
@ -358,30 +355,14 @@ namespace rm_serial_driver
if (serial_buffer_.empty())
break;
// 获取帧头第一个字节
// 获取 ID
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);
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;
}