From 01d236d1a0a2252e033553db9893b2a5235a4778 Mon Sep 17 00:00:00 2001 From: HelixCopex Date: Tue, 24 Mar 2026 01:24:15 +0800 Subject: [PATCH] =?UTF-8?q?=E6=94=B6=E4=BF=A1=E4=BA=8C=E8=8A=82=E7=82=B9?= =?UTF-8?q?=E5=AE=8C=E5=85=A8=E5=AE=9E=E7=8E=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 2 + README.md | 15 +- docs/IMU/packet/MSG_AHRS.md | 27 ++++ docs/IMU/packet/MSG_BODY_ACCELERATION.md | 26 ++++ package.xml | 1 + src/imu_receiver_node.cpp | 169 ++++++++++++++++------- 6 files changed, 181 insertions(+), 59 deletions(-) create mode 100644 docs/IMU/packet/MSG_AHRS.md create mode 100644 docs/IMU/packet/MSG_BODY_ACCELERATION.md diff --git a/CMakeLists.txt b/CMakeLists.txt index 396c1b5..92a135f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) # 添加 SDK 库路径 set(TRANSMITTER_SDK_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib/transmitter_sdk) @@ -39,6 +40,7 @@ ament_target_dependencies(imu_receiver_node rclcpp std_msgs geometry_msgs + sensor_msgs ) # ==================== 安装目标 ==================== diff --git a/README.md b/README.md index d1ab3a6..b75c444 100644 --- a/README.md +++ b/README.md @@ -10,8 +10,8 @@ - MV-SUA133GC-T 工业相机 (USB) - NUC10i7 微型计算机 (主机) -- FDI Systems DETA10 IMU 模块 (USB - CP210x 串口芯片 - UART) `/dev/ttyUSB1` -- CH340 USB 转 UART 模块 (USB - 模块 - UART) `/dev/ttyUSB0` +- FDI Systems DETA10 IMU 模块 (USB - CP210x 串口芯片 - UART) `/dev/ttyCH340` +- CH340 USB 转 UART 模块 (USB - 模块 - UART) `/dev/ttyIMU` ### 软件 @@ -39,18 +39,19 @@ ROS2 基本架构为节点。以每个节点的发布、解析、接收数据为 ### UART 收发节点 - 功能：主机对外通信的封装节点。物理上读取收发模块 -- 连接：CH340，协商 UART 通信速率 115200，`/dev/ttyUSB0` - IMU_CP210x，协商 UART 通信速率 921600，`/dev/ttyUSB1` +- 连接：CH340，协商 UART 通信速率 115200，`/dev/ttyCH340` - 接收：CH340.RX -> 裁判系统数据 - 订阅：中央节点 -> 控制指令 - 发送：CH340.TX -> 控制指令 -- 发布：IMU 数据（地磁角、六轴角加速度、加速度、陀螺仪） - 裁判系统数据（血量、比赛当前状态） +- 发布：裁判系统数据（血量、比赛当前状态） CH340 、IMU 连接 OK 标志 ### IMU 收发节点 -- 功能：IMU 与 +- 功能：IMU 与主机通信的封装节点。物理上读取 IMU（DETA10）数据 +- 连接：IMU_CP210x，协商 UART 通信速率 921600，`/dev/ttyUSB1` +- 接收：IMU_CP210x -> 六轴加速度、陀螺仪数据 +- 发布：IMU 数据（地磁角、六轴角加速度、加速度、陀螺仪） ### 中央节点 diff --git a/docs/IMU/packet/MSG_AHRS.md b/docs/IMU/packet/MSG_AHRS.md new file mode 100644 index 0000000..8b0e3cc --- /dev/null +++ b/docs/IMU/packet/MSG_AHRS.md @@ -0,0 +1,27 @@ +# MSG_AHRS | FDISYSTEMS支持中心 + +该数据包用于描述：卡尔曼滤波输出航姿参考系统数据 + +## 基本信息 + +| 属性 | 值 | +|------|-----| +| **Packet ID** | 0x41 | +| **Length** | 48 | +| **Read / Write** | Read | + +## 数据字段 + +| Offset | Size | Format | Field | Unit | Description | +|--------|------|--------|-------|------|-------------| +| 0 | 4 | float32_t | RollSpeed | rad/s | 滤波修正后的横滚角速度，等于MSG_IMU数据包里的Gyroscope_X减去卡尔曼滤波估计的X轴角速度零偏 | +| 4 | 4 | float32_t | PitchSpeed | rad/s | 滤波修正后的俯仰角速度，等于MSG_IMU数据包里的Gyroscope_Y减去卡尔曼滤波估计的Y轴角速度零偏 | +| 8 | 4 | float32_t | HeadingSpeed | rad/s | 滤波修正后的偏航角速度，等于MSG_IMU数据包里的Gyroscope_Z减去卡尔曼滤波估计的Z轴角速度零偏 | +| 12 | 4 | float32_t | Roll | rad | 横滚 | +| 16 | 4 | float32_t | Pitch | rad | 俯仰 | +| 20 | 4 | float32_t | Heading | rad | 偏航 | +| 24 | 4 | float32_t | Q1 | / | 四元数Q1 | +| 28 | 4 | float32_t | Q2 | / | 四元数Q2 | +| 32 | 4 | float32_t | Q3 | / | 四元数Q3 | +| 36 | 4 | float32_t | Q4 | / | 四元数Q4 | +| 40 | 8 | int64_t | Timestamp | us | 数据的时间戳，从上电开始启动的微秒数。时钟源为MCU外部晶振。 | \ No newline at end of file diff --git a/docs/IMU/packet/MSG_BODY_ACCELERATION.md b/docs/IMU/packet/MSG_BODY_ACCELERATION.md new file mode 100644 index 0000000..65e2d83 --- /dev/null +++ b/docs/IMU/packet/MSG_BODY_ACCELERATION.md @@ -0,0 +1,26 @@ +# MSG_BODY_ACCELERATION | FDISYSTEMS支持中心 + +![FDI SYSTEMS 支持中心](https://doc.fdisystems.cn/logo.png) + +[Back to FDISYSTEMS](https://fdisystems.cn) + +Developers / FDILink协议 / Data Packets / MSG_BODY_ACCELERATION + +该数据包用于描述：滤波修正后的机体系加速度，不包括重力加速度。 + +## 基本信息 + +| 属性 | 值 | +|------|-----| +| **Packet ID** | 0x62 | +| **Length** | 16 | +| **Read / Write** | Read | + +## 数据字段 + +| Offset | Size | Format | Field | Unit | Description | +|--------|------|--------|-------|------|-------------| +| 0 | 4 | float32_t | Body_acceleration_X | m/s/s | 滤波修正后的机体系X轴方向加速度 | +| 4 | 4 | float32_t | Body_acceleration_Y | m/s/s | 滤波修正后的机体系Y轴方向加速度 | +| 8 | 4 | float32_t | Body_acceleration_Z | m/s/s | 滤波修正后的机体系Z轴方向加速度 | +| 12 | 4 | float32_t | G_force | m/s/s | 当地重力加速度 | diff --git a/package.xml b/package.xml index cc512a8..9430c9b 100644 --- a/package.xml +++ b/package.xml @@ -12,6 +12,7 @@ rclcpp std_msgs geometry_msgs + sensor_msgs ament_cmake diff --git a/src/imu_receiver_node.cpp b/src/imu_receiver_node.cpp index 9c7d0ad..ad240bf 100644 --- a/src/imu_receiver_node.cpp +++ b/src/imu_receiver_node.cpp @@ -1,26 +1,28 @@ /** * @file imu_receiver_node.cpp - * @brief IMU NED 速度接收节点 (FDI DETA10) - 高实时性版本 + * @brief IMU 数据接收节点 (FDI DETA10) - 高实时性版本 * - * 通过串口读取 FDI DETA10 IMU 模块的北东地速度数据 - * 用于二维惯性导航 + * 通过串口读取 FDI DETA10 IMU 模块的数据 * 波特率：921600 * 协议：FDILink * - * 数据包：MSG_NED_VEL (0x5F) - * - Velocity_north: 北向速度 (m/s) - * - Velocity_east: 东向速度 (m/s) - * - Velocity_down: 地向速度 (m/s) + * 转发数据包： + * - MSG_AHRS (0x41): 航姿参考系统数据 + * - MSG_BODY_ACCELERATION (0x62): 机体系加速度数据 */ #include #include #include #include +#include #include +#include #include +#include #include #include +#include #include #include #include @@ -30,8 +32,9 @@ constexpr uint8_t FRAME_START = 0xFC; constexpr uint8_t FRAME_END = 0xFD; -// 数据包 ID - 只保留 NED 速度 -constexpr uint8_t MSG_NED_VEL = 0x5F; // 卡尔曼滤波融合的北东地速度 +// 数据包 ID +constexpr uint8_t MSG_AHRS = 0x41; // 航姿参考系统数据 +constexpr uint8_t MSG_BODY_ACCELERATION = 0x62; // 机体系加速度数据 // 默认串口设备 constexpr const char *DEFAULT_SERIAL_PORT = "/dev/ttyUSB1"; @@ -109,7 +112,7 @@ public: enable_crc_ = this->get_parameter("enable_crc").as_bool(); RCLCPP_INFO(this->get_logger(), "================================="); - RCLCPP_INFO(this->get_logger(), "IMU NED 速度接收节点启动 (高实时性版本)"); + RCLCPP_INFO(this->get_logger(), "IMU 数据接收节点启动 (高实时性版本)"); RCLCPP_INFO(this->get_logger(), "串口: %s", serial_port_.c_str()); RCLCPP_INFO(this->get_logger(), "波特率: %d", baudrate_); RCLCPP_INFO(this->get_logger(), "CRC校验: %s", @@ -117,12 +120,11 @@ public: RCLCPP_INFO(this->get_logger(), "================================="); // 创建发布者 - ned_vel_pub_ = this->create_publisher( - "imu/ned_velocity", 10); - vel_north_pub_ = this->create_publisher( - "imu/velocity_north", 10); - vel_east_pub_ = - this->create_publisher("imu/velocity_east", 10); + ahrs_pub_ = this->create_publisher("imu/ahrs", 10); + body_acc_pub_ = this->create_publisher( + "imu/body_acceleration", 10); + g_force_pub_ = + this->create_publisher("imu/g_force", 10); connection_status_pub_ = this->create_publisher( "imu/connection_status", 10); @@ -249,7 +251,7 @@ private: return crc16; } - // 接收循环 + // 高实时性接收循环 - 批量读取，无阻塞 void receiveLoop() { // 使用双缓冲区策略：一个用于接收，一个用于处理 std::vector rx_buffer; @@ -264,7 +266,7 @@ private: current_frame.reserve(MAX_FRAME_SIZE); while (running_ && rclcpp::ok()) { - // 批量读串口数据 + // 批量读取串口数据 - 非阻塞 ssize_t n = read(serial_fd_, read_buf, sizeof(read_buf)); if (n > 0) { @@ -402,50 +404,112 @@ private: } void parsePacket(uint8_t msg_id, const uint8_t *data, uint8_t len) { - if (msg_id == MSG_NED_VEL) { - parseNEDVelocity(data, len); + switch (msg_id) { + case MSG_AHRS: + ahrs_count_++; + parseAHRS(data, len); + break; + case MSG_BODY_ACCELERATION: + body_acc_count_++; + parseBodyAcceleration(data, len); + break; + default: + break; } } - // 解析 NED 速度数据 - void parseNEDVelocity(const uint8_t *data, uint8_t len) { - if (len < 12) + // 解析 AHRS 数据 (0x41) + void parseAHRS(const uint8_t *data, uint8_t len) { + if (len < 48) return; - float vel_north = *reinterpret_cast(data); - float vel_east = *reinterpret_cast(data + 4); - float vel_down = *reinterpret_cast(data + 8); + // 解析角速度 (rad/s) + float roll_speed = *reinterpret_cast(data); + float pitch_speed = *reinterpret_cast(data + 4); + float heading_speed = *reinterpret_cast(data + 8); - // 发布 Twist 消息 - geometry_msgs::msg::Twist twist_msg; - twist_msg.linear.x = vel_north; // 北向速度 - twist_msg.linear.y = vel_east; // 东向速度 - twist_msg.linear.z = vel_down; // 地向速度 + // 解析欧拉角 (rad) + float roll = *reinterpret_cast(data + 12); + float pitch = *reinterpret_cast(data + 16); + float heading = *reinterpret_cast(data + 20); - ned_vel_pub_->publish(twist_msg); + // 解析四元数 + float q1 = *reinterpret_cast(data + 24); + float q2 = *reinterpret_cast(data + 28); + float q3 = *reinterpret_cast(data + 32); + float q4 = *reinterpret_cast(data + 36); - // 单独发布北向和东向速度（方便二维导航使用） - std_msgs::msg::Float32 north_msg; - north_msg.data = vel_north; - vel_north_pub_->publish(north_msg); + // 发布 IMU 消息 + sensor_msgs::msg::Imu imu_msg; + imu_msg.header.stamp = this->now(); + imu_msg.header.frame_id = "imu_link"; - std_msgs::msg::Float32 east_msg; - east_msg.data = vel_east; - vel_east_pub_->publish(east_msg); + // 角速度 (转换为 ROS 坐标系: x=roll, y=pitch, z=heading) + imu_msg.angular_velocity.x = roll_speed; + imu_msg.angular_velocity.y = pitch_speed; + imu_msg.angular_velocity.z = heading_speed; - // 增加接收计数 - ned_vel_received_count_++; + // 四元数 (FDI: Q1=w, Q2=x, Q3=y, Q4=z -> ROS: x,y,z,w) + imu_msg.orientation.x = q2; + imu_msg.orientation.y = q3; + imu_msg.orientation.z = q4; + imu_msg.orientation.w = q1; + + // 线性加速度暂时设为0（从 MSG_BODY_ACCELERATION 获取） + imu_msg.linear_acceleration.x = 0; + imu_msg.linear_acceleration.y = 0; + imu_msg.linear_acceleration.z = 0; + + ahrs_pub_->publish(imu_msg); // 根据 verbose 参数输出日志 bool verbose = this->get_parameter("verbose").as_bool(); if (verbose) { RCLCPP_INFO(this->get_logger(), - "NED速度 - 北向: %.3f, 东向: %.3f, 地向: %.3f m/s", vel_north, - vel_east, vel_down); + "AHRS - Roll: %.3f, Pitch: %.3f, Heading: %.3f rad", roll, + pitch, heading); } else { RCLCPP_DEBUG(this->get_logger(), - "NED速度 - 北向: %.3f, 东向: %.3f, 地向: %.3f m/s", - vel_north, vel_east, vel_down); + "AHRS - Roll: %.3f, Pitch: %.3f, Heading: %.3f rad", roll, + pitch, heading); + } + } + + // 解析机体系加速度数据 (0x62) + void parseBodyAcceleration(const uint8_t *data, uint8_t len) { + if (len < 16) + return; + + // 解析机体系加速度 (m/s²) + float acc_x = *reinterpret_cast(data); + float acc_y = *reinterpret_cast(data + 4); + float acc_z = *reinterpret_cast(data + 8); + + // 解析重力加速度 + float g_force = *reinterpret_cast(data + 12); + + // 发布机体系加速度 + geometry_msgs::msg::Vector3 acc_msg; + acc_msg.x = acc_x; + acc_msg.y = acc_y; + acc_msg.z = acc_z; + body_acc_pub_->publish(acc_msg); + + // 发布重力加速度 + std_msgs::msg::Float32 g_msg; + g_msg.data = g_force; + g_force_pub_->publish(g_msg); + + // 根据 verbose 参数输出日志 + bool verbose = this->get_parameter("verbose").as_bool(); + if (verbose) { + RCLCPP_INFO(this->get_logger(), + "BodyAcc - X: %.3f, Y: %.3f, Z: %.3f m/s², G: %.3f", acc_x, + acc_y, acc_z, g_force); + } else { + RCLCPP_DEBUG(this->get_logger(), + "BodyAcc - X: %.3f, Y: %.3f, Z: %.3f m/s², G: %.3f", acc_x, + acc_y, acc_z, g_force); } } @@ -456,15 +520,15 @@ private: if (is_connected_) { RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, - "IMU NED 速度接收正常 - 累计接收数据包: %zu", - ned_vel_received_count_.load()); + "IMU 接收正常 - AHRS: %zu, BodyAcc: %zu", + ahrs_count_.load(), body_acc_count_.load()); } } // 成员变量 std::string serial_port_; int baudrate_; - bool enable_crc_ = true; // CRC校验开关 + bool enable_crc_ = false; // CRC校验开关 int serial_fd_ = -1; bool is_connected_ = false; @@ -472,11 +536,12 @@ private: std::thread receive_thread_; // 统计信息 - std::atomic ned_vel_received_count_; + std::atomic ahrs_count_; + std::atomic body_acc_count_; - rclcpp::Publisher::SharedPtr ned_vel_pub_; - rclcpp::Publisher::SharedPtr vel_north_pub_; - rclcpp::Publisher::SharedPtr vel_east_pub_; + rclcpp::Publisher::SharedPtr ahrs_pub_; + rclcpp::Publisher::SharedPtr body_acc_pub_; + rclcpp::Publisher::SharedPtr g_force_pub_; rclcpp::Publisher::SharedPtr connection_status_pub_; rclcpp::TimerBase::SharedPtr status_timer_; };