robofish/MOVE_AI

Fork 0

Robofish ea8fa41bd3 driver ok

2026-03-04 17:50:55 +08:00

9.7 KiB

Raw Blame History

RM Serial Driver - 协议修改指南

本文档详细说明如何修改通信协议以适应你的需求。

快速修改协议

1. 修改数据包 ID

在 include/rm_serial_driver/protocol.hpp 中修改：

// 修改数据包标识符
#define ID_MCU (0xC4)  // 单片机数据包 ID
#define ID_REF (0xA8)  // 裁判系统数据包 ID

// 如果需要添加新的数据包类型，添加新的 ID
#define ID_VISION (0xB5)  // 视觉数据包 ID（示例）

2. 修改 MCU 数据结构

修改从单片机接收的数据格式：

typedef struct __attribute__((packed))
{
    struct __attribute__((packed))
    {
        float q0;
        float q1;
        float q2;
        float q3;
    } quat; /* 四元数 */

    struct __attribute__((packed))
    {
        float yaw;
        float pit;
        float rol;
    } gimbal; /* 欧拉角 */

    uint8_t notice; /* 控制命令 */

    // 添加新字段示例：
    // float t
} DataMCU_t;emperature;  // 温度
    // uint16_t voltage;   // 电压

注意：修改后需要同步修改 rm_msgs 包中的消息定义！

3. 修改 AI 控制数据结构

修改发送给单片机的控制命令：

typedef struct __attribute__((packed))
{
    struct __attribute__((packed))
    {
        float yaw;
        float pit;
        float rol;
    } delta_eulr;  /* 欧拉角增量 */

    struct __attribute__((packed))
    {
        float vx;
        float vy;
        float wz;
    } delta_pos;  /* 位置增量 */

    uint8_t notice; /* 控制命令 */

    // 添加新字段示例：
    // uint8_t shoot_mode;  // 射击模式
    // uint16_t shoot_speed; // 射击速度
} DataAI_t;

4. 添加新的数据包类型

步骤 1：在 `protocol.hpp` 中定义新数据结构

// 定义新的数据包 ID
#define ID_VISION (0xB5)

// 定义数据结构
typedef struct __attribute__((packed))
{
    float target_x;
    float target_y;
    float target_z;
    uint8_t target_type;
} DataVision_t;

// 定义完整数据包（带 ID 和 CRC）
typedef struct __attribute__((packed))
{
    uint8_t id;
    DataVision_t data;
    uint16_t crc16;
} PackageVision_t;

步骤 2：在 `rm_serial_driver.cpp` 中添加处理逻辑

在 get_expected_length() 函数中添加：

size_t RMSerialDriver::get_expected_length(uint8_t id)
{
  switch (id)
  {
    case ID_MCU:
      return sizeof(PackageMCU_t);
    case ID_REF:
      return sizeof(PackageReferee_t);
    case ID_VISION:  // 新增
      return sizeof(PackageVision_t);
    default:
      return 0;
  }
}

在 process_packet() 函数中添加：

void RMSerialDriver::process_packet(uint8_t id, const char *data, size_t packet_size)
{
  // ... 现有代码 ...

  else if (id == ID_VISION)  // 新增
  {
    PackageVision_t vision;
    std::memcpy(&vision, data, packet_size);

    if (crc16::CRC16_Verify(reinterpret_cast<uint8_t *>(&vision) + 1, packet_size - 1))
    {
      rm_msgs::msg::DataVision msg;  // 需要在 rm_msgs 中定义
      std::memcpy(&msg, &vision.data, sizeof(vision.data));
      data_vision_pub_->publish(msg);

      rx_count_++;
      last_receive_time_ = std::chrono::steady_clock::now();
    }
    else
    {
      crc_error_count_++;
      RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000,
                           "Vision 数据 CRC 校验失败");
    }
  }
}

步骤 3：在头文件中添加发布者

在 rm_serial_driver.hpp 中添加：

private:
  rclcpp::Publisher<rm_msgs::msg::DataVision>::SharedPtr data_vision_pub_;

步骤 4：在构造函数中创建发布者

data_vision_pub_ = this->create_publisher<rm_msgs::msg::DataVision>(
    topic_prefix + "/data_vision", 10);

完整示例：添加电机状态数据包

1. 定义协议（protocol.hpp）

#define ID_MOTOR (0xD3)

typedef struct __attribute__((packed))
{
    int16_t motor1_spee16_t motor2_speed;
    int16_t motor3_speed;
    int16_t motor4_speed;
    uint16_t motor1_current;
    uint16_t motor2_current;
    uint16_t motor3_current;
    uint16_t motor4_current;
    uint8_t motor_status;  // 位标志：bit0=motor1, bit1=motor2...
} DataMotor_t;

typedef struct __attribute__((packed))
{
    uint8_t id;
    DataMotor_t data;
    uint16_t crc16;
} PackageMotor_t;

2. 创建 ROS 消息（rm_msgs/msg/DataMotor.msg）

int16 motor1_speed
int16 motor2_speed
int16 motor3_speed
int16 motor4_speed
uint16 motor1_current
uint16 motor2_current
uint16 motor3_current
uint16 motor4_current
uint8 motor_status

3. 修改驱动代码

在 rm_serial_driver.hpp 中：

#include "rm_msgs/msg/data_motor.hpp"

private:
  rclcpp::Publisher<rm_msgs::msg::DataMotor>::SharedPtr data_motor_pub_;

在 rm_serial_driver.cpp 构造函数中：

data_motor_pub_ = this->create_publisher<rm_msgs::msg::DataMotor>(
    topic_prefix + "/data_motor", 10);

