收信二节点完全实现

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
 )
 
 # ==================== 安装目标 ====================

README.md

@@ -10,8 +10,8 @@
 
 - MV-SUA133GC-T 工业相机 (USB)
 - NUC10i7 微型计算机 (主机)
-- FDI Systems DETA10 IMU 模块 (USB - CP210x 串口芯片 - UART) `/dev/ttyUSB1`
+- FDI Systems DETA10 IMU 模块 (USB - CP210x 串口芯片 - UART) `/dev/ttyCH340`
-- CH340 USB 转 UART 模块 (USB - 模块 - UART) `/dev/ttyUSB0`
+- CH340 USB 转 UART 模块 (USB - 模块 - UART) `/dev/ttyIMU`
 
 ### 软件
 
@@ -39,18 +39,19 @@ ROS2 基本架构为节点。以每个节点的发布、解析、接收数据为
 ### UART 收发节点
 
 - 功能：主机对外通信的封装节点。物理上读取收发模块
-- 连接：CH340，协商 UART 通信速率 115200，`/dev/ttyUSB0`
+- 连接：CH340，协商 UART 通信速率 115200，`/dev/ttyCH340`
-        IMU_CP210x，协商 UART 通信速率 921600，`/dev/ttyUSB1`
 - 接收：CH340.RX -> 裁判系统数据
 - 订阅：中央节点 -> 控制指令
 - 发送：CH340.TX -> 控制指令
-- 发布：IMU 数据（地磁角、六轴角加速度、加速度、陀螺仪）
+- 发布：裁判系统数据（血量、比赛当前状态）
-        裁判系统数据（血量、比赛当前状态）
         CH340 、IMU 连接 OK 标志
 
 ### IMU 收发节点
 
-- 功能：IMU 与
+- 功能：IMU 与主机通信的封装节点。物理上读取 IMU（DETA10）数据
+- 连接：IMU_CP210x，协商 UART 通信速率 921600，`/dev/ttyUSB1`
+- 接收：IMU_CP210x -> 六轴加速度、陀螺仪数据
+- 发布：IMU 数据（地磁角、六轴角加速度、加速度、陀螺仪）
 
 ### 中央节点
 

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外部晶振。 |

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 | 当地重力加速度 |

package.xml

@@ -12,6 +12,7 @@
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

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)
+ * - MSG_AHRS (0x41): 航姿参考系统数据
- * - Velocity_east: 东向速度 (m/s)
+ * - MSG_BODY_ACCELERATION (0x62): 机体系加速度数据
- * - Velocity_down: 地向速度 (m/s)
  */
 
 #include <atomic>
 #include <cstring>
 #include <errno.h>
 #include <fcntl.h>
+#include <geometry_msgs/msg/quaternion.hpp>
 #include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/imu.hpp>
 #include <std_msgs/msg/bool.hpp>
 #include <std_msgs/msg/float32.hpp>
+#include <std_msgs/msg/float64.hpp>
 #include <termios.h>
 #include <thread>
 #include <unistd.h>
@@ -30,8 +32,9 @@
 constexpr uint8_t FRAME_START = 0xFC;
 constexpr uint8_t FRAME_END = 0xFD;
 
-// 数据包 ID - 只保留 NED 速度
+// 数据包 ID
-constexpr uint8_t MSG_NED_VEL = 0x5F; // 卡尔曼滤波融合的北东地速度
+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<geometry_msgs::msg::Twist>(
+    ahrs_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("imu/ahrs", 10);
-        "imu/ned_velocity", 10);
+    body_acc_pub_ = this->create_publisher<geometry_msgs::msg::Vector3>(
-    vel_north_pub_ = this->create_publisher<std_msgs::msg::Float32>(
+        "imu/body_acceleration", 10);
-        "imu/velocity_north", 10);
+    g_force_pub_ =
-    vel_east_pub_ =
+        this->create_publisher<std_msgs::msg::Float32>("imu/g_force", 10);
-        this->create_publisher<std_msgs::msg::Float32>("imu/velocity_east", 10);
     connection_status_pub_ = this->create_publisher<std_msgs::msg::Bool>(
         "imu/connection_status", 10);
 
@@ -249,7 +251,7 @@ private:
     return crc16;
   }
 
-  // 接收循环
+  // 高实时性接收循环 - 批量读取，无阻塞
   void receiveLoop() {
     // 使用双缓冲区策略：一个用于接收，一个用于处理
     std::vector<uint8_t> 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) {
+    switch (msg_id) {
-      parseNEDVelocity(data, len);
+    case MSG_AHRS:
+      ahrs_count_++;
+      parseAHRS(data, len);
+      break;
+    case MSG_BODY_ACCELERATION:
+      body_acc_count_++;
+      parseBodyAcceleration(data, len);
+      break;
+    default:
+      break;
     }
   }
 
