惯导初步实现

Reviewed-on: #2

CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
 
 # 添加 SDK 库路径
 set(TRANSMITTER_SDK_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib/transmitter_sdk)
@@ -41,12 +42,25 @@ ament_target_dependencies(imu_receiver_node
   std_msgs
   geometry_msgs
   sensor_msgs
+  Eigen3
+)
+
+# ==================== 导航节点 ====================
+add_executable(navigation_node src/navigation_node.cpp)
+
+ament_target_dependencies(navigation_node 
+  rclcpp 
+  std_msgs
+  geometry_msgs
+  sensor_msgs
+  Eigen3
 )
 
 # ==================== 安装目标 ====================
 install(TARGETS 
   uart_transmitter_node
   imu_receiver_node
+  navigation_node
   DESTINATION lib/${PROJECT_NAME}
 )
 

README.md

@@ -21,10 +21,6 @@
 - MVSDK (工业相机 SDK)
 - 待补充...
 
-## 目标
-
-感谢同济 SuperPower 战队！自瞄算法参考同济的 sp_vision_26 。
-
 ## 软件架构
 
 ROS2 基本架构为节点。以每个节点的发布、解析、接收数据为例说明。
@@ -49,9 +45,9 @@ ROS2 基本架构为节点。以每个节点的发布、解析、接收数据为
 ### IMU 收发节点
 
 - 功能：IMU 与主机通信的封装节点。物理上读取 IMU（DETA10）数据
-- 连接：IMU_CP210x，协商 UART 通信速率 921600，`/dev/ttyUSB1`
+- 连接：IMU_CP210x，协商 UART 通信速率 921600，`/dev/ttyIMU`
 - 接收：IMU_CP210x -> 六轴加速度、陀螺仪数据
-- 发布：IMU 数据（地磁角、六轴角加速度、加速度、陀螺仪）
+- 发布：IMU 数据（加速度、陀螺仪）
 
 ### 中央节点
 
@@ -84,6 +80,15 @@ stateDiagram-v2
 ### 导航节点
 
 - 功能：根据 IMU 数据计算出当前位置，根据状态决定目标位置并发出移动到该位置所需的指令
-- 接收：收发节点 -> IMU.地磁角, IMU.速度, IMU.加速度, IMU.六轴陀螺仪
+- 接收：收发节点 -> IMU.加速度, IMU.六轴陀螺仪(AHRS)
         中央节点 -> 状态
 - 发布：导航运动指令
+
+考虑到 IMU 的实际情况，导航建模思路如下：
+- 地图范围：一个 $8 \times 12 m$ 的矩形，机器人运动的范围。以左下角为坐标原点 $ (0,0) $，地图对角为 $ (12,8) $，坐标单位为米。
+- 障碍区：不可跨越，不可侵入，由地图中两个矩形组成，第一个矩形的两个角点为 $ (2.5,1.5) ~ (4.5,8) $，第二个为 $ (7.5,0) ~ (9.5,6.5) $。
+- 基地：同样为场地中角点描述的矩形，我方基地为 $ (0,6) ~ (1.5,8) $。
+- 增益点：同样为场地中角点描述的矩形，范围为 $ (4.5,2.5) ~ (7.5,5.5) $.
+- 机器人：视为一直径 $ 0.75 m $ 的圆，圆心位置为 IMU 安装位置。开始时，圆心位于 $ (1.235,6.265) $。
+- 机器人的朝向和位置计算：IMU 安装于机器人圆心，开始时，IMU X 轴正方向（机器人朝向）与场地 Y 轴负方向相同，Y 轴正方向与场地 X 轴负方向相同，IMU Z 轴则朝向地面。当机器人朝向发生改变时，IMU 轴系的朝向也会相应发生改变，因此需要结合陀螺仪数据才能正确反向解算加速度计带来的加速度信息，从而完成惯性导航。
+- 根据已测量到的 AHRS 返回的数据，yaw 轴在 360° 范围内值为 $ (-3,3) $（左转为增，增到 3 再左转则为 -3）。根据最开始静止测量到的 yaw 值可以确定 **场地朝向**（因为开始时机器人始终朝向赛场坐标系 Y 轴负方向）。

docs/IMU/packet/MSG_IMU.md

@@ -0,0 +1,35 @@
+# MSG_IMU | FDISYSTEMS支持中心
+
+1. 机体坐标系及坐标系方向如图所示。在FDI系列产品中，模块表面都会标注准确的机体坐标系方向。具体以实际产品为准。
+
+2. 对于角速度，加速度的标定解释：
+   - (1) 转台标定：测量并补偿了陀螺仪和加速度计的零偏、尺度误差及三轴不垂直度。
+   - (2) 温箱标定：在宽温范围（如-40℃~85℃）下，测量了传感器误差随温度的变化规律，并生成了补偿模型。对陀螺和加表的输出数据进行了补偿。
+
+3. 对于加速度中未分离重力加速度解释：
+   加速度计测量的是载体运动与地球重力的"混合"结果。简单来说，当模块静止时，加速度读数并不为零，而会显示出一个重力值（例如，平放时Z轴约为+9.8 m/s²）。
+
+## 基本信息
+
+| 属性 | 值 |
+|------|-----|
+| **Packet ID** | 0x40 |
+| **Length** | 56 |
+| **Read / Write** | Read |
+
+## 数据字段
+
+| Offset | Size | Format | Field | Unit | Description |
+|--------|------|--------|-------|------|-------------|
+| 0 | 4 | float32_t | Gyroscope_X | rad/s | 机体系X轴角速度 |
+| 4 | 4 | float32_t | Gyroscope_Y | rad/s | 机体系Y轴角速度 |
+| 8 | 4 | float32_t | Gyroscope_Z | rad/s | 机体系Z轴角速度 |
+| 12 | 4 | float32_t | Accelerometer_X | m/s² | 机体系X轴加速度(未分离重力加速度) |
+| 16 | 4 | float32_t | Accelerometer_Y | m/s² | 机体系Y轴加速度(未分离重力加速度) |
+| 20 | 4 | float32_t | Accelerometer_Z | m/s² | 机体系Z轴加速度(未分离重力加速度) |
+| 24 | 4 | float32_t | Magnetometer_X | mG | 机体系X轴磁感应强度 |
+| 28 | 4 | float32_t | Magnetometer_Y | mG | 机体系Y轴磁感应强度 |
+| 32 | 4 | float32_t | Magnetometer_Z | mG | 机体系Z轴磁感应强度 |
+| 36 | 4 | float32_t | IMU_Temperature | deg C | 如果IMU数据由多个传感器组成则该值为这些传感器的平均温度 |
+| 40 | 4 | float32_t | Pressure | Pa | 气压值，如果没装气压计，默认为一个标准大气压101325Pa |
+| 48 | 8 | int64_t | Timestamp | us | 数据的时间戳，从上电开始启动的微秒数。时钟源为MCU外部晶振。 |

launch/uart_transmitter.launch.py