在 get_expected_length() 中：

case ID_MOTOR:
  return sizeof(PackageMotor_t);

在 process_packet() 中：

else if (id == ID_MOTOR)
{
  PackageMotor_t motor;
  std::memcpy(&motor, data, packet_size);

  if (crc16::CRC16_Verify(reinterpret_cast<uint8_t *>(&motor) + 1, packet_size - 1))
  {
    rm_msgs::msg::DataMotor msg;
    msg.motor1_speed = motor.data.motor1_speed;
    msg.motor2_speed = motor.data.motor2_speed;
    msg.motor3_speed = motor.data.motor3_speed;
    msg.motor4_speed = motor.data.motor4_speed;
    msg.motor1_current = motor.data.motor1_current;
    msg.motor2_current = motor.data.motor2_current;
    msg.motor3_current = motor.data.motor3_current;
    msg.motor4_current = motor.data.motor4_current;
    msg.motor_status = motor.data.motor_status;

    data_motor_pub_->publish(msg);
    rx_count_++;
    last_receive_time_ = std::chrono::steady_clock::now();

    RCLCPP_DEBUG(get_logger(), "接收电机数据: M1=%d, M2=%d, M3=%d, M4=%d",
                 motor.data.motor1_speed, motor.data.motor2_speed,
                 motor.data.motor3_speed, motor.data.motor4_speed);
  }
  else
  {
    crc_error_count_++;
    RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000,
                         "电机数据 CRC 校验失败");
  }
}

4. 单片机端代码（Arduino/STM32）

// 发送电机数据
void send_motor_data()
{
    PackageMotor_t package;
    package.id = ID_MOTOR;

    // 填充数据
    package.data.motor1_speed = get_motor_speed(1);
    package.data.motor2_speed = get_motor_speed(2);
    package.data.motor3_speed = get_motor_speed(3);
    package.data.motor4_speed = get_motor_speed(4);

    package.data.motor1_current = get_motor_current(1);
    package.data.motor2_current = get_motor_current(2);
    package.data.motor3_current = get_motor_current(3);
    package.data.motor4_current = get_motor_current(4);

    package.data.motor_status = get_motor_status();

    // 计算 CRC
    package.crc16 = CRC16_Calc(
        (uint8_t*)&package.data,
        sizeof(DataMotor_t),
        CRC16_INIT
    );

    // 发送
    Serial.write((uint8_t*)&package, sizeof(PackageMotor_t));
}

注意事项

1. 数据对齐

使用 __attribute__((packed)) 确保结构体紧凑排列，避免编译器自动填充：

typedef struct __attribute__((packed))
{
    uint8_t a;   // 1 字节
    uint16_t b;  // 2 字节，紧跟在 a 后面
    uint32_t c;  // 4 字节，紧跟在 b 后面
} MyData_t;

2. 字节序

当前实现假设上位机和下位机使用相同的字节序（通常都是小端序）。如果需要跨平台，考虑显式转换：

// 发送前转换为小端序
uint16_t value = 1234;
uint8_t bytes[2];
bytes[0] = value & 0xFF;
bytes[1] = (value >> 8) & 0xFF;

// 接收后从小端序转换
uint16_t received = bytes[0] | (bytes[1] << 8);

3. CRC 校验范围

当前 CRC 校验范围是从数据部分开始（不包括 ID），到数据结束（不包括 CRC 本身）：

+----+----------+-------+
| ID |   Data   |  CRC  |
+----+----------+-------+
      ^--------^
      CRC 校验范围

4. 编译步骤

修改协议后需要重新编译：

cd ~/MOVE_AI

# 先编译消息包
colcon build --packages-select rm_msgs

# 再编译驱动
colcon build --packages-select rm_serial_driver

# 加载环境
source install/setup.bash

调试技巧

1. 启用调试日志

ros2 run rm_serial_driver rm_serial_driver --ros-args --log-level debug

2. 查看原始数据

在 receive_data() 中添加：

// 打印接收到的原始数据
RCLCPP_INFO(get_logger(), "收到 %zu 字节: %s",
            buffer.size(),
            bytes_to_hex(buffer).c_str());

辅助函数：

std::string bytes_to_hex(const std::string& data)
{
    std::stringstream ss;
    ss << std::hex << std::setfill('0');
    for (unsigned char c : data) {
        ss << std::setw(2) << static_cast<int>(c) << " ";
    }
    return ss.str();
}

3. 验证数据包大小

RCLCPP_INFO(get_logger(), "数据包大小:");
RCLCPP_INFO(get_logger(), "  PackageMCU_t: %zu", sizeof(PackageMCU_t));
RCLCPP_INFO(get_logger(), "  PackageReferee_t: %zu", sizeof(PackageReferee_t));
RCLCPP_INFO(get_logger(), "  PackageAI_t: %zu", sizeof(PackageAI_t));

常见问题

Q: 修改协议后无法接收数据？

A: 检查以下几点：

数据包大小是否匹配（上位机和下位机）
CRC 计算是否一致
数据包 ID 是否正确
结构体是否使用了 __attribute__((packed))

Q: CRC 校验总是失败？

A: 确认：

CRC 计算范围是否一致
CRC 初始值是否相同（0xFFFF）
字节序是否一致

Q: 如何临时禁用 CRC 校验？

A: 在 process_packet() 中注释掉 CRC 检查：

// if (crc16::CRC16_Verify(...))
if (true)  // 临时禁用 CRC
{
    // 处理数据
}

注意：仅用于调试，生产环境必须启用 CRC！

9.7 KiB Raw Blame History Unescape Escape