-  // 解析 NED 速度数据
+  // 解析 AHRS 数据 (0x41)
-  void parseNEDVelocity(const uint8_t *data, uint8_t len) {
+  void parseAHRS(const uint8_t *data, uint8_t len) {
-    if (len < 12)
+    if (len < 48)
       return;
 
-    float vel_north = *reinterpret_cast<const float *>(data);
+    // 解析角速度 (rad/s)
-    float vel_east = *reinterpret_cast<const float *>(data + 4);
+    float roll_speed = *reinterpret_cast<const float *>(data);
-    float vel_down = *reinterpret_cast<const float *>(data + 8);
+    float pitch_speed = *reinterpret_cast<const float *>(data + 4);
+    float heading_speed = *reinterpret_cast<const float *>(data + 8);
 
-    // 发布 Twist 消息
+    // 解析欧拉角 (rad)
-    geometry_msgs::msg::Twist twist_msg;
+    float roll = *reinterpret_cast<const float *>(data + 12);
-    twist_msg.linear.x = vel_north; // 北向速度
+    float pitch = *reinterpret_cast<const float *>(data + 16);
-    twist_msg.linear.y = vel_east;  // 东向速度
+    float heading = *reinterpret_cast<const float *>(data + 20);
-    twist_msg.linear.z = vel_down;  // 地向速度
 
-    ned_vel_pub_->publish(twist_msg);
+    // 解析四元数
+    float q1 = *reinterpret_cast<const float *>(data + 24);
+    float q2 = *reinterpret_cast<const float *>(data + 28);
+    float q3 = *reinterpret_cast<const float *>(data + 32);
+    float q4 = *reinterpret_cast<const float *>(data + 36);
 
-    // 单独发布北向和东向速度（方便二维导航使用）
+    // 发布 IMU 消息
-    std_msgs::msg::Float32 north_msg;
+    sensor_msgs::msg::Imu imu_msg;
-    north_msg.data = vel_north;
+    imu_msg.header.stamp = this->now();
-    vel_north_pub_->publish(north_msg);
+    imu_msg.header.frame_id = "imu_link";
 
-    std_msgs::msg::Float32 east_msg;
+    // 角速度 (转换为 ROS 坐标系: x=roll, y=pitch, z=heading)
-    east_msg.data = vel_east;
+    imu_msg.angular_velocity.x = roll_speed;
-    vel_east_pub_->publish(east_msg);
+    imu_msg.angular_velocity.y = pitch_speed;
+    imu_msg.angular_velocity.z = heading_speed;
 
-    // 增加接收计数
+    // 四元数 (FDI: Q1=w, Q2=x, Q3=y, Q4=z -> ROS: x,y,z,w)
-    ned_vel_received_count_++;
+    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,
+                  "AHRS - Roll: %.3f, Pitch: %.3f, Heading: %.3f rad", roll,
-                  vel_east, vel_down);
+                  pitch, heading);
     } else {
       RCLCPP_DEBUG(this->get_logger(),
-                   "NED速度 - 北向: %.3f, 东向: %.3f, 地向: %.3f m/s",
+                   "AHRS - Roll: %.3f, Pitch: %.3f, Heading: %.3f rad", roll,
-                   vel_north, vel_east, vel_down);
+                   pitch, heading);
+    }
+  }
+
+  // 解析机体系加速度数据 (0x62)
+  void parseBodyAcceleration(const uint8_t *data, uint8_t len) {
+    if (len < 16)
+      return;
+
+    // 解析机体系加速度 (m/s²)
+    float acc_x = *reinterpret_cast<const float *>(data);
+    float acc_y = *reinterpret_cast<const float *>(data + 4);
+    float acc_z = *reinterpret_cast<const float *>(data + 8);
+
+    // 解析重力加速度
+    float g_force = *reinterpret_cast<const float *>(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",
+                           "IMU 接收正常 - AHRS: %zu, BodyAcc: %zu",
-                           ned_vel_received_count_.load());
+                           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<size_t> ned_vel_received_count_;
+  std::atomic<size_t> ahrs_count_;
+  std::atomic<size_t> body_acc_count_;
 
-  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr ned_vel_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr ahrs_pub_;
-  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr vel_north_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr body_acc_pub_;
-  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr vel_east_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr g_force_pub_;
   rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr connection_status_pub_;
   rclcpp::TimerBase::SharedPtr status_timer_;
 };