@@ -1,102 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-from launch.actions import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration
-
-
-def generate_launch_description():
-    # 声明启动参数
-    serial_port_arg = DeclareLaunchArgument(
-        'serial_port',
-        default_value='/dev/ttyCH340',
-        description='CH340 串口设备路径'
-    )
-    
-    baudrate_arg = DeclareLaunchArgument(
-        'baudrate',
-        default_value='115200',
-        description='串口波特率'
-    )
-    
-    send_frequency_arg = DeclareLaunchArgument(
-        'send_frequency',
-        default_value='10.0',
-        description='发送频率 (Hz)'
-    )
-    
-    # 控制参数
-    x_move_arg = DeclareLaunchArgument(
-        'x_move',
-        default_value='0',
-        description='平动左右 [-660, 660]'
-    )
-    
-    y_move_arg = DeclareLaunchArgument(
-        'y_move',
-        default_value='0',
-        description='平动前后 [-660, 660]'
-    )
-    
-    yaw_arg = DeclareLaunchArgument(
-        'yaw',
-        default_value='0',
-        description='云台偏航 [-660, 660]'
-    )
-    
-    pitch_arg = DeclareLaunchArgument(
-        'pitch',
-        default_value='0',
-        description='云台俯仰 [-660, 660]'
-    )
-    
-    feed_arg = DeclareLaunchArgument(
-        'feed',
-        default_value='0',
-        description='拨弹轮 [-660, 660]'
-    )
-    
-    left_switch_arg = DeclareLaunchArgument(
-        'left_switch',
-        default_value='3',
-        description='左拨杆 [1, 3]'
-    )
-    
-    right_switch_arg = DeclareLaunchArgument(
-        'right_switch',
-        default_value='3',
-        description='右拨杆 [1, 3]'
-    )
-
-    # 创建节点
-    uart_transmitter_node = Node(
-        package='amadeus_26',
-        executable='uart_transmitter_node',
-        name='uart_transmitter_node',
-        output='screen',
-        parameters=[{
-            'serial_port': LaunchConfiguration('serial_port'),
-            'baudrate': LaunchConfiguration('baudrate'),
-            'send_frequency': LaunchConfiguration('send_frequency'),
-            'x_move': LaunchConfiguration('x_move'),
-            'y_move': LaunchConfiguration('y_move'),
-            'yaw': LaunchConfiguration('yaw'),
-            'pitch': LaunchConfiguration('pitch'),
-            'feed': LaunchConfiguration('feed'),
-            'left_switch': LaunchConfiguration('left_switch'),
-            'right_switch': LaunchConfiguration('right_switch'),
-        }],
-    )
-
-    return LaunchDescription([
-        serial_port_arg,
-        baudrate_arg,
-        send_frequency_arg,
-        x_move_arg,
-        y_move_arg,
-        yaw_arg,
-        pitch_arg,
-        feed_arg,
-        left_switch_arg,
-        right_switch_arg,
-        uart_transmitter_node,
-    ])

src/imu_receiver_node.cpp

@@ -1,23 +1,34 @@
 /**
  * @file imu_receiver_node.cpp
- * @brief IMU 数据接收节点 (FDI DETA10) - 高实时性版本
+ * @brief IMU 数据接收节点 (FDI DETA10) - 环形缓冲区版本 + UKF
  *
  * 通过串口读取 FDI DETA10 IMU 模块的数据
  * 波特率：921600
  * 协议：FDILink
  *
  * 转发数据包：
- * - MSG_AHRS (0x41): 航姿参考系统数据
- * - MSG_BODY_ACCELERATION (0x62): 机体系加速度数据
+ * - MSG_BODY_ACCELERATION (0x62): 滤波修正后的机体系加速度（不包括重力）
+ * - MSG_AHRS (0x41): 航姿参考系统数据（四元数、角速度）
+ *
+ * 特性：
+ * - 使用环形缓冲区避免数据堆积爆发
+ * - 使用 UKF（无迹卡尔曼滤波器）对加速度数据进行滤波
+ * - 融合 AHRS 姿态数据和 UKF 滤波后的加速度
  */
 
 #include <atomic>
+#include <cmath>
+#include <condition_variable>
 #include <cstring>
+#include <eigen3/Eigen/Dense>
 #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 <mutex>
+#include <queue>
+#include <rclcpp/logging.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 #include <std_msgs/msg/bool.hpp>
@@ -33,8 +44,8 @@ constexpr uint8_t FRAME_START = 0xFC;
 constexpr uint8_t FRAME_END = 0xFD;
 
 // 数据包 ID
+constexpr uint8_t MSG_BODY_ACCELERATION = 0x62; // 滤波修正后的机体系加速度
 constexpr uint8_t MSG_AHRS = 0x41;              // 航姿参考系统数据
-constexpr uint8_t MSG_BODY_ACCELERATION = 0x62; // 机体系加速度数据
 
 // 默认串口设备
 constexpr const char *DEFAULT_SERIAL_PORT = "/dev/ttyIMU";
@@ -44,6 +55,20 @@ constexpr int DEFAULT_BAUDRATE = 921600;
 constexpr size_t SERIAL_READ_BUF_SIZE = 4096;
 constexpr size_t MAX_FRAME_SIZE = 256;
 
+// 环形缓冲区配置
+constexpr size_t RING_BUFFER_SIZE = 16;     // 环形缓冲区最大帧数
+constexpr size_t MAX_PUBLISH_RATE_HZ = 100; // 最大发布频率 100Hz
+constexpr auto PUBLISH_PERIOD =
+    std::chrono::microseconds(1000000 / MAX_PUBLISH_RATE_HZ); // 10ms
+
+// UKF 参数
+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,
@@ -97,34 +122,378 @@ static const uint16_t CRC16Table[256] = {
     0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, 0x6E17, 0x7E36, 0x4E55, 0x5E74,
     0x2E93, 0x3EB2, 0x0ED1, 0x1EF0};
 
