滤波参数，四元数

src/imu_receiver_node.cpp

@@ -8,10 +8,12 @@
  *
  * 转发数据包：
  * - MSG_BODY_ACCELERATION (0x62): 滤波修正后的机体系加速度（不包括重力）
+ * - MSG_AHRS (0x41): 航姿参考系统数据（四元数、角速度）
  *
  * 特性：
  * - 使用环形缓冲区避免数据堆积爆发
  * - 使用 UKF（无迹卡尔曼滤波器）对加速度数据进行滤波
+ * - 融合 AHRS 姿态数据和 UKF 滤波后的加速度
  */
 
 #include <atomic>
@@ -41,8 +43,9 @@
 constexpr uint8_t FRAME_START = 0xFC;
 constexpr uint8_t FRAME_END = 0xFD;
 
-// 数据包 ID - 只使用 MSG_BODY_ACCELERATION
+// 数据包 ID
 constexpr uint8_t MSG_BODY_ACCELERATION = 0x62; // 滤波修正后的机体系加速度
+constexpr uint8_t MSG_AHRS = 0x41;              // 航姿参考系统数据
 
 // 默认串口设备
 constexpr const char *DEFAULT_SERIAL_PORT = "/dev/ttyIMU";
@@ -63,6 +66,9 @@ constexpr int UKF_STATE_DIM = 2; // 状态维度 [位置, 速度]
 constexpr int UKF_MEAS_DIM = 1;  // 测量维度 [位置]
 constexpr int UKF_SIGMA_POINTS = 2 * UKF_STATE_DIM + 1; // 2n+1 个 Sigma 点
 
+// 数据同步超时 (ms)
+constexpr int DATA_SYNC_TIMEOUT_MS = 10;
+
 // CRC8 查找表
 static const uint8_t CRC8Table[] = {
     0,   94,  188, 226, 97,  63,  221, 131, 194, 156, 126, 32,  163, 253, 31,
@@ -319,8 +325,32 @@ private:
   bool initialized_;
 };
 
