diff --git a/src/imu_receiver_node.cpp b/src/imu_receiver_node.cpp index e205c8e..1f69d14 100644 --- a/src/imu_receiver_node.cpp +++ b/src/imu_receiver_node.cpp @@ -8,10 +8,12 @@ * * 转发数据包： * - MSG_BODY_ACCELERATION (0x62): 滤波修正后的机体系加速度（不包括重力） + * - MSG_AHRS (0x41): 航姿参考系统数据（四元数、角速度） * * 特性： * - 使用环形缓冲区避免数据堆积爆发 * - 使用 UKF（无迹卡尔曼滤波器）对加速度数据进行滤波 + * - 融合 AHRS 姿态数据和 UKF 滤波后的加速度 */ #include @@ -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 数据包 -struct ImuDataFrame { +// 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²) - 不包括重力 @@ -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 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 &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(payload); + data.pitch_speed = *reinterpret_cast(payload + 4); + data.heading_speed = *reinterpret_cast(payload + 8); + + // 欧拉角 (rad) + data.roll = *reinterpret_cast(payload + 12); + data.pitch = *reinterpret_cast(payload + 16); + data.heading = *reinterpret_cast(payload + 20); + + // 四元数 + data.q1 = *reinterpret_cast(payload + 24); + data.q2 = *reinterpret_cast(payload + 28); + data.q3 = *reinterpret_cast(payload + 32); + data.q4 = *reinterpret_cast(payload + 36); + + // 时间戳 (us) + data.timestamp_us = *reinterpret_cast(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 对加速度数据进行滤波 - 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); - - // 发布加速度消息 (滤波后的数据) + // 发布加速度消息 (UKF 滤波后的数据) geometry_msgs::msg::Vector3 accel_msg; - accel_msg.x = filtered_acc_x; - accel_msg.y = filtered_acc_y; - accel_msg.z = filtered_acc_z; + 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 = 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.y = filtered_acc_y; - imu_msg.linear_acceleration.z = filtered_acc_z; + imu_msg.linear_acceleration.x = data.acc_x; + imu_msg.linear_acceleration.y = data.acc_y; + imu_msg.linear_acceleration.z = data.acc_z; - // 角速度和姿态暂时设为0（MSG_BODY_ACCELERATION 不包含这些数据） - imu_msg.angular_velocity.x = 0; - imu_msg.angular_velocity.y = 0; - imu_msg.angular_velocity.z = 0; + // 角速度 - 使用 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; - imu_msg.orientation.x = 0; - imu_msg.orientation.y = 0; - imu_msg.orientation.z = 0; - imu_msg.orientation.w = 1; // 单位四元数 + // 姿态 - 使用 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; imu_pub_->publish(imu_msg); // 日志 - RCLCPP_INFO( - this->get_logger(), - "Body Accel(raw): [%.3f, %.3f, %.3f] | Accel(UKF): [%.3f, %.3f, " - "%.3f] m/s² | G-force: %.3f m/s²", - data.acc_x, data.acc_y, data.acc_z, filtered_acc_x, filtered_acc_y, - filtered_acc_z, data.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() { @@ -809,10 +938,13 @@ private: bool is_connected_ = false; std::atomic running_{true}; std::thread receive_thread_; + std::thread fusion_thread_; // 线程 std::thread process_thread_; - RingBuffer ring_buffer_; + RingBuffer accel_ring_buffer_; + RingBuffer ahrs_ring_buffer_; + RingBuffer output_ring_buffer_; // UKF 滤波器（用于加速度滤波） UKF ukf_x_;