+/**
+ * @brief 无迹卡尔曼滤波器 (UKF) 用于加速度滤波
+ *
+ * 状态向量: [位置, 速度]^T
+ * 测量: 位置
+ *
+ * 使用 Sigma 点变换处理非线性
+ */
+class UKF {
+public:
+  UKF()
+      : x_(Eigen::Vector2d::Zero()), P_(Eigen::Matrix2d::Identity()),
+        Q_(Eigen::Matrix2d::Identity() * 0.001),
+        R_(Eigen::Matrix<double, 1, 1>::Identity() * 0.03), dt_(0.01),
+        initialized_(false) {
+    // 初始化权重
+    computeWeights();
+  }
+
+  /**
+   * @brief 设置过程噪声
+   */
+  void setProcessNoise(double q) {
+    Q_ = Eigen::Matrix2d::Identity() * q;
+    Q_(1, 1) = q * 10; // 速度噪声更大一些
+  }
+
+  /**
+   * @brief 设置测量噪声
+   */
+  void setMeasurementNoise(double r) { R_(0, 0) = r; }
+
+  /**
+   * @brief 更新滤波器 - 简单的一维位置滤波
+   * @param measurement 测量值 (加速度/位置)
+   * @return 滤波后的值
+   */
+  double update(double measurement) {
+    if (!initialized_) {
+      x_(0) = measurement;
+      x_(1) = 0; // 初始速度为0
+      initialized_ = true;
+      return measurement;
+    }
+
+    // === 预测步骤 ===
+    // 生成 Sigma 点
+    Eigen::Matrix<double, UKF_STATE_DIM, UKF_SIGMA_POINTS> sigma_points;
+    generateSigmaPoints(sigma_points);
+
+    // 传播 Sigma 点通过状态转移方程
+    Eigen::Matrix<double, UKF_STATE_DIM, UKF_SIGMA_POINTS> sigma_points_pred;
+    for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
+      // 状态转移: 恒定速度模型
+      // x_k+1 = x_k + v_k * dt
+      // v_k+1 = v_k
+      sigma_points_pred(0, i) = sigma_points(0, i) + sigma_points(1, i) * dt_;
+      sigma_points_pred(1, i) = sigma_points(1, i);
+    }
+
+    // 计算预测均值
+    x_pred_.setZero();
+    for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
+      x_pred_ += weights_m_[i] * sigma_points_pred.col(i);
+    }
+
+    // 计算预测协方差
+    P_pred_.setZero();
+    for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
+      Eigen::Vector2d diff = sigma_points_pred.col(i) - x_pred_;
+      P_pred_ += weights_c_[i] * diff * diff.transpose();
+    }
+    P_pred_ += Q_;
+
+    // === 更新步骤 ===
+    // 传播 Sigma 点到测量空间
+    Eigen::Matrix<double, UKF_MEAS_DIM, UKF_SIGMA_POINTS> Z_sigma;
+    for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
+      // 测量模型: 只测量位置
+      Z_sigma(0, i) = sigma_points_pred(0, i);
+    }
+
+    // 计算预测测量均值
+    z_pred_.setZero();
+    for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
+      z_pred_ += weights_m_[i] * Z_sigma.col(i);
+    }
+
+    // 计算预测测量协方差
+    Eigen::Matrix<double, UKF_MEAS_DIM, UKF_MEAS_DIM> S;
+    S.setZero();
+    for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
+      Eigen::Matrix<double, UKF_MEAS_DIM, 1> diff = Z_sigma.col(i) - z_pred_;
+      S += weights_c_[i] * diff * diff.transpose();
+    }
+    S += R_;
+
+    // 计算互协方差
+    Eigen::Matrix<double, UKF_STATE_DIM, UKF_MEAS_DIM> T;
+    T.setZero();
+    for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
+      Eigen::Vector2d diff_x = sigma_points_pred.col(i) - x_pred_;
+      Eigen::Matrix<double, UKF_MEAS_DIM, 1> diff_z = Z_sigma.col(i) - z_pred_;
+      T += weights_c_[i] * diff_x * diff_z.transpose();
+    }
+
+    // 计算卡尔曼增益
+    Eigen::Matrix<double, UKF_STATE_DIM, UKF_MEAS_DIM> K = T * S.inverse();
+
+    // 更新状态
+    Eigen::Matrix<double, UKF_MEAS_DIM, 1> z;
+    z(0) = measurement;
+    x_ = x_pred_ + K * (z - z_pred_);
+
+    // 更新协方差
+    P_ = P_pred_ - K * S * K.transpose();
+
+    return x_(0);
+  }
+
+  /**
+   * @brief 设置采样时间
+   */
+  void setDt(double dt) { dt_ = dt; }
+
+  /**
+   * @brief 获取当前估计值
+   */
+  double getEstimate() const { return x_(0); }
+
+  /**
+   * @brief 获取当前速度估计
+   */
+  double getVelocity() const { return x_(1); }
+
+private:
+  /**
+   * @brief 计算 Sigma 点权重
+   */
+  void computeWeights() {
+    // 使用标准 UKF 参数
+    // alpha = 1e-3, beta = 2, kappa = 0
+    double alpha = 1e-3;
+    double beta = 2.0;
+    double kappa = 0.0;
+
+    lambda_ = alpha * alpha * (UKF_STATE_DIM + kappa) - UKF_STATE_DIM;
+
+    weights_m_.resize(UKF_SIGMA_POINTS);
+    weights_c_.resize(UKF_SIGMA_POINTS);
+
+    weights_m_[0] = lambda_ / (UKF_STATE_DIM + lambda_);
+    weights_c_[0] = weights_m_[0] + (1 - alpha * alpha + beta);
+
+    for (int i = 1; i < UKF_SIGMA_POINTS; ++i) {
+      weights_m_[i] = 0.5 / (UKF_STATE_DIM + lambda_);
+      weights_c_[i] = weights_m_[i];
+    }
+  }
+
+  /**
+   * @brief 生成 Sigma 点
+   */
+  void generateSigmaPoints(
+      Eigen::Matrix<double, UKF_STATE_DIM, UKF_SIGMA_POINTS> &sigma_points) {
+    // 计算矩阵平方根
+    Eigen::Matrix2d sqrt_P = ((UKF_STATE_DIM + lambda_) * P_).llt().matrixL();
+
+    sigma_points.col(0) = x_;
+
+    for (int i = 0; i < UKF_STATE_DIM; ++i) {
+      sigma_points.col(i + 1) = x_ + sqrt_P.col(i);
+      sigma_points.col(i + 1 + UKF_STATE_DIM) = x_ - sqrt_P.col(i);
+    }
+  }
+
+  // 状态向量 [位置, 速度]
+  Eigen::Vector2d x_;
+  Eigen::Vector2d x_pred_;
+
+  // 协方差矩阵
+  Eigen::Matrix2d P_;
+  Eigen::Matrix2d P_pred_;
+
+  // 过程噪声和测量噪声
+  Eigen::Matrix2d Q_;
+  Eigen::Matrix<double, UKF_MEAS_DIM, UKF_MEAS_DIM> R_;
+
+  // 预测测量
+  Eigen::Matrix<double, UKF_MEAS_DIM, 1> z_pred_;
+
+  // UKF 参数
+  double lambda_;
+  std::vector<double> weights_m_;
+  std::vector<double> weights_c_;
+
+  // 采样时间
+  double dt_;
+
+  // 初始化标志
+  bool initialized_;
+};
+
+// AHRS 数据结构
+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²) - 不包括重力
+  float acc_x = 0, acc_y = 0, acc_z = 0;
+
+  // 当地重力加速度 (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:
+  explicit RingBuffer(size_t size) : max_size_(size) {}
+
+  // 推入数据，如果满则丢弃最旧的数据
+  void push(const T &item) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (buffer_.size() >= max_size_) {
+      buffer_.pop(); // 丢弃最旧的数据
+    }
+    buffer_.push(item);
+    cond_var_.notify_one();
+  }
+
+  // 非阻塞弹出
+  bool try_pop(T &item) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (buffer_.empty()) {
+      return false;
+    }
+    item = buffer_.front();
+    buffer_.pop();
+    return true;
+  }
+
+  // 获取最新数据（不清除）
+  bool peek_latest(T &item) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (buffer_.empty()) {
+      return false;
+    }
+    item = buffer_.back(); // 返回最新的
+    return true;
+  }
+
+  // 清空缓冲区
+  void clear() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    while (!buffer_.empty())
+      buffer_.pop();
+  }
+
+  size_t size() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return buffer_.size();
+  }
+
+  bool empty() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return buffer_.empty();
+  }
+
+private:
+  std::queue<T> buffer_;
+  size_t max_size_;
+  std::mutex mutex_;
+  std::condition_variable cond_var_;
+};
+
 class ImuReceiverNode : public rclcpp::Node {
 public:
-  ImuReceiverNode() : Node("imu_receiver_node") {
+  ImuReceiverNode()
+      : 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);
     this->declare_parameter("verbose", true); // 是否输出详细日志
     this->declare_parameter("enable_crc", false); // 是否启用CRC校验（默认关闭）
 
+    // UKF 参数
+    this->declare_parameter("ukf_process_noise", 0.001);    // 过程噪声
+    this->declare_parameter("ukf_measurement_noise", 0.03); // 测量噪声
+    this->declare_parameter("ukf_dt", 0.01);                // 采样时间
+
     // 获取参数
     serial_port_ = this->get_parameter("serial_port").as_string();
     baudrate_ = this->get_parameter("baudrate").as_int();
     enable_crc_ = this->get_parameter("enable_crc").as_bool();
 
-    RCLCPP_INFO(this->get_logger(), "=================================");
-    RCLCPP_INFO(this->get_logger(), "IMU 数据接收节点启动 (高实时性版本)");
+    // 获取 UKF 参数并设置
+    double process_noise = this->get_parameter("ukf_process_noise").as_double();
+    double measurement_noise =
+        this->get_parameter("ukf_measurement_noise").as_double();
+    double dt = this->get_parameter("ukf_dt").as_double();
+
+    // 设置 UKF 参数
+    ukf_x_.setProcessNoise(process_noise);
+    ukf_x_.setMeasurementNoise(measurement_noise);
+    ukf_x_.setDt(dt);
+    ukf_y_.setProcessNoise(process_noise);
+    ukf_y_.setMeasurementNoise(measurement_noise);
+    ukf_y_.setDt(dt);
+    ukf_z_.setProcessNoise(process_noise);
+    ukf_z_.setMeasurementNoise(measurement_noise);
+    ukf_z_.setDt(dt);
+
+    RCLCPP_INFO(this->get_logger(), "---------------------------------");
+    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",
                 enable_crc_ ? "启用" : "禁用");
-    RCLCPP_INFO(this->get_logger(), "=================================");
+    RCLCPP_INFO(this->get_logger(),
+                "UKF 参数 - 过程噪声: %.4f, 测量噪声: %.4f, dt: %.4f",
+                process_noise, measurement_noise, dt);
+    RCLCPP_INFO(this->get_logger(), "---------------------------------");
 
     // 创建发布者
