VSAG/amadeus_26
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

收信二节点完全实现

Browse Source
This commit is contained in:
Huang Haoyu
2026-03-24 01:24:15 +08:00
parent 860decf873
commit 01d236d1a0
6 changed files with 181 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

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

15
README.md
View File

@@ -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 数据（地磁角、六轴角加速度、加速度、陀螺仪）
### 中央节点

27
docs/IMU/packet/MSG_AHRS.md Normal file
View File

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

26
docs/IMU/packet/MSG_BODY_ACCELERATION.md Normal file
View File

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

1
package.xml
View File

@@ -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>

169
src/imu_receiver_node.cpp
View File

@@ -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 <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 速度
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<geometry_msgs::msg::Twist>(
"imu/ned_velocity", 10);
vel_north_pub_ = this->create_publisher<std_msgs::msg::Float32>(
"imu/velocity_north", 10);
vel_east_pub_ =
this->create_publisher<std_msgs::msg::Float32>("imu/velocity_east", 10);
ahrs_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("imu/ahrs", 10);
body_acc_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);
@@ -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) {
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<const float *>(data);
float vel_east = *reinterpret_cast<const float *>(data + 4);
float vel_down = *reinterpret_cast<const float *>(data + 8);
// 解析角速度 (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);
// 发布 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<const float *>(data + 12);
float pitch = *reinterpret_cast<const float *>(data + 16);
float heading = *reinterpret_cast<const float *>(data + 20);
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);
// 单独发布北向和东向速度（方便二维导航使用）
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<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",
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<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<std_msgs::msg::Float32>::SharedPtr vel_north_pub_;
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr vel_east_pub_;
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<std_msgs::msg::Bool>::SharedPtr connection_status_pub_;
rclcpp::TimerBase::SharedPtr status_timer_;
};