-// IMU 数据结构 - 使用 MSG_BODY_ACCELERATION 数据包
+// AHRS 数据结构
-struct ImuDataFrame {
+struct AhrsData {
+  bool valid = false;
+
+  // 角速度 (rad/s)
+  float roll_speed = 0;
+  float pitch_speed = 0;
+  float heading_speed = 0;
+
+  // 欧拉角 (rad)
+  float roll = 0;
+  float pitch = 0;
+  float heading = 0;
+
+  // 四元数
+  float q1 = 0, q2 = 0, q3 = 0, q4 = 0;
+
+  // 时间戳 (us)
+  int64_t timestamp_us = 0;
+
+  // 接收时间
+  std::chrono::steady_clock::time_point receive_time;
+};
+
+// 加速度数据结构
+struct AccelData {
   bool valid = false;
 
   // 滤波修正后的机体系加速度 (m/s²) - 不包括重力
@@ -329,10 +359,27 @@ struct ImuDataFrame {
   // 当地重力加速度 (m/s²)
   float g_force = 0;
 
-  // 时间戳 (从接收到数据的时间)
+  // 接收时间
   std::chrono::steady_clock::time_point receive_time;
 };
 
+// 融合的 IMU 数据结构
+struct ImuDataFrame {
+  bool valid = false;
+
+  // 加速度 (UKF 滤波后)
+  float acc_x = 0, acc_y = 0, acc_z = 0;
+
+  // 角速度 (来自 AHRS)
+  float gyro_x = 0, gyro_y = 0, gyro_z = 0;
+
+  // 四元数 (来自 AHRS)
+  float q1 = 0, q2 = 0, q3 = 0, q4 = 0;
+
+  // 时间戳
+  rclcpp::Time timestamp;
+};
+
 // 线程安全的环形缓冲区
 template <typename T> class RingBuffer {
 public:
@@ -396,7 +443,9 @@ private:
 class ImuReceiverNode : public rclcpp::Node {
 public:
   ImuReceiverNode()
-      : Node("imu_receiver_node"), ring_buffer_(RING_BUFFER_SIZE) {
+      : Node("imu_receiver_node"), accel_ring_buffer_(RING_BUFFER_SIZE),
+        ahrs_ring_buffer_(RING_BUFFER_SIZE),
+        output_ring_buffer_(RING_BUFFER_SIZE) {
     // 声明参数
     this->declare_parameter("serial_port", DEFAULT_SERIAL_PORT);
     this->declare_parameter("baudrate", DEFAULT_BAUDRATE);
@@ -431,7 +480,7 @@ public:
     ukf_z_.setDt(dt);
 
     RCLCPP_INFO(this->get_logger(), "---------------------------------");
-    RCLCPP_INFO(this->get_logger(), "IMU 数据接收节点 (UKF 版本)");
+    RCLCPP_INFO(this->get_logger(), "IMU 数据接收节点 (UKF + AHRS 融合)");
     RCLCPP_INFO(this->get_logger(), "串口: %s", serial_port_.c_str());
     RCLCPP_INFO(this->get_logger(), "波特率: %d", baudrate_);
     RCLCPP_INFO(this->get_logger(), "CRC校验: %s",
@@ -458,6 +507,9 @@ public:
     // 创建接收线程
     receive_thread_ = std::thread(&ImuReceiverNode::receiveLoop, this);
 
+    // 创建融合线程
+    fusion_thread_ = std::thread(&ImuReceiverNode::fusionLoop, this);
+
     // 创建处理线程（固定频率处理数据）
     process_thread_ = std::thread(&ImuReceiverNode::processLoop, this);
 
@@ -476,6 +528,9 @@ public:
     if (receive_thread_.joinable()) {
       receive_thread_.join();
     }
+    if (fusion_thread_.joinable()) {
+      fusion_thread_.join();
+    }
     if (process_thread_.joinable()) {
       process_thread_.join();
     }
@@ -671,7 +726,7 @@ private:
     }
   }
 
-  // 解析帧 - 只处理 MSG_BODY_ACCELERATION
+  // 解析帧 - 处理 MSG_BODY_ACCELERATION 和 MSG_AHRS
   void parseFrame(const std::vector<uint8_t> &frame) {
     if (frame.size() < 8)
       return;
@@ -693,10 +748,10 @@ private:
 
     const uint8_t *payload = frame.data() + 7;
 
-    // 只处理 MSG_BODY_ACCELERATION 数据包 (0x62)
+    // 处理 MSG_BODY_ACCELERATION 数据包 (0x62)
     if (msg_id == MSG_BODY_ACCELERATION) {
       if (data_len >= 16) {
-        ImuDataFrame data;
+        AccelData data;
         data.valid = true;
 
         // 滤波修正后的机体系加速度 (m/s²) - 不包括重力
@@ -709,11 +764,91 @@ private:
 
         data.receive_time = std::chrono::steady_clock::now();
 
-        // 推入环形缓冲区
+        // 推入加速度环形缓冲区
-        ring_buffer_.push(data);
+        accel_ring_buffer_.push(data);
       }
     }
-    // 其他数据包类型被忽略
+    // 处理 MSG_AHRS 数据包 (0x41)
+    else if (msg_id == MSG_AHRS) {
+      if (data_len >= 48) {
+        AhrsData data;
+        data.valid = true;
+
+        // 角速度 (rad/s)
+        data.roll_speed = *reinterpret_cast<const float *>(payload);
+        data.pitch_speed = *reinterpret_cast<const float *>(payload + 4);
+        data.heading_speed = *reinterpret_cast<const float *>(payload + 8);
+
+        // 欧拉角 (rad)
+        data.roll = *reinterpret_cast<const float *>(payload + 12);
+        data.pitch = *reinterpret_cast<const float *>(payload + 16);
+        data.heading = *reinterpret_cast<const float *>(payload + 20);
+
+        // 四元数
+        data.q1 = *reinterpret_cast<const float *>(payload + 24);
+        data.q2 = *reinterpret_cast<const float *>(payload + 28);
+        data.q3 = *reinterpret_cast<const float *>(payload + 32);
+        data.q4 = *reinterpret_cast<const float *>(payload + 36);
+
+        // 时间戳 (us)
+        data.timestamp_us = *reinterpret_cast<const int64_t *>(payload + 40);
+
+        data.receive_time = std::chrono::steady_clock::now();
+
+        // 推入 AHRS 环形缓冲区
+        ahrs_ring_buffer_.push(data);
+      }
+    }
+  }
+
+  // 融合线程 - 同步加速度数据和 AHRS 数据
+  void fusionLoop() {
+    while (running_ && rclcpp::ok()) {
+      // 尝试获取最新的加速度和 AHRS 数据
+      AccelData accel_data;
+      AhrsData ahrs_data;
+
+      bool has_accel = accel_ring_buffer_.peek_latest(accel_data);
+      bool has_ahrs = ahrs_ring_buffer_.peek_latest(ahrs_data);
+
+      if (has_accel && has_ahrs) {
+        // 检查时间同步（简单策略：都取最新的）
+        // 使用 UKF 对加速度进行滤波
+        float filtered_acc_x = ukf_x_.update(accel_data.acc_x);
+        float filtered_acc_y = ukf_y_.update(accel_data.acc_y);
+        float filtered_acc_z = ukf_z_.update(accel_data.acc_z);
+
+        // 创建融合后的数据
+        ImuDataFrame fused_data;
+        fused_data.valid = true;
+
+        // UKF 滤波后的加速度
+        fused_data.acc_x = filtered_acc_x;
+        fused_data.acc_y = filtered_acc_y;
+        fused_data.acc_z = filtered_acc_z;
+
+        // AHRS 提供的角速度 (注意：AHRS中的RollSpeed对应X轴角速度)
+        fused_data.gyro_x = ahrs_data.roll_speed;
+        fused_data.gyro_y = ahrs_data.pitch_speed;
+        fused_data.gyro_z = ahrs_data.heading_speed;
+
+        // AHRS 提供的四元数 (q1是实部，ROS使用q4作为实部，注意转换)
+        // FDI: [q1, q2, q3, q4] where q1 is scalar
+        // ROS: [x, y, z, w] where w is scalar
+        fused_data.q1 = ahrs_data.q2; // x
+        fused_data.q2 = ahrs_data.q3; // y
+        fused_data.q3 = ahrs_data.q4; // z
+        fused_data.q4 = ahrs_data.q1; // w (实部)
+
+        fused_data.timestamp = this->now();
+
+        // 推入输出缓冲区
+        output_ring_buffer_.push(fused_data);
+      }
+
+      // 短暂休眠
+      std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
   }
 
   // 处理循环 - 固定频率从环形缓冲区读取并发布
@@ -732,7 +867,7 @@ private:
       ImuDataFrame latest_data;
 
       // 清空缓冲区，只保留最新的
-      while (ring_buffer_.try_pop(data)) {
+      while (output_ring_buffer_.try_pop(data)) {
         latest_data = data;
         has_data = true;
       }
@@ -743,51 +878,45 @@ private:
     }
   }
 
-  // 发布数据 - 使用 UKF 对加速度进行滤波
+  // 发布数据
   void publishData(const ImuDataFrame &data) {
-    rclcpp::Time current_time = this->now();
+    // 发布加速度消息 (UKF 滤波后的数据)
-
-    // 使用 UKF 对加速度数据进行滤波
-    float filtered_acc_x = ukf_x_.update(data.acc_x);
-    float filtered_acc_y = ukf_y_.update(data.acc_y);
-    float filtered_acc_z = ukf_z_.update(data.acc_z);
-
-    // 发布加速度消息 (滤波后的数据)
     geometry_msgs::msg::Vector3 accel_msg;
-    accel_msg.x = filtered_acc_x;
+    accel_msg.x = data.acc_x;
-    accel_msg.y = filtered_acc_y;
+    accel_msg.y = data.acc_y;
-    accel_msg.z = filtered_acc_z;
+    accel_msg.z = data.acc_z;
     accel_pub_->publish(accel_msg);
 
     // 发布 IMU 消息
     sensor_msgs::msg::Imu imu_msg;
-    imu_msg.header.stamp = current_time;
+    imu_msg.header.stamp = data.timestamp;
     imu_msg.header.frame_id = "imu_link";
 
     // 线性加速度 - 使用 UKF 滤波后的值
-    imu_msg.linear_acceleration.x = filtered_acc_x;
+    imu_msg.linear_acceleration.x = data.acc_x;
-    imu_msg.linear_acceleration.y = filtered_acc_y;
+    imu_msg.linear_acceleration.y = data.acc_y;
-    imu_msg.linear_acceleration.z = filtered_acc_z;
+    imu_msg.linear_acceleration.z = data.acc_z;
 
-    // 角速度和姿态暂时设为0（MSG_BODY_ACCELERATION 不包含这些数据）
+    // 角速度 - 使用 AHRS 提供的角速度
-    imu_msg.angular_velocity.x = 0;
+    imu_msg.angular_velocity.x = data.gyro_x;
-    imu_msg.angular_velocity.y = 0;
+    imu_msg.angular_velocity.y = data.gyro_y;
-    imu_msg.angular_velocity.z = 0;
+    imu_msg.angular_velocity.z = data.gyro_z;
 
-    imu_msg.orientation.x = 0;
+    // 姿态 - 使用 AHRS 提供的四元数
-    imu_msg.orientation.y = 0;
+    imu_msg.orientation.x = data.q1;
-    imu_msg.orientation.z = 0;
+    imu_msg.orientation.y = data.q2;
-    imu_msg.orientation.w = 1; // 单位四元数
+    imu_msg.orientation.z = data.q3;
+    imu_msg.orientation.w = data.q4;
 
     imu_pub_->publish(imu_msg);
 
     // 日志
-    RCLCPP_INFO(
+    RCLCPP_INFO(this->get_logger(),
-        this->get_logger(),
+                "Accel(UKF): [%.3f, %.3f, %.3f] m/s² | "
-        "Body Accel(raw): [%.3f, %.3f, %.3f] | Accel(UKF): [%.3f, %.3f, "
+                "Gyro: [%.3f, %.3f, %.3f] rad/s | "
-        "%.3f] m/s² | G-force: %.3f m/s²",
+                "Quat: [%.3f, %.3f, %.3f, %.3f]",
-        data.acc_x, data.acc_y, data.acc_z, filtered_acc_x, filtered_acc_y,
+                data.acc_x, data.acc_y, data.acc_z, data.gyro_x, data.gyro_y,
-        filtered_acc_z, data.g_force);
+                data.gyro_z, data.q1, data.q2, data.q3, data.q4);
   }
 
   void publishStatus() {
@@ -809,10 +938,13 @@ private:
   bool is_connected_ = false;
   std::atomic<bool> running_{true};
   std::thread receive_thread_;
+  std::thread fusion_thread_;
 
   // 线程
   std::thread process_thread_;
-  RingBuffer<ImuDataFrame> ring_buffer_;
+  RingBuffer<AccelData> accel_ring_buffer_;
+  RingBuffer<AhrsData> ahrs_ring_buffer_;
+  RingBuffer<ImuDataFrame> output_ring_buffer_;
 
   // UKF 滤波器（用于加速度滤波）
   UKF ukf_x_;