-    ahrs_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("imu/ahrs", 10);
-    body_acc_pub_ = this->create_publisher<geometry_msgs::msg::Vector3>(
+    imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
+    accel_pub_ = this->create_publisher<geometry_msgs::msg::Vector3>(
         "imu/body_acceleration", 10);
-    g_force_pub_ =
-        this->create_publisher<std_msgs::msg::Float32>("imu/g_force", 10);
     connection_status_pub_ = this->create_publisher<std_msgs::msg::Bool>(
         "imu/connection_status", 10);
 
@@ -138,12 +507,20 @@ public:
     // 创建接收线程
     receive_thread_ = std::thread(&ImuReceiverNode::receiveLoop, this);
 
+    // 创建融合线程
+    fusion_thread_ = std::thread(&ImuReceiverNode::fusionLoop, this);
+
+    // 创建处理线程（固定频率处理数据）
+    process_thread_ = std::thread(&ImuReceiverNode::processLoop, this);
+
     // 创建状态发布定时器
     status_timer_ = this->create_wall_timer(
         std::chrono::seconds(1),
         std::bind(&ImuReceiverNode::publishStatus, this));
 
     RCLCPP_INFO(this->get_logger(), "IMU 节点初始化完成");
+    RCLCPP_INFO(this->get_logger(), "环形缓冲区大小: %zu, 最大发布频率: %zu Hz",
+                RING_BUFFER_SIZE, MAX_PUBLISH_RATE_HZ);
   }
 
   ~ImuReceiverNode() {
@@ -151,6 +528,12 @@ public:
     if (receive_thread_.joinable()) {
       receive_thread_.join();
     }
+    if (fusion_thread_.joinable()) {
+      fusion_thread_.join();
+    }
+    if (process_thread_.joinable()) {
+      process_thread_.join();
+    }
     if (serial_fd_ >= 0) {
       close(serial_fd_);
     }
@@ -251,45 +634,33 @@ private:
     return crc16;
   }
 
-  // 高实时性接收循环 - 批量读取，无阻塞
+  // 接收循环 - 只负责解析数据并存入环形缓冲区
   void receiveLoop() {
-    // 使用双缓冲区策略：一个用于接收，一个用于处理
     std::vector<uint8_t> rx_buffer;
     rx_buffer.reserve(SERIAL_READ_BUF_SIZE * 2);
-
-    // 临时读取缓冲区
     uint8_t read_buf[SERIAL_READ_BUF_SIZE];
-
-    // 帧解析状态
     bool in_frame = false;
     std::vector<uint8_t> current_frame;
     current_frame.reserve(MAX_FRAME_SIZE);
 
     while (running_ && rclcpp::ok()) {
-      // 批量读取串口数据 - 非阻塞
       ssize_t n = read(serial_fd_, read_buf, sizeof(read_buf));
 
       if (n > 0) {
-        // 将新数据追加到缓冲区
         rx_buffer.insert(rx_buffer.end(), read_buf, read_buf + n);
 
-        // 处理缓冲区中的数据
         size_t i = 0;
         while (i < rx_buffer.size()) {
           uint8_t byte = rx_buffer[i];
 
           if (!in_frame) {
-            // 查找帧头
             if (byte == FRAME_START) {
-              // 检查是否有足够的字节来读取帧头信息
               if (i + 5 <= rx_buffer.size()) {
-                // 读取帧头信息
                 uint8_t msg_id = rx_buffer[i + 1];
                 uint8_t data_len = rx_buffer[i + 2];
                 uint8_t seq = rx_buffer[i + 3];
                 uint8_t header_crc = rx_buffer[i + 4];
 
-                // 可选的CRC校验
                 bool header_valid = true;
                 if (enable_crc_) {
                   uint8_t header[4] = {FRAME_START, msg_id, data_len, seq};
@@ -297,7 +668,6 @@ private:
                 }
 
                 if (header_valid) {
-                  // 开始收集帧数据
                   in_frame = true;
                   current_frame.clear();
                   current_frame.push_back(FRAME_START);
@@ -306,32 +676,24 @@ private:
                   current_frame.push_back(seq);
                   current_frame.push_back(header_crc);
 
-                  // 计算还需要读取多少字节
-                  size_t total_frame_len =
-                      5 + 2 + data_len + 1; // 头+CRC16+数据+尾
-                  size_t remaining = total_frame_len - 5; // 已经读取了5字节
+                  size_t total_frame_len = 5 + 2 + data_len + 1;
+                  size_t remaining = total_frame_len - 5;
 
-                  // 检查缓冲区中是否有足够的数据
                   if (i + 5 + remaining <= rx_buffer.size()) {
-                    // 数据足够，直接复制剩余部分
                     current_frame.insert(current_frame.end(),
                                          rx_buffer.begin() + i + 5,
                                          rx_buffer.begin() + i + 5 + remaining);
 
-                    // 检查帧尾
                     if (current_frame.back() == FRAME_END) {
                       // 解析帧
-                      processFrame(current_frame);
+                      parseFrame(current_frame);
                     }
-                    // 无论是否有帧尾，都结束当前帧处理
                     in_frame = false;
                     i += total_frame_len;
                     continue;
                   } else {
-                    // 数据不够，需要等待更多数据
-                    // 删除已处理的部分，保留当前帧的后续数据
                     rx_buffer.erase(rx_buffer.begin(), rx_buffer.begin() + i);
-                    i = 5; // 保留当前帧头
+                    i = 5;
                     break;
                   }
                 }
@@ -339,17 +701,12 @@ private:
             }
             i++;
           } else {
-            // 已经在帧中，继续收集数据
             current_frame.push_back(byte);
-
-            // 检查是否到达帧尾
             if (byte == FRAME_END) {
-              // 解析帧
-              processFrame(current_frame);
+              parseFrame(current_frame);
               in_frame = false;
               i++;
             } else if (current_frame.size() >= MAX_FRAME_SIZE) {
-              // 帧太长，丢弃
               in_frame = false;
               i++;
             } else {
@@ -358,40 +715,30 @@ private:
           }
         }
 
-        // 如果不在帧中且缓冲区太大，清理旧数据
         if (!in_frame && rx_buffer.size() > SERIAL_READ_BUF_SIZE) {
-          // 保留最后一部分数据，可能包含不完整的帧头
           if (rx_buffer.size() > 100) {
             rx_buffer.erase(rx_buffer.begin(), rx_buffer.end() - 100);
           }
         }
       } else if (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK) {
-        // 读取错误
         RCLCPP_ERROR(this->get_logger(), "串口读取错误: %s", strerror(errno));
       }
-      // n == 0 或 EAGAIN: 没有数据可读，继续循环（无sleep，保证实时性）
     }
   }
 
-  // 处理完整的一帧数据
-  void processFrame(const std::vector<uint8_t> &frame) {
-    // 最小帧长度检查：头(5) + CRC16(2) + 尾(1) = 8
+  // 解析帧 - 处理 MSG_BODY_ACCELERATION 和 MSG_AHRS
+  void parseFrame(const std::vector<uint8_t> &frame) {
     if (frame.size() < 8)
       return;
-
-    // 检查帧尾
     if (frame.back() != FRAME_END)
       return;
 
     uint8_t msg_id = frame[1];
     uint8_t data_len = frame[2];
-
-    // 验证数据长度
     size_t expected_len = 5 + 2 + data_len + 1;
     if (frame.size() != expected_len)
       return;
 
-    // 可选的数据CRC校验
     if (enable_crc_) {
       uint16_t data_crc = (frame[5] << 8) | frame[6];
       uint16_t calc_crc = calculateCRC16(frame.data() + 7, data_len);
@@ -399,144 +746,177 @@ private:
         return;
     }
 
-    // 解析数据包
-    parsePacket(msg_id, frame.data() + 7, data_len);
-  }
+    const uint8_t *payload = frame.data() + 7;
 
-  void parsePacket(uint8_t msg_id, const uint8_t *data, uint8_t 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;
+    // 处理 MSG_BODY_ACCELERATION 数据包 (0x62)
+    if (msg_id == MSG_BODY_ACCELERATION) {
+      if (data_len >= 16) {
+        AccelData data;
+        data.valid = true;
+
+        // 滤波修正后的机体系加速度 (m/s²) - 不包括重力
+        data.acc_x = *reinterpret_cast<const float *>(payload);
+        data.acc_y = *reinterpret_cast<const float *>(payload + 4);
+        data.acc_z = *reinterpret_cast<const float *>(payload + 8);
+
+        // 当地重力加速度 (m/s²)
+        data.g_force = *reinterpret_cast<const float *>(payload + 12);
+
+        data.receive_time = std::chrono::steady_clock::now();
+
+        // 推入加速度环形缓冲区
+        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 数据 (0x41)
-  void parseAHRS(const uint8_t *data, uint8_t len) {
-    if (len < 48)
-      return;
+  // 融合线程 - 同步加速度数据和 AHRS 数据
+  void fusionLoop() {
+    while (running_ && rclcpp::ok()) {
+      // 尝试获取最新的加速度和 AHRS 数据
+      AccelData accel_data;
+      AhrsData ahrs_data;
 
-    // 解析角速度 (rad/s)
-    float roll_speed = *reinterpret_cast<const float *>(data);
-    float pitch_speed = *reinterpret_cast<const float *>(data + 4);
-    float heading_speed = *reinterpret_cast<const float *>(data + 8);
+      bool has_accel = accel_ring_buffer_.peek_latest(accel_data);
+      bool has_ahrs = ahrs_ring_buffer_.peek_latest(ahrs_data);
 
-    // 解析欧拉角 (rad)
-    float roll = *reinterpret_cast<const float *>(data + 12);
-    float pitch = *reinterpret_cast<const float *>(data + 16);
-    float heading = *reinterpret_cast<const float *>(data + 20);
+      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);
 
-    // 解析四元数
-    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);
+        // 创建融合后的数据
+        ImuDataFrame fused_data;
+        fused_data.valid = true;
 
-    // 解析时间戳 (int64_t, us)
-    int64_t timestamp_us = *reinterpret_cast<const int64_t *>(data + 40);
+        // UKF 滤波后的加速度
+        fused_data.acc_x = filtered_acc_x;
+        fused_data.acc_y = filtered_acc_y;
+        fused_data.acc_z = filtered_acc_z;
 
-    // 将 IMU 时间戳转换为 ROS 时间
-    // 使用当前 ROS 时间作为基准，计算相对时间
-    rclcpp::Time current_time = this->now();
-    rclcpp::Time imu_time =
-        imu_base_time_ + rclcpp::Duration(0, timestamp_us * 1000);
+        // 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;
 
-    // 如果是第一个数据包，初始化基准时间
-    if (!imu_time_initialized_) {
-      imu_base_time_ = current_time - rclcpp::Duration(0, timestamp_us * 1000);
-      imu_time = current_time;
-      imu_time_initialized_ = true;
+        // 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));
     }
+  }
 
-    // 保存最新的 IMU 时间戳，供 BodyAcceleration 使用
-    latest_imu_timestamp_ = imu_time;
+  // 处理循环 - 固定频率从环形缓冲区读取并发布
+  void processLoop() {
+    ImuDataFrame data;
+    auto next_publish_time = std::chrono::steady_clock::now();
+
+    while (running_ && rclcpp::ok()) {
+      // 等待到下一次发布时间
+      std::this_thread::sleep_until(next_publish_time);
+      next_publish_time += PUBLISH_PERIOD;
+
+      // 获取最新的数据（如果有）
+      // 策略：只取最新的一个数据，丢弃中间堆积的数据
+      bool has_data = false;
+      ImuDataFrame latest_data;
+
+      // 清空缓冲区，只保留最新的
+      while (output_ring_buffer_.try_pop(data)) {
+        latest_data = data;
+        has_data = true;
+      }
+
+      if (has_data) {
+        publishData(latest_data);
+      }
+    }
+  }
+
+  // 发布数据
+  void publishData(const ImuDataFrame &data) {
+    // 发布加速度消息 (UKF 滤波后的数据)
+    geometry_msgs::msg::Vector3 accel_msg;
+    accel_msg.x = data.acc_x;
+    accel_msg.y = data.acc_y;
+    accel_msg.z = data.acc_z;
+    accel_pub_->publish(accel_msg);
 
     // 发布 IMU 消息
     sensor_msgs::msg::Imu imu_msg;
-    imu_msg.header.stamp = imu_time;
+    imu_msg.header.stamp = data.timestamp;
     imu_msg.header.frame_id = "imu_link";
 
-    // 角速度 (转换为 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;
+    // 线性加速度 - 使用 UKF 滤波后的值
+    imu_msg.linear_acceleration.x = data.acc_x;
+    imu_msg.linear_acceleration.y = data.acc_y;
+    imu_msg.linear_acceleration.z = data.acc_z;
 
-    // 四元数 (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;
+    // 角速度 - 使用 AHRS 提供的角速度
+    imu_msg.angular_velocity.x = data.gyro_x;
+    imu_msg.angular_velocity.y = data.gyro_y;
+    imu_msg.angular_velocity.z = data.gyro_z;
 
-    // 线性加速度暂时设为0（从 MSG_BODY_ACCELERATION 获取）
-    imu_msg.linear_acceleration.x = 0;
-    imu_msg.linear_acceleration.y = 0;
-    imu_msg.linear_acceleration.z = 0;
+    // 姿态 - 使用 AHRS 提供的四元数
+    imu_msg.orientation.x = data.q1;
+    imu_msg.orientation.y = data.q2;
+    imu_msg.orientation.z = data.q3;
+    imu_msg.orientation.w = data.q4;
 
-    ahrs_pub_->publish(imu_msg);
+    imu_pub_->publish(imu_msg);
 
-    // 根据 verbose 参数输出日志
-    bool verbose = this->get_parameter("verbose").as_bool();
-    if (verbose) {
-      RCLCPP_INFO(
-          this->get_logger(),
-          "AHRS - Roll: %.3f, Pitch: %.3f, Heading: %.3f rad, Time: %ld us",
-          roll, pitch, heading, timestamp_us);
-    } else {
-      RCLCPP_DEBUG(this->get_logger(),
-                   "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<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);
-
-    // 使用与 AHRS 相同的时间戳（如果可用），否则使用当前时间
-    rclcpp::Time msg_time = latest_imu_timestamp_;
-    if (msg_time.nanoseconds() == 0) {
-      msg_time = this->now();
-    }
-
-    // 发布机体系加速度
-    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/s2, G: %.3f", acc_x,
-                  acc_y, acc_z, g_force);
-    } else {
-      RCLCPP_DEBUG(this->get_logger(),
-                   "BodyAcc - X: %.3f, Y: %.3f, Z: %.3f m/s2, G: %.3f", acc_x,
-                   acc_y, acc_z, g_force);
-    }
+    // 日志
+    RCLCPP_INFO(this->get_logger(),
+                "Accel(UKF): [%.3f, %.3f, %.3f] m/s² | "
+                "Gyro: [%.3f, %.3f, %.3f] rad/s | "
+                "Quat: [%.3f, %.3f, %.3f, %.3f]",
+                data.acc_x, data.acc_y, data.acc_z, data.gyro_x, data.gyro_y,
+                data.gyro_z, data.q1, data.q2, data.q3, data.q4);
   }
 
   void publishStatus() {
@@ -544,10 +924,8 @@ private:
     msg.data = is_connected_ && (serial_fd_ >= 0);
     connection_status_pub_->publish(msg);
 
-    if (is_connected_) {
-      RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
-                           "IMU 接收正常 - AHRS: %zu, BodyAcc: %zu",
-                           ahrs_count_.load(), body_acc_count_.load());
+    if (!is_connected_) {
+      RCLCPP_WARN(this->get_logger(), "IMU 接收不正常");
     }
   }
 
@@ -560,19 +938,21 @@ private:
   bool is_connected_ = false;
   std::atomic<bool> running_{true};
   std::thread receive_thread_;
+  std::thread fusion_thread_;
 
-  // 统计信息
-  std::atomic<size_t> ahrs_count_;
-  std::atomic<size_t> body_acc_count_;
+  // 线程
+  std::thread process_thread_;
+  RingBuffer<AccelData> accel_ring_buffer_;
+  RingBuffer<AhrsData> ahrs_ring_buffer_;
+  RingBuffer<ImuDataFrame> output_ring_buffer_;
 
-  // IMU 时间戳同步
-  bool imu_time_initialized_ = false;
-  rclcpp::Time imu_base_time_;
-  rclcpp::Time latest_imu_timestamp_;
+  // UKF 滤波器（用于加速度滤波）
+  UKF ukf_x_;
+  UKF ukf_y_;
+  UKF ukf_z_;
 
-  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr ahrs_pub_;
-  rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr body_acc_pub_;
-  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr g_force_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr accel_pub_;
   rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr connection_status_pub_;
   rclcpp::TimerBase::SharedPtr status_timer_;
 };

src/navigation_node.cpp

@@ -0,0 +1,511 @@
+/**
+ * @file navigation_node.cpp
+ * @brief 导航节点 - 根据IMU数据进行惯性导航
+ *
+ * 功能：
+ * - 根据IMU加速度数据计算机器人位置（双重积分）
+ * - 根据AHRS的heading计算机器人在场地坐标系中的朝向
+ * - 订阅机器人状态，根据状态决定导航策略
+ *
+ * 坐标系说明：
+ * - 场地坐标系：左下角为原点(0,0)，X轴向右(0-12m)，Y轴向上(0-8m)
+ * - IMU坐标系：开始时X轴朝场地Y轴负方向，Y轴朝场地X轴负方向，Z轴朝地面
+ */
+
+#include <cmath>
+#include <deque>
+#include <eigen3/Eigen/Dense>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <std_msgs/msg/float64.hpp>
+#include <std_msgs/msg/int32.hpp>
+
+// 地图配置
+constexpr double MAP_WIDTH = 12.0;     // 场地宽度 (X方向) 单位：米
+constexpr double MAP_HEIGHT = 8.0;     // 场地高度 (Y方向) 单位：米
+constexpr double ROBOT_RADIUS = 0.375; // 机器人半径 0.75m直径
+
+// 初始位置
+constexpr double INITIAL_X = 1.235;
+constexpr double INITIAL_Y = 6.265;
+
+// 障碍区定义
+struct Rectangle {
+  double x1, y1, x2, y2;
+};
+
+constexpr Rectangle OBSTACLE1 = {2.5, 1.5, 4.5, 8.0};
+constexpr Rectangle OBSTACLE2 = {7.5, 0.0, 9.5, 6.5};
+
+// 我方基地
+constexpr Rectangle HOME_BASE = {0.0, 6.0, 1.5, 8.0};
+
+// 增益点
+constexpr Rectangle BUFF_ZONE = {4.5, 2.5, 7.5, 5.5};
+
+// 机器人状态
+enum class RobotState {
+  WAITING = 0,     // 等待
+  MOVING_TO_POINT, // 进点
+  SEARCHING,       // 索敌
+  ATTACKING,       // 攻击
+  RETURNING_HOME   // 回家
+};
+
+// 简单低通滤波器
+class LowPassFilter {
+public:
+  LowPassFilter(double alpha = 0.1) : alpha_(alpha), initialized_(false) {}
+
+  double filter(double value) {
+    if (!initialized_) {
+      last_value_ = value;
+      initialized_ = true;
+      return value;
+    }
+    last_value_ = alpha_ * value + (1.0 - alpha_) * last_value_;
+    return last_value_;
+  }
+
+  void reset() { initialized_ = false; }
+
+private:
+  double alpha_;
+  double last_value_;
+  bool initialized_;
+};
+
+// 滑动窗口平均滤波器
+class MovingAverageFilter {
+public:
+  explicit MovingAverageFilter(size_t window_size = 10)
+      : window_size_(window_size) {}
+
+  double filter(double value) {
+    buffer_.push_back(value);
+    if (buffer_.size() > window_size_) {
+      buffer_.pop_front();
+    }
+
+    if (buffer_.empty())
+      return value;
+
+    double sum = 0.0;
+    for (double v : buffer_) {
+      sum += v;
+    }
+    return sum / buffer_.size();
+  }
+
+  void reset() { buffer_.clear(); }
+
+private:
+  size_t window_size_;
+  std::deque<double> buffer_;
+};
+
+class NavigationNode : public rclcpp::Node {
+public:
+  NavigationNode() : Node("navigation_node") {
+    // 声明参数
+    this->declare_parameter("initial_x", INITIAL_X);
+    this->declare_parameter("initial_y", INITIAL_Y);
+    this->declare_parameter("velocity_decay", 0.95); // 速度衰减系数（零速校正）
+    this->declare_parameter("position_correction", 0.001); // 位置漂移校正
+    this->declare_parameter("static_threshold", 0.05); // 静止检测阈值 (m/s²)
+    this->declare_parameter("calibration_samples", 100); // 初始校准采样数
+    this->declare_parameter("acc_lpf_alpha", 0.1); // 加速度低通滤波系数
+    this->declare_parameter("vel_lpf_alpha", 0.2); // 速度低通滤波系数
+
+    // 获取参数
+    pos_x_ = this->get_parameter("initial_x").as_double();
+    pos_y_ = this->get_parameter("initial_y").as_double();
+    velocity_decay_ = this->get_parameter("velocity_decay").as_double();
+    position_correction_ =
+        this->get_parameter("position_correction").as_double();
+    static_threshold_ = this->get_parameter("static_threshold").as_double();
+    calibration_samples_ = this->get_parameter("calibration_samples").as_int();
+    acc_lpf_alpha_ = this->get_parameter("acc_lpf_alpha").as_double();
+    vel_lpf_alpha_ = this->get_parameter("vel_lpf_alpha").as_double();
+
+    // 初始化滤波器
+    acc_filter_x_.reset(new LowPassFilter(acc_lpf_alpha_));
+    acc_filter_y_.reset(new LowPassFilter(acc_lpf_alpha_));
+    vel_filter_x_.reset(new LowPassFilter(vel_lpf_alpha_));
+    vel_filter_y_.reset(new LowPassFilter(vel_lpf_alpha_));
+
+    RCLCPP_INFO(this->get_logger(), "========================================");
+    RCLCPP_INFO(this->get_logger(), "导航节点初始化");
+    RCLCPP_INFO(this->get_logger(), "初始位置: (%.3f, %.3f)", pos_x_, pos_y_);
+    RCLCPP_INFO(this->get_logger(), "速度衰减系数: %.3f", velocity_decay_);
+    RCLCPP_INFO(this->get_logger(), "静止检测阈值: %.3f m/s²",
+                static_threshold_);
+    RCLCPP_INFO(this->get_logger(), "校准采样数: %d", calibration_samples_);
+    RCLCPP_INFO(this->get_logger(), "========================================");
+
+    // 创建订阅者
+    imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
+        "imu/data", 10,
+        std::bind(&NavigationNode::imuCallback, this, std::placeholders::_1));
+
+    state_sub_ = this->create_subscription<std_msgs::msg::Int32>(
+        "robot_state", 10,
+        std::bind(&NavigationNode::stateCallback, this, std::placeholders::_1));
+
+    // 创建发布者
+    pose_pub_ =
+        this->create_publisher<geometry_msgs::msg::Point>("robot_pose", 10);
+    velocity_pub_ = this->create_publisher<geometry_msgs::msg::Vector3>(
+        "robot_velocity", 10);
+    heading_pub_ =
+        this->create_publisher<std_msgs::msg::Float64>("robot_heading", 10);
+    nav_cmd_pub_ =
+        this->create_publisher<geometry_msgs::msg::Vector3>("nav_cmd", 10);
+
+    // 创建定时器，用于定期发布导航信息
+    timer_ = this->create_wall_timer(
+        std::chrono::milliseconds(20), // 50Hz
+        std::bind(&NavigationNode::navigationLoop, this));
+
+    RCLCPP_INFO(this->get_logger(), "导航节点初始化完成");
+  }
+
+private:
+  // IMU数据回调
+  void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) {
+    // 提取四元数 (ROS格式: x, y, z, w)
+    double qx = msg->orientation.x;
+    double qy = msg->orientation.y;
+    double qz = msg->orientation.z;
+    double qw = msg->orientation.w;
+
+    // 将四元数转换为欧拉角 (roll, pitch, yaw)
+    // yaw: 绕Z轴旋转 (heading)
+    double siny_cosp = 2.0 * (qw * qz + qx * qy);
+    double cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz);
+    double yaw = std::atan2(siny_cosp, cosy_cosp);
+
+    // roll: 绕X轴旋转
+    double sinr_cosp = 2.0 * (qw * qx + qy * qz);
+    double cosr_cosp = 1.0 - 2.0 * (qx * qx + qy * qy);
+    double roll = std::atan2(sinr_cosp, cosr_cosp);
+
+    // pitch: 绕Y轴旋转
+    double sinp = 2.0 * (qw * qy - qz * qx);
+    double pitch = std::asin(std::clamp(sinp, -1.0, 1.0));
+
+    // 保存姿态数据
+    current_roll_ = roll;
+    current_pitch_ = pitch;
+    current_heading_ = yaw; // 这是IMU坐标系下的heading
+
+    // 初始校准阶段：收集初始heading样本
+    if (is_calibrating_) {
+      heading_samples_.push_back(yaw);
+      if (heading_samples_.size() >=
+          static_cast<size_t>(calibration_samples_)) {
+        // 计算平均初始heading
+        double sum = 0.0;
+        for (double h : heading_samples_) {
+          sum += h;
+        }
+        initial_heading_ = sum / heading_samples_.size();
+        is_calibrating_ = false;
+        RCLCPP_INFO(this->get_logger(),
+                    "初始校准完成! 初始heading: %.4f rad (%.2f°)",
+                    initial_heading_, initial_heading_ * 180.0 / M_PI);
+      }
+      return;
+    }
+
+    // 计算场地坐标系下的朝向
+    // 初始时IMU X轴朝场地Y轴负方向，即heading = -PI/2 对应场地朝向0
+    // 需要建立IMU heading与场地朝向的映射关系
+
+    // 计算相对于初始朝向的变化
+    double heading_delta = normalizeAngle(yaw - initial_heading_);
+
+    // 场地朝向：初始时机器人朝场地Y轴负方向（朝向下方）
+    // 当IMU顺时针旋转（yaw增大），场地朝向顺时针变化
+    field_heading_ = normalizeAngle(-M_PI_2 + heading_delta);
+
+    // 提取加速度 (IMU坐标系)
+    double acc_x_imu = msg->linear_acceleration.x;
+    double acc_y_imu = msg->linear_acceleration.y;
+
+    // 应用低通滤波
+    acc_x_imu = acc_filter_x_->filter(acc_x_imu);
+    acc_y_imu = acc_filter_y_->filter(acc_y_imu);
+
+    // 静止检测：加速度小于阈值时认为是静止状态
+    double acc_magnitude =
+        std::sqrt(acc_x_imu * acc_x_imu + acc_y_imu * acc_y_imu);
+    is_static_ = acc_magnitude < static_threshold_;
+
+    // 将加速度从IMU坐标系转换到场地坐标系
+    // IMU坐标系到场地坐标系的旋转：
+    // - 初始时IMU X轴朝场地-Y方向，IMU Y轴朝场地-X方向
+    // - 需要先将加速度旋转到与场地坐标系对齐
+
+    // 旋转矩阵：从IMU坐标系到场地坐标系
+    // 机器人朝向field_heading_时，需要将IMU加速度旋转
+    double cos_h = std::cos(field_heading_);
+    double sin_h = std::sin(field_heading_);
+
+    // IMU X轴朝机器人前方，Y轴朝机器人左方
+    // 场地坐标系：X向右，Y向上
+    // 机器人前方对应场地坐标系的(cos(field_heading_), sin(field_heading_))
+    // 机器人左方对应场地坐标系的(-sin(field_heading_), cos(field_heading_))
+
+    // 注意：IMU的X轴是机器人前方，Y轴是左方
+    // 场地坐标系转换：前方分量映射到场地朝向，左方分量垂直于场地朝向
+    acc_field_x_ = acc_x_imu * cos_h - acc_y_imu * sin_h;
+    acc_field_y_ = acc_x_imu * sin_h + acc_y_imu * cos_h;
+
+    // 获取当前时间
+    rclcpp::Time current_time = msg->header.stamp;
+    if (!last_time_.nanoseconds()) {
+      last_time_ = current_time;
+      return;
+    }
+
+    // 计算时间差
+    double dt = (current_time - last_time_).seconds();
+    last_time_ = current_time;
+
+    if (dt <= 0 || dt > 0.1) { // 时间差异常，跳过本次计算
+      return;
+    }
+
+    // 速度积分（使用梯形积分）
+    vel_x_ += (acc_field_x_ + last_acc_field_x_) * 0.5 * dt;
+    vel_y_ += (acc_field_y_ + last_acc_field_y_) * 0.5 * dt;
+
+    // 保存当前加速度用于下次积分
+    last_acc_field_x_ = acc_field_x_;
+    last_acc_field_y_ = acc_field_y_;
+
+    // 零速校正：如果检测到静止，逐渐减小速度
+    if (is_static_) {
+      vel_x_ *= velocity_decay_;
+      vel_y_ *= velocity_decay_;
+
+      // 如果速度很小，直接置零
+      if (std::abs(vel_x_) < 0.01)
+        vel_x_ = 0.0;
+      if (std::abs(vel_y_) < 0.01)
+        vel_y_ = 0.0;
+    }
+
+    // 应用速度滤波
+    vel_x_ = vel_filter_x_->filter(vel_x_);
+    vel_y_ = vel_filter_y_->filter(vel_y_);
+
+    // 位置积分（使用梯形积分）
+    pos_x_ += vel_x_ * dt;
+    pos_y_ += vel_y_ * dt;
+
+    // 边界检查：确保机器人在场地范围内
+    pos_x_ = std::clamp(pos_x_, ROBOT_RADIUS, MAP_WIDTH - ROBOT_RADIUS);
+    pos_y_ = std::clamp(pos_y_, ROBOT_RADIUS, MAP_HEIGHT - ROBOT_RADIUS);
+
+    has_imu_data_ = true;
+  }
+
+  // 状态回调
+  void stateCallback(const std_msgs::msg::Int32::SharedPtr msg) {
+    current_state_ = static_cast<RobotState>(msg->data);
+
+    // 根据状态更新目标位置
+    switch (current_state_) {
+    case RobotState::WAITING:
+      // 等待状态，不需要移动
+      break;
+    case RobotState::MOVING_TO_POINT:
+      // 进点：目标为增益点中心
+      target_x_ = (BUFF_ZONE.x1 + BUFF_ZONE.x2) / 2.0;
+      target_y_ = (BUFF_ZONE.y1 + BUFF_ZONE.y2) / 2.0;
+      break;
+    case RobotState::SEARCHING:
+      // 索敌：在增益点附近巡逻
+      target_x_ = (BUFF_ZONE.x1 + BUFF_ZONE.x2) / 2.0;
+      target_y_ = (BUFF_ZONE.y1 + BUFF_ZONE.y2) / 2.0;
+      break;
+    case RobotState::ATTACKING:
+      // 攻击：保持在当前位置或轻微调整
+      break;
+    case RobotState::RETURNING_HOME:
+      // 回家：目标为我方基地中心
+      target_x_ = (HOME_BASE.x1 + HOME_BASE.x2) / 2.0;
+      target_y_ = (HOME_BASE.y1 + HOME_BASE.y2) / 2.0;
+      break;
+    }
+  }
+
+  // 导航主循环
+  void navigationLoop() {
+    if (!has_imu_data_) {
+      return;
+    }
+
+    // 发布机器人位置 (使用Point，z表示heading)
+    auto pose_msg = geometry_msgs::msg::Point();
+    pose_msg.x = pos_x_;
+    pose_msg.y = pos_y_;
+    pose_msg.z = field_heading_; // 借用z字段存储heading
+    pose_pub_->publish(pose_msg);
+
+    // 发布速度
+    auto vel_msg = geometry_msgs::msg::Vector3();
+    vel_msg.x = vel_x_;
+    vel_msg.y = vel_y_;
+    vel_msg.z = 0.0; // 平面运动，Z向速度为0
+    velocity_pub_->publish(vel_msg);
+
+    // 发布朝向
+    auto heading_msg = std_msgs::msg::Float64();
+    heading_msg.data = field_heading_;
+    heading_pub_->publish(heading_msg);
+
+    // 计算导航指令（简单的位置控制）
+    geometry_msgs::msg::Vector3 nav_cmd;
+
+    if (current_state_ == RobotState::WAITING) {
+      // 等待状态：不移动
+      nav_cmd.x = 0.0; // 前进速度
+      nav_cmd.y = 0.0; // 横向速度
+      nav_cmd.z = 0.0; // 旋转速度
+    } else {
+      // 计算到目标点的向量
+      double dx = target_x_ - pos_x_;
+      double dy = target_y_ - pos_y_;
+      double distance = std::sqrt(dx * dx + dy * dy);
+
+      // 计算目标方向在场地坐标系中的角度
+      double target_angle = std::atan2(dy, dx);
+
+      // 计算需要旋转的角度（目标方向与当前朝向的差）
+      double angle_diff = normalizeAngle(target_angle - field_heading_);
+
+      // 简单的导航控制
+      // 如果距离目标较远，先朝向目标再前进
+      if (distance > 0.3) { // 30cm阈值
+        // 旋转控制：将角度差转换为旋转速度
+        double max_rot_speed = 1.0; // 最大旋转速度 rad/s
+        nav_cmd.z = std::clamp(angle_diff * 2.0, -max_rot_speed, max_rot_speed);
+
+        // 前进控制：当朝向接近目标方向时前进
+        if (std::abs(angle_diff) < 0.3) { // 约17度以内
+          double max_speed = 1.0;         // 最大前进速度 m/s
+          nav_cmd.x = std::min(distance * 2.0, max_speed);
+        } else {
+          nav_cmd.x = 0.0; // 先转向
+        }
+        nav_cmd.y = 0.0; // 暂时不使用横向移动
+      } else {
+        // 到达目标附近，停止
+        nav_cmd.x = 0.0;
+        nav_cmd.y = 0.0;
+        nav_cmd.z = 0.0;
+      }
+    }
+
+    nav_cmd_pub_->publish(nav_cmd);
+
+    // 日志输出（降低频率）
+    static int log_counter = 0;
+    if (++log_counter >= 10) {
+      log_counter = 0;
+      RCLCPP_INFO(this->get_logger(),
+                  "Pos: (%.3f, %.3f) | Heading: %.2f° | Vel: (%.3f, %.3f) | "
+                  "State: %d | Static: %s",
+                  pos_x_, pos_y_, field_heading_ * 180.0 / M_PI, vel_x_, vel_y_,
+                  static_cast<int>(current_state_), is_static_ ? "Yes" : "No");
+    }
+  }
+
+  // 角度归一化到 [-PI, PI]
+  double normalizeAngle(double angle) {
+    while (angle > M_PI)
+      angle -= 2.0 * M_PI;
+    while (angle < -M_PI)
+      angle += 2.0 * M_PI;
+    return angle;
+  }
+
+  // 订阅者
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr state_sub_;
+
+  // 发布者
+  rclcpp::Publisher<geometry_msgs::msg::Point>::SharedPtr pose_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr velocity_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr heading_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr nav_cmd_pub_;
+
+  // 定时器
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  // 状态
+  RobotState current_state_ = RobotState::WAITING;
+
+  // 位置 (场地坐标系)
+  double pos_x_ = INITIAL_X;
+  double pos_y_ = INITIAL_Y;
+
+  // 速度 (场地坐标系)
+  double vel_x_ = 0.0;
+  double vel_y_ = 0.0;
+
+  // 加速度 (场地坐标系)
+  double acc_field_x_ = 0.0;
+  double acc_field_y_ = 0.0;
+  double last_acc_field_x_ = 0.0;
+  double last_acc_field_y_ = 0.0;
+
+  // 姿态
+  double current_roll_ = 0.0;
+  double current_pitch_ = 0.0;
+  double current_heading_ = 0.0; // IMU坐标系下的heading
+  double field_heading_ = -M_PI_2; // 场地坐标系下的朝向（初始朝Y轴负方向）
+  double initial_heading_ = 0.0; // 初始heading（用于校准）
+
+  // 目标位置
+  double target_x_ = 0.0;
+  double target_y_ = 0.0;
+
+  // 滤波器
+  std::unique_ptr<LowPassFilter> acc_filter_x_;
+  std::unique_ptr<LowPassFilter> acc_filter_y_;
+  std::unique_ptr<LowPassFilter> vel_filter_x_;
+  std::unique_ptr<LowPassFilter> vel_filter_y_;
+
+  // 校准
+  std::vector<double> heading_samples_;
+  bool is_calibrating_ = true;
+  int calibration_samples_ = 100;
+
+  // 参数
+  double velocity_decay_ = 0.95;
+  double position_correction_ = 0.001;
+  double static_threshold_ = 0.05;
+  double acc_lpf_alpha_ = 0.1;
+  double vel_lpf_alpha_ = 0.2;
+
+  // 时间
+  rclcpp::Time last_time_;
+
+  // 标志
+  bool has_imu_data_ = false;
+  bool is_static_ = false;
+};
+
+int main(int argc, char *argv[]) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<NavigationNode>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}

src/uart_transmitter_node.cpp

@@ -38,13 +38,16 @@ public:
     this->declare_parameter("send_frequency", 50.0); // Hz
 
     // 控制参数
-    this->declare_parameter("x_move", 0);       // 平动左右 [-660, 660]
-    this->declare_parameter("y_move", 0);       // 平动前后 [-660, 660]
-    this->declare_parameter("yaw", 0);          // 云台偏航 [-660, 660]
-    this->declare_parameter("pitch", 0);        // 云台俯仰 [-660, 660]
-    this->declare_parameter("feed", 0);         // 拨弹轮 [-660, 660]
-    this->declare_parameter("left_switch", 0);  // 左拨杆 [1, 3]
-    this->declare_parameter("right_switch", 0); // 右拨杆 [1, 3]
+    this->declare_parameter("x_move", 0); // 平动左右 [-660, 660] 负为向左
+    this->declare_parameter("y_move", 0); // 平动前后 [-660, 660] 负为向后
+    this->declare_parameter("yaw", 0); // 云台偏航 [-660, 660] 负为向左
+    this->declare_parameter("pitch", 0); // 云台俯仰 [-660, 660] 负为向下
+    this->declare_parameter("feed",
+                            0); // 拨弹轮 [-660, 660]， 660 开火，0 无动作
+    this->declare_parameter("left_switch",
+                            3); // 左拨杆 [1, 3] 2 摩擦轮打开 3 关闭
+    this->declare_parameter("right_switch",
+                            3); // 右拨杆 [1, 3] 2 小陀螺 3 关闭
 
     // 获取参数
     serial_port_ = this->get_parameter("serial_port").as_string();
@@ -178,7 +181,7 @@ private:
     case 921600:
       return B921600;
     default:
-      RCLCPP_WARN(this->get_logger(), "不支持的波特率 %d，使用 115200",
+      RCLCPP_WARN(this->get_logger(), "不支持的波特率 %d, 使用 115200",
                   baudrate);
       return B115200;
     }
@@ -240,9 +243,10 @@ private:
     // CRC8 (除帧头和帧尾外的所有数据，不包括CRC本身)
     // 当前idx=13, 数据长度=11 (从frame[2]到frame[12])
     // uint8_t crc_value = calculateCRC8(frame + 2, 11);
-    frame[idx++] = 0xCC; // crc_value
+    // 帧尾 1，接收没有写 CRC8，因此固定为 CC
+    frame[idx++] = 0xCC;
 
-    // 帧尾
+    // 帧尾 2
     frame[idx++] = FRAME_TAIL;
 
     // 调试输出：打印完整帧内容