Compare commits
2 Commits
main
...
dev_transm
| Author | SHA1 | Date | |
|---|---|---|---|
| d2f759ab68 | |||
| f2ebd8e78a |
74
.gitignore
vendored
74
.gitignore
vendored
@@ -1,24 +1,33 @@
|
|||||||
# ROS 2 / Colcon
|
# ROS2
|
||||||
build/
|
build/
|
||||||
install/
|
install/
|
||||||
log/
|
log/
|
||||||
|
|
||||||
# CMake
|
# C/C++
|
||||||
CMakeCache.txt
|
|
||||||
CMakeFiles/
|
|
||||||
cmake_install.cmake
|
|
||||||
Makefile
|
|
||||||
CTestTestfile.cmake
|
|
||||||
Testing/
|
|
||||||
|
|
||||||
# C++ 编译输出
|
|
||||||
*.o
|
*.o
|
||||||
*.obj
|
|
||||||
*.so
|
*.so
|
||||||
*.a
|
*.a
|
||||||
*.dll
|
|
||||||
*.exe
|
*.exe
|
||||||
*.out
|
*.out
|
||||||
|
*.app
|
||||||
|
*.i
|
||||||
|
*.ii
|
||||||
|
*.s
|
||||||
|
*.su
|
||||||
|
*.gcno
|
||||||
|
*.gcda
|
||||||
|
*.dSYM/
|
||||||
|
*.stackdump
|
||||||
|
|
||||||
|
# Python
|
||||||
|
__pycache__/
|
||||||
|
*.py[cod]
|
||||||
|
*$py.class
|
||||||
|
.Python
|
||||||
|
*.egg-info/
|
||||||
|
dist/
|
||||||
|
build/
|
||||||
|
*.egg
|
||||||
|
|
||||||
# IDE
|
# IDE
|
||||||
.vscode/
|
.vscode/
|
||||||
@@ -26,22 +35,37 @@ Testing/
|
|||||||
*.swp
|
*.swp
|
||||||
*.swo
|
*.swo
|
||||||
*~
|
*~
|
||||||
|
|
||||||
# Python
|
|
||||||
__pycache__/
|
|
||||||
*.py[cod]
|
|
||||||
*$py.class
|
|
||||||
*.so
|
|
||||||
.Python
|
|
||||||
|
|
||||||
# 系统文件
|
|
||||||
.DS_Store
|
.DS_Store
|
||||||
Thumbs.db
|
|
||||||
|
|
||||||
# 临时文件
|
# Compiled files
|
||||||
|
*.com
|
||||||
|
*.class
|
||||||
|
*.dll
|
||||||
|
*.so
|
||||||
|
*.dylib
|
||||||
|
|
||||||
|
# Test files
|
||||||
|
test_debug
|
||||||
|
test_c
|
||||||
|
*.test
|
||||||
|
|
||||||
|
# Temporary files
|
||||||
*.tmp
|
*.tmp
|
||||||
*.temp
|
*.temp
|
||||||
*.log
|
*.log
|
||||||
|
*.bak
|
||||||
|
|
||||||
# 保留 compile_commands.json 给 clangd 使用
|
# Core dumps
|
||||||
# (不忽略 build/compile_commands.json，通过 CMake 导出到项目根目录)
|
core
|
||||||
|
core.*
|
||||||
|
vgcore.*
|
||||||
|
|
||||||
|
# Documentation build
|
||||||
|
docs/_build/
|
||||||
|
|
||||||
|
# Libraries (keep headers, ignore compiled)
|
||||||
|
# Note: We keep lib/ for SDK headers and .so files that are part of the project
|
||||||
|
|
||||||
|
# System specific
|
||||||
|
Thumbs.db
|
||||||
|
.directory
|
||||||
28
.vscode/settings.json
vendored
28
.vscode/settings.json
vendored
@@ -1,28 +1,8 @@
|
|||||||
{
|
{
|
||||||
// Clangd 配置
|
"clangd.arguments": [
|
||||||
"clangd.path": "/usr/bin/clangd",
|
|
||||||
"clangd.arguments": [
|
|
||||||
"--background-index",
|
|
||||||
"--compile-commands-dir=${workspaceFolder}/build",
|
"--compile-commands-dir=${workspaceFolder}/build",
|
||||||
"--header-insertion=iwyu",
|
"--completion-style=detailed",
|
||||||
"--completion-style=bundled",
|
"--query-driver=/usr/bin/clang",
|
||||||
"--pch-storage=memory",
|
"--header-insertion=never"
|
||||||
"--cross-file-rename"
|
|
||||||
],
|
],
|
||||||
// C++ 配置
|
|
||||||
"C_Cpp.intelliSenseEngine": "disabled", // 使用 clangd 替代默认引擎
|
|
||||||
"C_Cpp.autocomplete": "disabled",
|
|
||||||
"C_Cpp.errorSquiggles": "disabled",
|
|
||||||
// 文件关联
|
|
||||||
"files.associations": {
|
|
||||||
"*.h": "c",
|
|
||||||
"*.hpp": "cpp",
|
|
||||||
"*.cpp": "cpp"
|
|
||||||
},
|
|
||||||
// 编辑器配置
|
|
||||||
"editor.formatOnSave": true,
|
|
||||||
"editor.tabSize": 4,
|
|
||||||
"editor.insertSpaces": true,
|
|
||||||
// ROS 2 配置
|
|
||||||
"ros.distro": "humble"
|
|
||||||
}
|
}
|
||||||
@@ -5,16 +5,10 @@ project(amadeus_26)
|
|||||||
set(CMAKE_CXX_STANDARD 17)
|
set(CMAKE_CXX_STANDARD 17)
|
||||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||||
|
|
||||||
# 导出 compile_commands.json 给 clangd 使用
|
|
||||||
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
|
|
||||||
|
|
||||||
# 查找依赖
|
# 查找依赖
|
||||||
find_package(ament_cmake REQUIRED)
|
find_package(ament_cmake REQUIRED)
|
||||||
find_package(rclcpp REQUIRED)
|
find_package(rclcpp REQUIRED)
|
||||||
find_package(std_msgs REQUIRED)
|
find_package(std_msgs REQUIRED)
|
||||||
find_package(geometry_msgs REQUIRED)
|
|
||||||
find_package(sensor_msgs REQUIRED)
|
|
||||||
find_package(Eigen3 REQUIRED)
|
|
||||||
|
|
||||||
# 添加 SDK 库路径
|
# 添加 SDK 库路径
|
||||||
set(TRANSMITTER_SDK_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib/transmitter_sdk)
|
set(TRANSMITTER_SDK_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib/transmitter_sdk)
|
||||||
@@ -26,33 +20,34 @@ include_directories(
|
|||||||
${TRANSMITTER_SDK_PATH}/inc
|
${TRANSMITTER_SDK_PATH}/inc
|
||||||
)
|
)
|
||||||
|
|
||||||
# ==================== UART 节点 ====================
|
# 创建可执行文件
|
||||||
add_executable(uart_transmitter_node src/uart_transmitter_node.cpp)
|
add_executable(transmitter_test_node src/transmitter_test_node.cpp)
|
||||||
|
|
||||||
ament_target_dependencies(uart_transmitter_node
|
# 设置 RPATH，使可执行文件能找到共享库
|
||||||
|
set_target_properties(transmitter_test_node PROPERTIES
|
||||||
|
BUILD_WITH_INSTALL_RPATH FALSE
|
||||||
|
LINK_FLAGS "-Wl,-rpath,'${TRANSMITTER_SDK_PATH}/lib/linux/x64'"
|
||||||
|
)
|
||||||
|
|
||||||
|
# 链接依赖
|
||||||
|
ament_target_dependencies(transmitter_test_node
|
||||||
rclcpp
|
rclcpp
|
||||||
std_msgs
|
std_msgs
|
||||||
)
|
)
|
||||||
|
|
||||||
# ==================== IMU 节点 ====================
|
# 链接 SDK 库和依赖
|
||||||
add_executable(imu_receiver_node src/imu_receiver_node.cpp)
|
target_link_libraries(transmitter_test_node
|
||||||
|
dm_device
|
||||||
ament_target_dependencies(imu_receiver_node
|
usb-1.0
|
||||||
rclcpp
|
pthread
|
||||||
std_msgs
|
|
||||||
geometry_msgs
|
|
||||||
sensor_msgs
|
|
||||||
Eigen3
|
|
||||||
)
|
)
|
||||||
|
|
||||||
# ==================== 安装目标 ====================
|
# 安装目标
|
||||||
install(TARGETS
|
install(TARGETS transmitter_test_node
|
||||||
uart_transmitter_node
|
|
||||||
imu_receiver_node
|
|
||||||
DESTINATION lib/${PROJECT_NAME}
|
DESTINATION lib/${PROJECT_NAME}
|
||||||
)
|
)
|
||||||
|
|
||||||
# 安装 launch 文件
|
# 安装 launch 文件（如果有）
|
||||||
install(DIRECTORY launch
|
install(DIRECTORY launch
|
||||||
DESTINATION share/${PROJECT_NAME}
|
DESTINATION share/${PROJECT_NAME}
|
||||||
OPTIONAL
|
OPTIONAL
|
||||||
|
|||||||
40
README.md
40
README.md
@@ -10,8 +10,8 @@
|
|||||||
|
|
||||||
- MV-SUA133GC-T 工业相机 (USB)
|
- MV-SUA133GC-T 工业相机 (USB)
|
||||||
- NUC10i7 微型计算机 (主机)
|
- NUC10i7 微型计算机 (主机)
|
||||||
- FDI Systems DETA10 IMU 模块 (USB - CP210x 串口芯片 - UART) `/dev/ttyCH340`
|
- FDI Systems DETA10 IMU 模块 (USB - CP210x 串口芯片 - UART) `/dev/ttyUSB0`
|
||||||
- CH340 USB 转 UART 模块 (USB - 模块 - UART) `/dev/ttyIMU`
|
- 达妙 USB2CAN-DUAL USB 转 CAN 模块 (USB - 模块 - CAN*2) `/dev/ttyACM0`
|
||||||
|
|
||||||
### 软件
|
### 软件
|
||||||
|
|
||||||
@@ -19,8 +19,13 @@
|
|||||||
- OpenCV 4.13 with Contrib
|
- OpenCV 4.13 with Contrib
|
||||||
- Eigen 3
|
- Eigen 3
|
||||||
- MVSDK (工业相机 SDK)
|
- MVSDK (工业相机 SDK)
|
||||||
|
- dm_device (USB 转 CAN 模块 SDK)
|
||||||
- 待补充...
|
- 待补充...
|
||||||
|
|
||||||
|
## 目标
|
||||||
|
|
||||||
|
感谢同济 SuperPower 战队！自瞄算法参考同济的 sp_vision_26 。项目目标为使 2026 年采用的
|
||||||
|
|
||||||
## 软件架构
|
## 软件架构
|
||||||
|
|
||||||
ROS2 基本架构为节点。以每个节点的发布、解析、接收数据为例说明。
|
ROS2 基本架构为节点。以每个节点的发布、解析、接收数据为例说明。
|
||||||
@@ -32,22 +37,17 @@ ROS2 基本架构为节点。以每个节点的发布、解析、接收数据为
|
|||||||
- 发布：描述该节点使用 ROS 发布的数据
|
- 发布：描述该节点使用 ROS 发布的数据
|
||||||
- 发送：描述该节点使用直连硬件发送的数据
|
- 发送：描述该节点使用直连硬件发送的数据
|
||||||
|
|
||||||
### UART 收发节点
|
### 收发节点
|
||||||
|
|
||||||
- 功能：主机对外通信的封装节点。物理上读取收发模块
|
- 功能：主机对外通信的封装节点。物理上读取收发模块
|
||||||
- 连接：CH340，协商 UART 通信速率 115200，`/dev/ttyCH340`
|
- 连接：USB2CAN，协商 CAN 通信速率 1000kbps，`/dev/ttyACM0`
|
||||||
- 接收：CH340.RX -> 裁判系统数据
|
IMU_CP210x，协商 UART 通信速率 921600，`/dev/ttyUSB0`
|
||||||
|
- 接收：USB2CAN -> 裁判系统数据
|
||||||
- 订阅：中央节点 -> 控制指令
|
- 订阅：中央节点 -> 控制指令
|
||||||
- 发送：CH340.TX -> 控制指令
|
- 发送：USB2CAN.CAN2 -> 控制指令
|
||||||
- 发布：裁判系统数据（血量、比赛当前状态）
|
- 发布：IMU 数据（地磁角、六轴角加速度、加速度、陀螺仪）
|
||||||
CH340 、IMU 连接 OK 标志
|
裁判系统数据（血量、比赛当前状态）
|
||||||
|
USB2CAN 、IMU 连接 OK 标志
|
||||||
### IMU 收发节点
|
|
||||||
|
|
||||||
- 功能：IMU 与主机通信的封装节点。物理上读取 IMU（DETA10）数据
|
|
||||||
- 连接：IMU_CP210x，协商 UART 通信速率 921600，`/dev/ttyIMU`
|
|
||||||
- 接收：IMU_CP210x -> 六轴加速度、陀螺仪数据
|
|
||||||
- 发布：IMU 数据（加速度、陀螺仪）
|
|
||||||
|
|
||||||
### 中央节点
|
### 中央节点
|
||||||
|
|
||||||
@@ -80,14 +80,6 @@ stateDiagram-v2
|
|||||||
### 导航节点
|
### 导航节点
|
||||||
|
|
||||||
- 功能：根据 IMU 数据计算出当前位置，根据状态决定目标位置并发出移动到该位置所需的指令
|
- 功能：根据 IMU 数据计算出当前位置，根据状态决定目标位置并发出移动到该位置所需的指令
|
||||||
- 接收：收发节点 -> IMU.加速度, IMU.六轴陀螺仪(AHRS)
|
- 接收：收发节点 -> IMU.地磁角, IMU.速度, IMU.加速度, IMU.六轴陀螺仪
|
||||||
中央节点 -> 状态
|
中央节点 -> 状态
|
||||||
- 发布：导航运动指令
|
- 发布：导航运动指令
|
||||||
|
|
||||||
考虑到 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 轴系的朝向也会相应发生改变，因此需要结合陀螺仪数据才能正确反向解算加速度计带来的加速度信息，从而完成惯性导航。IMU 安装并不完全平整，且机器人运动有颠簸，因此需要将 Z 轴数据结合解算出在二维平面上的加速度和位移。
|
|
||||||
@@ -1,27 +0,0 @@
|
|||||||
# 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外部晶振。 |
|
|
||||||
@@ -1,26 +0,0 @@
|
|||||||
# MSG_BODY_ACCELERATION | FDISYSTEMS支持中心
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
[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,35 +0,0 @@
|
|||||||
# 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外部晶振。 |
|
|
||||||
@@ -1,19 +0,0 @@
|
|||||||
# MSG_NED_VEL | FDISYSTEMS支持中心
|
|
||||||
|
|
||||||
该数据包用于描述：卡尔曼滤波融合的北东地速度
|
|
||||||
|
|
||||||
## 基本信息
|
|
||||||
|
|
||||||
| 属性 | 值 |
|
|
||||||
|------|-----|
|
|
||||||
| **Packet ID** | 0x5F |
|
|
||||||
| **Length** | 12 |
|
|
||||||
| **Read / Write** | Read |
|
|
||||||
|
|
||||||
## 数据字段
|
|
||||||
|
|
||||||
| Offset | Size | Format | Field | Unit | Description |
|
|
||||||
|--------|------|--------|-------|------|-------------|
|
|
||||||
| 0 | 4 | float32_t | Velocity_north | m/s | 滤波修正的北向速度 |
|
|
||||||
| 4 | 4 | float32_t | Velocity_east | m/s 7 | 滤波修正的东向速度 |
|
|
||||||
| 8 | 4 | float32_t | Velocity_down | m/s | 滤波修正的地向速度 |
|
|
||||||
178
docs/TRANSMITTER_TEST.md
Normal file
178
docs/TRANSMITTER_TEST.md
Normal file
@@ -0,0 +1,178 @@
|
|||||||
|
# 收发模块测试节点使用说明
|
||||||
|
|
||||||
|
## 功能概述
|
||||||
|
|
||||||
|
`transmitter_test_node` 是一个用于测试 USB2CAN 收发模块功能的 ROS2 节点，主要功能包括：
|
||||||
|
|
||||||
|
1. **设备初始化**：自动查找并连接 USB2CAN 设备
|
||||||
|
2. **定时发送**：以指定频率发送测试控制指令
|
||||||
|
- ID 0x149: 运动控制指令 (X/Y 平动、偏航、俯仰)
|
||||||
|
- ID 0x189: 攻击控制指令 (拨弹盘、小陀螺、摩擦轮)
|
||||||
|
3. **数据接收**：接收并打印 CAN 总线上的数据
|
||||||
|
4. **状态监控**：发布连接状态到 ROS 话题
|
||||||
|
5. **环路测试**：支持双通道环路测试（通道0发，通道1收）
|
||||||
|
|
||||||
|
## 编译
|
||||||
|
|
||||||
|
```bash
|
||||||
|
# 在工作空间根目录执行
|
||||||
|
colcon build --packages-select amadeus_26
|
||||||
|
```
|
||||||
|
|
||||||
|
## 运行
|
||||||
|
|
||||||
|
### 方式一：直接运行
|
||||||
|
|
||||||
|
```bash
|
||||||
|
# 设置环境
|
||||||
|
source install/setup.bash
|
||||||
|
|
||||||
|
# 运行节点（使用默认参数）
|
||||||
|
ros2 run amadeus_26 transmitter_test_node
|
||||||
|
|
||||||
|
# 或指定参数运行
|
||||||
|
ros2 run amadeus_26 transmitter_test_node --ros-args \
|
||||||
|
-p tx_channel:=0 \
|
||||||
|
-p rx_channel:=1 \
|
||||||
|
-p can_id_motion:=329 \
|
||||||
|
-p can_id_attack:=393
|
||||||
|
```
|
||||||
|
|
||||||
|
### 方式二：使用 Launch 文件
|
||||||
|
|
||||||
|
```bash
|
||||||
|
# 设置环境
|
||||||
|
source install/setup.bash
|
||||||
|
|
||||||
|
# 使用默认参数启动
|
||||||
|
ros2 launch amadeus_26 transmitter_test.launch.py
|
||||||
|
|
||||||
|
# 或指定参数启动
|
||||||
|
ros2 launch amadeus_26 transmitter_test.launch.py \
|
||||||
|
tx_channel:=0 \
|
||||||
|
rx_channel:=1 \
|
||||||
|
can_id_motion:=329 \
|
||||||
|
can_id_attack:=393 \
|
||||||
|
use_extended_frame:=false \
|
||||||
|
use_canfd:=false
|
||||||
|
```
|
||||||
|
|
||||||
|
## 参数说明
|
||||||
|
|
||||||
|
| 参数名 | 类型 | 默认值 | 说明 |
|
||||||
|
|--------|------|--------|------|
|
||||||
|
| `send_frequency` | double | 50.0 | CAN 发送频率 (Hz) |
|
||||||
|
| `tx_channel` | int | 0 | CAN 发送通道 (0 或 1) |
|
||||||
|
| `rx_channel` | int | 1 | CAN 接收通道 (0 或 1) |
|
||||||
|
| `can_id_motion` | int | 329 (0x149) | 运动控制 CAN ID |
|
||||||
|
| `can_id_attack` | int | 393 (0x189) | 攻击控制 CAN ID |
|
||||||
|
| `use_extended_frame` | bool | false | 是否使用扩展帧(29位ID) |
|
||||||
|
| `use_canfd` | bool | false | 是否使用 CANFD |
|
||||||
|
| `use_brs` | bool | false | CANFD 是否启用波特率切换 |
|
||||||
|
| `rx_filter_enabled` | bool | false | 是否启用接收 ID 过滤 |
|
||||||
|
| `rx_filter_id_min` | int | 0 | 接收过滤 ID 最小值 |
|
||||||
|
| `rx_filter_id_max` | int | 2047 | 接收过滤 ID 最大值 |
|
||||||
|
|
||||||
|
## 话题
|
||||||
|
|
||||||
|
### 发布的话题
|
||||||
|
|
||||||
|
- `/transmitter/connection_status` (std_msgs/Bool): USB2CAN 连接状态
|
||||||
|
|
||||||
|
## CAN 协议
|
||||||
|
|
||||||
|
### 运动控制 (ID: 0x149)
|
||||||
|
|
||||||
|
数据格式 (8 bytes):
|
||||||
|
```
|
||||||
|
[0-1]: X轴平动 (左右), 范围 [-660, 660]
|
||||||
|
[2-3]: Y轴平动 (前后), 范围 [-660, 660]
|
||||||
|
[4-5]: 云台偏航, 范围 [-660, 660]
|
||||||
|
[6-7]: 云台俯仰, 范围 [-660, 660]
|
||||||
|
```
|
||||||
|
|
||||||
|
### 攻击控制 (ID: 0x189)
|
||||||
|
|
||||||
|
数据格式 (8 bytes):
|
||||||
|
```
|
||||||
|
[0-1]: 拨弹盘 (660=进弹, 0=关闭, -660=退弹)
|
||||||
|
[2-3]: 小陀螺开关 (660=开启, 0=关闭)
|
||||||
|
[4-5]: 摩擦轮开关 (660=开启, 0=关闭)
|
||||||
|
[6-7]: 预留
|
||||||
|
```
|
||||||
|
|
||||||
|
## 环路测试
|
||||||
|
|
||||||
|
用于调试收发功能，将两个 CAN 端口连接在一起：
|
||||||
|
|
||||||
|
### 物理连接
|
||||||
|
|
||||||
|
```
|
||||||
|
USB2CAN 模块
|
||||||
|
├── 通道0 (CAN0) ──┐
|
||||||
|
│ - CAN_H │
|
||||||
|
│ - CAN_L ├── 短接在一起（H-H, L-L）
|
||||||
|
├── 通道1 (CAN1) ──┘
|
||||||
|
- CAN_H
|
||||||
|
- CAN_L
|
||||||
|
```
|
||||||
|
|
||||||
|
**注意**：需要在总线两端各加一个 **120Ω 终端电阻**！
|
||||||
|
|
||||||
|
### 运行环路测试
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 launch amadeus_26 transmitter_test.launch.py \
|
||||||
|
tx_channel:=0 \
|
||||||
|
rx_channel:=1
|
||||||
|
```
|
||||||
|
|
||||||
|
### 预期输出
|
||||||
|
|
||||||
|
```
|
||||||
|
[发送回调] ID: 0x149
|
||||||
|
[发送回调] ID: 0x189
|
||||||
|
[接收回调] 通道1 ID: 0x149, 长度: 8, 数据: ...
|
||||||
|
[接收回调] 通道1 ID: 0x189, 长度: 8, 数据: ...
|
||||||
|
```
|
||||||
|
|
||||||
|
## 注意事项
|
||||||
|
|
||||||
|
1. **设备权限**：确保当前用户有权限访问 USB 设备
|
||||||
|
```bash
|
||||||
|
sudo usermod -aG dialout $USER
|
||||||
|
# 重新登录后生效
|
||||||
|
```
|
||||||
|
|
||||||
|
2. **udev 规则**：设置 USB 设备权限规则
|
||||||
|
```bash
|
||||||
|
sudo cp lib/transmitter_sdk/50-usb2can.rules /etc/udev/rules.d/
|
||||||
|
sudo udevadm control --reload-rules
|
||||||
|
sudo udevadm trigger
|
||||||
|
```
|
||||||
|
|
||||||
|
3. **终端电阻**：CAN 总线必须在两端各加 120Ω 终端电阻
|
||||||
|
|
||||||
|
4. **波特率**：默认使用 1000000 bps (1Mbps)，确保所有设备波特率一致
|
||||||
|
|
||||||
|
## 常见问题
|
||||||
|
|
||||||
|
### Q: 节点启动时提示 "未找到 USB2CAN 设备"
|
||||||
|
A: 检查设备是否连接，使用 `lsusb` 查看设备是否存在
|
||||||
|
|
||||||
|
### Q: 运行时报错 "cannot open shared object file"
|
||||||
|
A: 设置库文件路径：
|
||||||
|
```bash
|
||||||
|
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$(pwd)/lib/transmitter_sdk/lib/linux/x64
|
||||||
|
```
|
||||||
|
|
||||||
|
### Q: 接收不到数据
|
||||||
|
A:
|
||||||
|
1. 确认 CAN 线正确连接（H-H, L-L）
|
||||||
|
2. 检查是否有 120Ω 终端电阻
|
||||||
|
3. 确认波特率设置正确
|
||||||
|
|
||||||
|
### Q: 发送回调突然停止
|
||||||
|
A: 这是 CAN 协议的 ACK 机制。如果没有设备响应 ACK，发送会停止。确保：
|
||||||
|
1. 总线上至少有一个设备能响应 ACK
|
||||||
|
2. 或者进行环路测试（自己发自己收）
|
||||||
27
docs/Transmit/ctrl_command.md
Normal file
27
docs/Transmit/ctrl_command.md
Normal file
@@ -0,0 +1,27 @@
|
|||||||
|
# 控制指令的发送说明
|
||||||
|
|
||||||
|
在主机外接收的控制板中的控制代码的实现是：
|
||||||
|
|
||||||
|
```c
|
||||||
|
// CAN 速率 1000kbps
|
||||||
|
void CToC_CANDataProcess(unit32_t ID, uint8_t *Data)
|
||||||
|
{
|
||||||
|
// 以下所有变量有效范围为 [-660,660]
|
||||||
|
if(ID == 0x149) // 运动控制
|
||||||
|
{
|
||||||
|
|
||||||
|
Remote_RxData.Remote_R_RL =(int16_t)((uint16_t)Data[0]<<8 | Data[1]); // 平动 X 轴（左右）
|
||||||
|
Remote_RxData.Remote_R_UD =(int16_t)((uint16_t)Data[2]<<8 | Data[3]); // 平动 Y 轴（前后）
|
||||||
|
Remote_RxData.Remote_L_RL =(int16_t)((uint16_t)Data[4]<<8 | Data[5]); // 云台偏航 （左右）
|
||||||
|
Remote_RxData.Remote_L_UD =(int16_t)((uint16_t)Data[6]<<8 | Data[7]); // 云台俯仰 （上下）
|
||||||
|
}
|
||||||
|
else if(ID == 0x189) // 攻击控制
|
||||||
|
{
|
||||||
|
Remote_RxData.Remote_ThumbWheel =(int16_t)((uint16_t)Data[0]<<8 | Data[1]); // 拨弹盘（进弹/退弹），660 为进弹，0 为关闭，-660 为退弹
|
||||||
|
Remote_RxData.Remote_RS =(int16_t)((uint16_t)Data[2]<<8 | Data[3]); // 小陀螺（反自瞄）开关，置 660 为开启，0 为关闭
|
||||||
|
Remote_RxData.Remote_LS =(int16_t)((uint16_t)Data[4]<<8 | Data[5]); // 摩擦轮开关， 置 660 为开启，0 为关闭
|
||||||
|
}
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
两个 ID 分别是云台和底盘的控制板，在处于进点/回家状态时不应开启小陀螺，发射弹丸需要**打开摩擦轮 同时 进弹**，因此只有在处于攻击状态时开启摩擦轮，此时可以将进弹视为扳机。
|
||||||
@@ -1,37 +0,0 @@
|
|||||||
# SRCP 通信协议帧结构
|
|
||||||
|
|
||||||
## 帧格式
|
|
||||||
|
|
||||||
| 字段 | 内容 | 长度 | 说明 |
|
|
||||||
|------|------|------|------|
|
|
||||||
| SOF | 0xBB 0x77 | 2 bytes | 帧头 |
|
|
||||||
| x_move | -660~660 | 2 bytes | 平动左右 (小端序) |
|
|
||||||
| y_move | -660~660 | 2 bytes | 平动前后 (小端序) |
|
|
||||||
| yaw | -660~660 | 2 bytes | 云台偏航 (小端序) |
|
|
||||||
| pitch | -660~660 | 2 bytes | 云台俯仰 (小端序) |
|
|
||||||
| feed | -660~660 | 2 bytes | 拨弹轮 (小端序) |
|
|
||||||
| key | 0~15 | 1 byte | 按键 |
|
|
||||||
| crc8 | CRC8-MAXIM | 1 byte | 校验 |
|
|
||||||
| EOF | 0xEE | 1 byte | 帧尾 |
|
|
||||||
|
|
||||||
## 帧长度
|
|
||||||
|
|
||||||
```
|
|
||||||
SOF(2) + x_move(2) + y_move(2) + yaw(2) + pitch(2) + feed(2) + key(1) + crc8(1) + EOF(1) = 15 bytes
|
|
||||||
```
|
|
||||||
|
|
||||||
## 字节序
|
|
||||||
|
|
||||||
- 所有 int16_t 类型字段（x_move, y_move, yaw, pitch, feed）使用 **小端序**
|
|
||||||
|
|
||||||
## 代码示例
|
|
||||||
|
|
||||||
```cpp
|
|
||||||
constexpr int FRAME_LENGTH = 15; // 总帧长度
|
|
||||||
|
|
||||||
frame[0] = 0xBB; // SOF
|
|
||||||
frame[1] = 0x77; // SOF
|
|
||||||
frame[2] = x_move & 0xFF; // 低字节
|
|
||||||
frame[3] = (x_move >> 8) & 0xFF; // 高字节
|
|
||||||
// ... 其他字段
|
|
||||||
frame[14] = 0xEE; // EOF
|
|
||||||
@@ -1,87 +0,0 @@
|
|||||||
# USB 串口设备权限设置指南
|
|
||||||
|
|
||||||
## 文件说明
|
|
||||||
|
|
||||||
| 文件 | 说明 |
|
|
||||||
|------|------|
|
|
||||||
| `99-usb-serial.rules` | udev 规则文件，固定设备名称和权限 |
|
|
||||||
| `setup_usb_permissions.sh` | 自动化设置脚本 |
|
|
||||||
|
|
||||||
## 操作步骤
|
|
||||||
|
|
||||||
### 方法一：使用自动化脚本（推荐）
|
|
||||||
|
|
||||||
```bash
|
|
||||||
cd ~/code/amadeus_26
|
|
||||||
./setup_usb_permissions.sh
|
|
||||||
```
|
|
||||||
|
|
||||||
### 方法二：手动设置
|
|
||||||
|
|
||||||
1. **识别设备 VID/PID**
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 查看 CH340 设备信息
|
|
||||||
udevadm info -a -n /dev/ttyUSB0 | grep -E "idVendor|idProduct"
|
|
||||||
|
|
||||||
# 查看 IMU 设备信息
|
|
||||||
udevadm info -a -n /dev/ttyUSB1 | grep -E "idVendor|idProduct"
|
|
||||||
```
|
|
||||||
|
|
||||||
2. **安装 udev 规则**
|
|
||||||
|
|
||||||
```bash
|
|
||||||
sudo cp 99-usb-serial.rules /etc/udev/rules.d/
|
|
||||||
sudo udevadm control --reload-rules
|
|
||||||
sudo udevadm trigger
|
|
||||||
```
|
|
||||||
|
|
||||||
3. **添加用户权限**
|
|
||||||
|
|
||||||
```bash
|
|
||||||
sudo usermod -aG dialout $USER
|
|
||||||
```
|
|
||||||
|
|
||||||
4. **生效**
|
|
||||||
|
|
||||||
- 重新插拔 USB 设备，或重启系统
|
|
||||||
- 注销并重新登录，使权限生效
|
|
||||||
|
|
||||||
## 设备映射
|
|
||||||
|
|
||||||
设置完成后，设备将以固定名称出现：
|
|
||||||
|
|
||||||
| 设备 | 固定名称 | 原名称 |
|
|
||||||
|------|----------|--------|
|
|
||||||
| CH340 发送模块 | `/dev/ttyCH340` | `/dev/ttyUSB0` |
|
|
||||||
| IMU 模块 | `/dev/ttyIMU` | `/dev/ttyUSB1` |
|
|
||||||
|
|
||||||
## 验证
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 检查设备链接
|
|
||||||
ls -la /dev/ttyCH340 /dev/ttyIMU
|
|
||||||
|
|
||||||
# 检查权限
|
|
||||||
ls -la /dev/ttyUSB*
|
|
||||||
```
|
|
||||||
|
|
||||||
## 使用
|
|
||||||
|
|
||||||
修改 launch 文件中的默认设备路径：
|
|
||||||
|
|
||||||
```python
|
|
||||||
# launch/uart_transmitter.launch.py
|
|
||||||
serial_port_arg = DeclareLaunchArgument(
|
|
||||||
'serial_port',
|
|
||||||
default_value='/dev/ttyCH340', # 使用固定名称
|
|
||||||
description='CH340 串口设备路径'
|
|
||||||
)
|
|
||||||
```
|
|
||||||
|
|
||||||
运行节点：
|
|
||||||
|
|
||||||
```bash
|
|
||||||
ros2 launch amadeus_26 uart_transmitter.launch.py
|
|
||||||
# 或指定 IMU 设备
|
|
||||||
ros2 launch amadeus_26 uart_transmitter.launch.py serial_port:=/dev/ttyIMU
|
|
||||||
112
launch/transmitter_test.launch.py
Normal file
112
launch/transmitter_test.launch.py
Normal file
@@ -0,0 +1,112 @@
|
|||||||
|
from launch import LaunchDescription
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
from launch.actions import DeclareLaunchArgument
|
||||||
|
from launch.substitutions import LaunchConfiguration
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
# 声明启动参数 - 基本设置
|
||||||
|
send_frequency_arg = DeclareLaunchArgument(
|
||||||
|
'send_frequency',
|
||||||
|
default_value='50.0',
|
||||||
|
description='CAN 发送频率 (Hz)'
|
||||||
|
)
|
||||||
|
|
||||||
|
tx_channel_arg = DeclareLaunchArgument(
|
||||||
|
'tx_channel',
|
||||||
|
default_value='0',
|
||||||
|
description='CAN 发送通道 (0 或 1)'
|
||||||
|
)
|
||||||
|
|
||||||
|
rx_channel_arg = DeclareLaunchArgument(
|
||||||
|
'rx_channel',
|
||||||
|
default_value='1',
|
||||||
|
description='CAN 接收通道 (0 或 1)'
|
||||||
|
)
|
||||||
|
|
||||||
|
# 声明启动参数 - CAN ID 设置
|
||||||
|
can_id_motion_arg = DeclareLaunchArgument(
|
||||||
|
'can_id_motion',
|
||||||
|
default_value='329', # 0x149 = 329
|
||||||
|
description='运动控制 CAN ID (十进制，默认 0x149=329)'
|
||||||
|
)
|
||||||
|
|
||||||
|
can_id_attack_arg = DeclareLaunchArgument(
|
||||||
|
'can_id_attack',
|
||||||
|
default_value='393', # 0x189 = 393
|
||||||
|
description='攻击控制 CAN ID (十进制，默认 0x189=393)'
|
||||||
|
)
|
||||||
|
|
||||||
|
# 声明启动参数 - CAN 帧格式
|
||||||
|
use_extended_frame_arg = DeclareLaunchArgument(
|
||||||
|
'use_extended_frame',
|
||||||
|
default_value='false',
|
||||||
|
description='是否使用扩展帧(29位ID)，false=标准帧(11位)'
|
||||||
|
)
|
||||||
|
|
||||||
|
use_canfd_arg = DeclareLaunchArgument(
|
||||||
|
'use_canfd',
|
||||||
|
default_value='false',
|
||||||
|
description='是否使用 CANFD，false=CAN2.0'
|
||||||
|
)
|
||||||
|
|
||||||
|
use_brs_arg = DeclareLaunchArgument(
|
||||||
|
'use_brs',
|
||||||
|
default_value='false',
|
||||||
|
description='CANFD 是否启用波特率切换(BRS)'
|
||||||
|
)
|
||||||
|
|
||||||
|
# 声明启动参数 - 接收过滤
|
||||||
|
rx_filter_enabled_arg = DeclareLaunchArgument(
|
||||||
|
'rx_filter_enabled',
|
||||||
|
default_value='false',
|
||||||
|
description='是否启用接收 ID 过滤'
|
||||||
|
)
|
||||||
|
|
||||||
|
rx_filter_id_min_arg = DeclareLaunchArgument(
|
||||||
|
'rx_filter_id_min',
|
||||||
|
default_value='0',
|
||||||
|
description='接收过滤 ID 最小值'
|
||||||
|
)
|
||||||
|
|
||||||
|
rx_filter_id_max_arg = DeclareLaunchArgument(
|
||||||
|
'rx_filter_id_max',
|
||||||
|
default_value='2047', # 0x7FF
|
||||||
|
description='接收过滤 ID 最大值'
|
||||||
|
)
|
||||||
|
|
||||||
|
# 创建节点
|
||||||
|
transmitter_test_node = Node(
|
||||||
|
package='amadeus_26',
|
||||||
|
executable='transmitter_test_node',
|
||||||
|
name='transmitter_test_node',
|
||||||
|
output='screen',
|
||||||
|
parameters=[{
|
||||||
|
'send_frequency': LaunchConfiguration('send_frequency'),
|
||||||
|
'tx_channel': LaunchConfiguration('tx_channel'),
|
||||||
|
'rx_channel': LaunchConfiguration('rx_channel'),
|
||||||
|
'can_id_motion': LaunchConfiguration('can_id_motion'),
|
||||||
|
'can_id_attack': LaunchConfiguration('can_id_attack'),
|
||||||
|
'use_extended_frame': LaunchConfiguration('use_extended_frame'),
|
||||||
|
'use_canfd': LaunchConfiguration('use_canfd'),
|
||||||
|
'use_brs': LaunchConfiguration('use_brs'),
|
||||||
|
'rx_filter_enabled': LaunchConfiguration('rx_filter_enabled'),
|
||||||
|
'rx_filter_id_min': LaunchConfiguration('rx_filter_id_min'),
|
||||||
|
'rx_filter_id_max': LaunchConfiguration('rx_filter_id_max'),
|
||||||
|
}],
|
||||||
|
)
|
||||||
|
|
||||||
|
return LaunchDescription([
|
||||||
|
send_frequency_arg,
|
||||||
|
tx_channel_arg,
|
||||||
|
rx_channel_arg,
|
||||||
|
can_id_motion_arg,
|
||||||
|
can_id_attack_arg,
|
||||||
|
use_extended_frame_arg,
|
||||||
|
use_canfd_arg,
|
||||||
|
use_brs_arg,
|
||||||
|
rx_filter_enabled_arg,
|
||||||
|
rx_filter_id_min_arg,
|
||||||
|
rx_filter_id_max_arg,
|
||||||
|
transmitter_test_node,
|
||||||
|
])
|
||||||
14
lib/transmitter_sdk/50-usb2can.rules
Normal file
14
lib/transmitter_sdk/50-usb2can.rules
Normal file
@@ -0,0 +1,14 @@
|
|||||||
|
# 达妙 USB2CAN 设备权限规则
|
||||||
|
# 使用方法:
|
||||||
|
# 1. 复制到 /etc/udev/rules.d/: sudo cp 50-usb2can.rules /etc/udev/rules.d/
|
||||||
|
# 2. 重新加载规则: sudo udevadm control --reload-rules && sudo udevadm trigger
|
||||||
|
# 3. 重新插拔设备或重启
|
||||||
|
|
||||||
|
# 达妙 USB2CAN-DUAL (根据 lsusb 输出的 34b7:6632)
|
||||||
|
SUBSYSTEM=="usb", ATTR{idVendor}=="34b7", ATTR{idProduct}=="6632", MODE="0666", GROUP="dialout"
|
||||||
|
|
||||||
|
# 备用常见 VID:PID (STM32 虚拟串口等)
|
||||||
|
SUBSYSTEM=="usb", ATTR{idVendor}=="0483", ATTR{idProduct}=="5740", MODE="0666", GROUP="dialout"
|
||||||
|
|
||||||
|
# ACM 设备 (ttyACM*)
|
||||||
|
KERNEL=="ttyACM[0-9]*", MODE="0666", GROUP="dialout"
|
||||||
0
lib/transmitter_sdk/example.c
Normal file
0
lib/transmitter_sdk/example.c
Normal file
149
lib/transmitter_sdk/inc/pub_user.h
Executable file
149
lib/transmitter_sdk/inc/pub_user.h
Executable file
@@ -0,0 +1,149 @@
|
|||||||
|
//
|
||||||
|
// Created by 93094 on 2025/12/23.
|
||||||
|
//
|
||||||
|
|
||||||
|
#ifndef DM_DEVICE_PUB_USER_H
|
||||||
|
#define DM_DEVICE_PUB_USER_H
|
||||||
|
|
||||||
|
#define DEVICE_EXPORTS
|
||||||
|
|
||||||
|
#ifdef _WIN32
|
||||||
|
#ifdef DEVICE_EXPORTS
|
||||||
|
#define DEVICE_API __declspec(dllexport)
|
||||||
|
#else
|
||||||
|
#define DEVICE_API __declspec(dllimport)
|
||||||
|
#endif
|
||||||
|
#else
|
||||||
|
#define DEVICE_API __attribute__((visibility("default")))
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
extern "C"
|
||||||
|
{
|
||||||
|
typedef struct damiao_handle hDamiaoUsb;
|
||||||
|
typedef struct device_handle hDevice;
|
||||||
|
|
||||||
|
#pragma pack(push,1)
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
uint32_t can_id:29; //can id
|
||||||
|
uint32_t esi:1; //错误状态指示 一般不用
|
||||||
|
uint32_t ext:1; //拓展帧 1-拓展帧 0-标准帧
|
||||||
|
uint32_t rtr:1; //远程帧 1-远程帧 0-数据帧
|
||||||
|
uint8_t canfd:1; //canfd 1-canfd帧 0-can2.0帧
|
||||||
|
uint8_t brs:1; //波特率切换 1-切换 0-不切换
|
||||||
|
uint8_t id_inc:1; //ID自增 1-自增 0-不自增
|
||||||
|
uint8_t data_inc:1; //数据自增 1-自增 0-不自增
|
||||||
|
uint8_t dlc:4; //数据长度
|
||||||
|
uint8_t channel; //通道号
|
||||||
|
uint16_t reserved; //预留字节
|
||||||
|
uint16_t step_id; //步进ID
|
||||||
|
uint32_t stop_id; //停止ID
|
||||||
|
uint32_t interval; //发送间隔
|
||||||
|
int32_t send_times; //发送次数
|
||||||
|
|
||||||
|
}usb_tx_frame_head_t;
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
usb_tx_frame_head_t head;
|
||||||
|
uint8_t payload[64];
|
||||||
|
|
||||||
|
}usb_tx_frame_t;
|
||||||
|
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
uint32_t can_id:29; //can id
|
||||||
|
uint32_t esi:1; //错误状态指示 一般不用
|
||||||
|
uint32_t ext:1; //类型：标准/拓展
|
||||||
|
uint32_t rtr:1; //类型：数据/远程
|
||||||
|
uint64_t time_stamp; //时间戳
|
||||||
|
uint8_t channel; //发送通道
|
||||||
|
uint8_t canfd:1; //类型：2.0/fd
|
||||||
|
uint8_t dir:1; //方向：rx/tx
|
||||||
|
uint8_t brs:1; //BRS
|
||||||
|
uint8_t ack:1; //应答标志
|
||||||
|
uint8_t dlc:4; //长度
|
||||||
|
uint16_t reserved; //预留字节
|
||||||
|
}usb_rx_frame_head_t;
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
usb_rx_frame_head_t head;
|
||||||
|
uint8_t payload[64];
|
||||||
|
|
||||||
|
}usb_rx_frame_t ;
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
int can_baudrate;
|
||||||
|
int canfd_baudrate;
|
||||||
|
float can_sp;
|
||||||
|
float canfd_sp;
|
||||||
|
}device_baud_t;
|
||||||
|
|
||||||
|
typedef enum
|
||||||
|
{
|
||||||
|
DEV_None=-1,
|
||||||
|
DEV_USB2CANFD=0,
|
||||||
|
DEV_USB2CANFD_DUAL,
|
||||||
|
DEV_ECAT2CANFD
|
||||||
|
}device_def_t;
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
uint8_t channel;
|
||||||
|
uint8_t can_fd;
|
||||||
|
uint8_t can_seg1;
|
||||||
|
uint8_t can_seg2;
|
||||||
|
uint8_t can_sjw;
|
||||||
|
uint8_t can_prescaler;
|
||||||
|
uint8_t canfd_seg1;
|
||||||
|
uint8_t canfd_seg2;
|
||||||
|
uint8_t canfd_sjw;
|
||||||
|
uint8_t canfd_prescaler;
|
||||||
|
}dmcan_ch_can_config_t;
|
||||||
|
|
||||||
|
|
||||||
|
#pragma pack(pop)
|
||||||
|
|
||||||
|
|
||||||
|
typedef void (*dev_rec_callback)(usb_rx_frame_t* rec_frame);
|
||||||
|
typedef void (*dev_sent_callback)(usb_rx_frame_t* sent_frame);
|
||||||
|
typedef void (*dev_err_callback)(usb_rx_frame_t* err_frame);
|
||||||
|
|
||||||
|
|
||||||
|
DEVICE_API damiao_handle* damiao_handle_create(device_def_t type);
|
||||||
|
DEVICE_API void damiao_handle_destroy(damiao_handle* handle);
|
||||||
|
DEVICE_API void damiao_print_version(damiao_handle* handle);
|
||||||
|
DEVICE_API void damiao_get_sdk_version(damiao_handle* handle, char* version_buf, size_t buf_size);
|
||||||
|
DEVICE_API int damiao_handle_find_devices(damiao_handle* handle);
|
||||||
|
DEVICE_API void damiao_handle_get_devices(damiao_handle* handle,device_handle** dev_list, int* device_count);
|
||||||
|
DEVICE_API void device_get_version(device_handle* dev, char* version_buf, size_t buf_size);
|
||||||
|
DEVICE_API void device_get_pid_vid(device_handle* dev, int* pid, int* vid);
|
||||||
|
DEVICE_API void device_get_serial_number(device_handle* dev, char* serial_buf, size_t buf_size);
|
||||||
|
DEVICE_API void device_get_type(device_handle* dev, device_def_t* type);
|
||||||
|
DEVICE_API bool device_open(device_handle* dev);
|
||||||
|
DEVICE_API bool device_close(device_handle* dev);
|
||||||
|
DEVICE_API bool device_save_config(device_handle* dev);
|
||||||
|
DEVICE_API bool device_open_channel(device_handle* dev, uint8_t channel);
|
||||||
|
DEVICE_API bool device_close_channel(device_handle* dev, uint8_t channel);
|
||||||
|
DEVICE_API bool device_channel_get_baudrate(device_handle*dev ,uint8_t channel,device_baud_t* baud);
|
||||||
|
DEVICE_API bool device_channel_set_baud(device_handle*dev ,uint8_t channel,bool canfd,int bitrate,int dbitrate);
|
||||||
|
DEVICE_API bool device_channel_set_baud_with_sp(device_handle*dev ,uint8_t channel,bool canfd,int bitrate,int dbitrate,float can_sp,float canfd_sp);
|
||||||
|
DEVICE_API void device_hook_to_rec(device_handle*dev,dev_rec_callback callback);
|
||||||
|
DEVICE_API void device_hook_to_sent(device_handle*dev,dev_sent_callback callback);
|
||||||
|
DEVICE_API void device_hook_to_err(device_handle*dev,dev_err_callback callback);
|
||||||
|
DEVICE_API void device_channel_send(device_handle*dev,usb_tx_frame_t frame);
|
||||||
|
DEVICE_API void device_channel_send_fast(device_handle*dev,uint8_t ch,uint32_t can_id,int32_t cnt,bool ext,bool canfd,bool brs,uint8_t len,uint8_t* payload);
|
||||||
|
DEVICE_API void device_channel_send_advanced(device_handle*dev,uint8_t ch,uint32_t can_id,uint16_t step_id,uint32_t stop_id,int32_t cnt,bool id_inc,bool data_inc,bool ext,bool canfd,bool brs,uint8_t len,uint8_t* payload);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif //DM_DEVICE_PUB_USER_H
|
||||||
@@ -3,16 +3,17 @@
|
|||||||
<package format="3">
|
<package format="3">
|
||||||
<name>amadeus_26</name>
|
<name>amadeus_26</name>
|
||||||
<version>0.0.1</version>
|
<version>0.0.1</version>
|
||||||
<description>Amadeus 26 transmission and control package</description>
|
<description>Simba Robotics 2026 哨兵机器人识别、导航、决策算法</description>
|
||||||
<maintainer email="maintainer@example.com">maintainer</maintainer>
|
<maintainer email="your-email@example.com">maintainer</maintainer>
|
||||||
<license>MIT</license>
|
<license>MIT</license>
|
||||||
|
|
||||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
<depend>rclcpp</depend>
|
<depend>rclcpp</depend>
|
||||||
<depend>std_msgs</depend>
|
<depend>std_msgs</depend>
|
||||||
<depend>geometry_msgs</depend>
|
|
||||||
<depend>sensor_msgs</depend>
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
||||||
<export>
|
<export>
|
||||||
<build_type>ament_cmake</build_type>
|
<build_type>ament_cmake</build_type>
|
||||||
|
|||||||
@@ -1,971 +0,0 @@
|
|||||||
/**
|
|
||||||
* @file imu_receiver_node.cpp
|
|
||||||
* @brief IMU 数据接收节点 (FDI DETA10) - 环形缓冲区版本 + UKF
|
|
||||||
*
|
|
||||||
* 通过串口读取 FDI DETA10 IMU 模块的数据
|
|
||||||
* 波特率：921600
|
|
||||||
* 协议：FDILink
|
|
||||||
*
|
|
||||||
* 转发数据包：
|
|
||||||
* - 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>
|
|
||||||
#include <std_msgs/msg/float32.hpp>
|
|
||||||
#include <std_msgs/msg/float64.hpp>
|
|
||||||
#include <termios.h>
|
|
||||||
#include <thread>
|
|
||||||
#include <unistd.h>
|
|
||||||
#include <vector>
|
|
||||||
|
|
||||||
// FDI Link 帧定义
|
|
||||||
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 const char *DEFAULT_SERIAL_PORT = "/dev/ttyIMU";
|
|
||||||
constexpr int DEFAULT_BAUDRATE = 921600;
|
|
||||||
|
|
||||||
// 串口读取缓冲区大小 (921600bps ≈ 115KB/s, 缓冲区需要足够大)
|
|
||||||
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,
|
|
||||||
65, 157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96,
|
|
||||||
130, 220, 35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128,
|
|
||||||
222, 60, 98, 190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158,
|
|
||||||
29, 67, 161, 255, 70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56,
|
|
||||||
102, 229, 187, 89, 7, 219, 133, 103, 57, 186, 228, 6, 88, 25, 71,
|
|
||||||
165, 251, 120, 38, 196, 154, 101, 59, 217, 135, 4, 90, 184, 230, 167,
|
|
||||||
249, 27, 69, 198, 152, 122, 36, 248, 166, 68, 26, 153, 199, 37, 123,
|
|
||||||
58, 100, 134, 216, 91, 5, 231, 185, 140, 210, 48, 110, 237, 179, 81,
|
|
||||||
15, 78, 16, 242, 172, 47, 113, 147, 205, 17, 79, 173, 243, 112, 46,
|
|
||||||
204, 146, 211, 141, 111, 49, 178, 236, 14, 80, 175, 241, 19, 77, 206,
|
|
||||||
144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238, 50, 108, 142, 208,
|
|
||||||
83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115, 202, 148, 118,
|
|
||||||
40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139, 87, 9,
|
|
||||||
235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22, 233,
|
|
||||||
183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,
|
|
||||||
116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107,
|
|
||||||
53};
|
|
||||||
|
|
||||||
// CRC16 查找表
|
|
||||||
static const uint16_t CRC16Table[256] = {
|
|
||||||
0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, 0x8108,
|
|
||||||
0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, 0x1231, 0x0210,
|
|
||||||
0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, 0x9339, 0x8318, 0xB37B,
|
|
||||||
0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE, 0x2462, 0x3443, 0x0420, 0x1401,
|
|
||||||
0x64E6, 0x74C7, 0x44A4, 0x5485, 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE,
|
|
||||||
0xF5CF, 0xC5AC, 0xD58D, 0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6,
|
|
||||||
0x5695, 0x46B4, 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D,
|
|
||||||
0xC7BC, 0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
|
|
||||||
0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B, 0x5AF5,
|
|
||||||
0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, 0xDBFD, 0xCBDC,
|
|
||||||
0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A, 0x6CA6, 0x7C87, 0x4CE4,
|
|
||||||
0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD,
|
|
||||||
0xAD2A, 0xBD0B, 0x8D68, 0x9D49, 0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13,
|
|
||||||
0x2E32, 0x1E51, 0x0E70, 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A,
|
|
||||||
0x9F59, 0x8F78, 0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E,
|
|
||||||
0xE16F, 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
|
|
||||||
0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, 0x02B1,
|
|
||||||
0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256, 0xB5EA, 0xA5CB,
|
|
||||||
0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, 0x34E2, 0x24C3, 0x14A0,
|
|
||||||
0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 0xA7DB, 0xB7FA, 0x8799, 0x97B8,
|
|
||||||
0xE75F, 0xF77E, 0xC71D, 0xD73C, 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657,
|
|
||||||
0x7676, 0x4615, 0x5634, 0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9,
|
|
||||||
0xB98A, 0xA9AB, 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882,
|
|
||||||
0x28A3, 0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
|
|
||||||
0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92, 0xFD2E,
|
|
||||||
0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, 0x7C26, 0x6C07,
|
|
||||||
0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1, 0xEF1F, 0xFF3E, 0xCF5D,
|
|
||||||
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"), 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();
|
|
||||||
|
|
||||||
// 获取 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(),
|
|
||||||
"UKF 参数 - 过程噪声: %.4f, 测量噪声: %.4f, dt: %.4f",
|
|
||||||
process_noise, measurement_noise, dt);
|
|
||||||
RCLCPP_INFO(this->get_logger(), "---------------------------------");
|
|
||||||
|
|
||||||
// 创建发布者
|
|
||||||
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);
|
|
||||||
connection_status_pub_ = this->create_publisher<std_msgs::msg::Bool>(
|
|
||||||
"imu/connection_status", 10);
|
|
||||||
|
|
||||||
// 初始化串口
|
|
||||||
if (!initSerial()) {
|
|
||||||
RCLCPP_ERROR(this->get_logger(), "串口初始化失败，节点退出");
|
|
||||||
rclcpp::shutdown();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 创建接收线程
|
|
||||||
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() {
|
|
||||||
running_ = false;
|
|
||||||
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_);
|
|
||||||
}
|
|
||||||
RCLCPP_INFO(this->get_logger(), "IMU 串口已关闭");
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
bool initSerial() {
|
|
||||||
RCLCPP_INFO(this->get_logger(), "正在打开 IMU 串口 %s...",
|
|
||||||
serial_port_.c_str());
|
|
||||||
|
|
||||||
serial_fd_ = open(serial_port_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
|
|
||||||
if (serial_fd_ < 0) {
|
|
||||||
RCLCPP_ERROR(this->get_logger(), "无法打开串口 %s: %s",
|
|
||||||
serial_port_.c_str(), strerror(errno));
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
struct termios tty;
|
|
||||||
memset(&tty, 0, sizeof(tty));
|
|
||||||
|
|
||||||
if (tcgetattr(serial_fd_, &tty) != 0) {
|
|
||||||
RCLCPP_ERROR(this->get_logger(), "tcgetattr 错误: %s", strerror(errno));
|
|
||||||
close(serial_fd_);
|
|
||||||
serial_fd_ = -1;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
speed_t baud = convertBaudrate(baudrate_);
|
|
||||||
cfsetospeed(&tty, baud);
|
|
||||||
cfsetispeed(&tty, baud);
|
|
||||||
|
|
||||||
tty.c_cflag &= ~PARENB;
|
|
||||||
tty.c_cflag &= ~CSTOPB;
|
|
||||||
tty.c_cflag &= ~CSIZE;
|
|
||||||
tty.c_cflag |= CS8;
|
|
||||||
tty.c_cflag |= CREAD | CLOCAL;
|
|
||||||
tty.c_cflag &= ~CRTSCTS;
|
|
||||||
|
|
||||||
tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
|
|
||||||
tty.c_iflag &= ~(IXON | IXOFF | IXANY);
|
|
||||||
tty.c_oflag &= ~OPOST;
|
|
||||||
|
|
||||||
// 非阻塞读取，最小读取0字节，超时0
|
|
||||||
tty.c_cc[VMIN] = 0;
|
|
||||||
tty.c_cc[VTIME] = 0;
|
|
||||||
|
|
||||||
if (tcsetattr(serial_fd_, TCSANOW, &tty) != 0) {
|
|
||||||
RCLCPP_ERROR(this->get_logger(), "tcsetattr 错误: %s", strerror(errno));
|
|
||||||
close(serial_fd_);
|
|
||||||
serial_fd_ = -1;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
tcflush(serial_fd_, TCIOFLUSH);
|
|
||||||
|
|
||||||
RCLCPP_INFO(this->get_logger(), "IMU 串口打开成功");
|
|
||||||
is_connected_ = true;
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
speed_t convertBaudrate(int baudrate) {
|
|
||||||
switch (baudrate) {
|
|
||||||
case 9600:
|
|
||||||
return B9600;
|
|
||||||
case 19200:
|
|
||||||
return B19200;
|
|
||||||
case 38400:
|
|
||||||
return B38400;
|
|
||||||
case 57600:
|
|
||||||
return B57600;
|
|
||||||
case 115200:
|
|
||||||
return B115200;
|
|
||||||
case 230400:
|
|
||||||
return B230400;
|
|
||||||
case 460800:
|
|
||||||
return B460800;
|
|
||||||
case 921600:
|
|
||||||
return B921600;
|
|
||||||
default:
|
|
||||||
return B921600;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
uint8_t calculateCRC8(const uint8_t *data, uint8_t len) {
|
|
||||||
uint8_t crc8 = 0;
|
|
||||||
for (uint8_t i = 0; i < len; i++) {
|
|
||||||
crc8 = CRC8Table[crc8 ^ data[i]];
|
|
||||||
}
|
|
||||||
return crc8;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint16_t calculateCRC16(const uint8_t *data, uint8_t len) {
|
|
||||||
uint16_t crc16 = 0;
|
|
||||||
for (uint8_t i = 0; i < len; i++) {
|
|
||||||
crc16 = CRC16Table[((crc16 >> 8) ^ data[i]) & 0xFF] ^ (crc16 << 8);
|
|
||||||
}
|
|
||||||
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];
|
|
||||||
|
|
||||||
bool header_valid = true;
|
|
||||||
if (enable_crc_) {
|
|
||||||
uint8_t header[4] = {FRAME_START, msg_id, data_len, seq};
|
|
||||||
header_valid = (calculateCRC8(header, 4) == header_crc);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (header_valid) {
|
|
||||||
in_frame = true;
|
|
||||||
current_frame.clear();
|
|
||||||
current_frame.push_back(FRAME_START);
|
|
||||||
current_frame.push_back(msg_id);
|
|
||||||
current_frame.push_back(data_len);
|
|
||||||
current_frame.push_back(seq);
|
|
||||||
current_frame.push_back(header_crc);
|
|
||||||
|
|
||||||
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) {
|
|
||||||
// 解析帧
|
|
||||||
parseFrame(current_frame);
|
|
||||||
}
|
|
||||||
in_frame = false;
|
|
||||||
i += total_frame_len;
|
|
||||||
continue;
|
|
||||||
} else {
|
|
||||||
rx_buffer.erase(rx_buffer.begin(), rx_buffer.begin() + i);
|
|
||||||
i = 5;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
i++;
|
|
||||||
} else {
|
|
||||||
current_frame.push_back(byte);
|
|
||||||
if (byte == FRAME_END) {
|
|
||||||
parseFrame(current_frame);
|
|
||||||
in_frame = false;
|
|
||||||
i++;
|
|
||||||
} else if (current_frame.size() >= MAX_FRAME_SIZE) {
|
|
||||||
in_frame = false;
|
|
||||||
i++;
|
|
||||||
} else {
|
|
||||||
i++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
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));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 解析帧 - 处理 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;
|
|
||||||
|
|
||||||
if (enable_crc_) {
|
|
||||||
uint16_t data_crc = (frame[5] << 8) | frame[6];
|
|
||||||
uint16_t calc_crc = calculateCRC16(frame.data() + 7, data_len);
|
|
||||||
if (data_crc != calc_crc)
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
const uint8_t *payload = frame.data() + 7;
|
|
||||||
|
|
||||||
// 处理 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 数据
|
|
||||||
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));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 处理循环 - 固定频率从环形缓冲区读取并发布
|
|
||||||
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 = data.timestamp;
|
|
||||||
imu_msg.header.frame_id = "imu_link";
|
|
||||||
|
|
||||||
// 线性加速度 - 使用 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;
|
|
||||||
|
|
||||||
// 角速度 - 使用 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;
|
|
||||||
|
|
||||||
// 姿态 - 使用 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(),
|
|
||||||
"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() {
|
|
||||||
auto msg = std_msgs::msg::Bool();
|
|
||||||
msg.data = is_connected_ && (serial_fd_ >= 0);
|
|
||||||
connection_status_pub_->publish(msg);
|
|
||||||
|
|
||||||
if (!is_connected_) {
|
|
||||||
RCLCPP_WARN(this->get_logger(), "IMU 接收不正常");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 成员变量
|
|
||||||
std::string serial_port_;
|
|
||||||
int baudrate_;
|
|
||||||
bool enable_crc_ = false; // CRC校验开关
|
|
||||||
|
|
||||||
int serial_fd_ = -1;
|
|
||||||
bool is_connected_ = false;
|
|
||||||
std::atomic<bool> running_{true};
|
|
||||||
std::thread receive_thread_;
|
|
||||||
std::thread fusion_thread_;
|
|
||||||
|
|
||||||
// 线程
|
|
||||||
std::thread process_thread_;
|
|
||||||
RingBuffer<AccelData> accel_ring_buffer_;
|
|
||||||
RingBuffer<AhrsData> ahrs_ring_buffer_;
|
|
||||||
RingBuffer<ImuDataFrame> output_ring_buffer_;
|
|
||||||
|
|
||||||
// UKF 滤波器（用于加速度滤波）
|
|
||||||
UKF ukf_x_;
|
|
||||||
UKF ukf_y_;
|
|
||||||
UKF ukf_z_;
|
|
||||||
|
|
||||||
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_;
|
|
||||||
};
|
|
||||||
|
|
||||||
int main(int argc, char *argv[]) {
|
|
||||||
rclcpp::init(argc, argv);
|
|
||||||
|
|
||||||
auto node = std::make_shared<ImuReceiverNode>();
|
|
||||||
|
|
||||||
if (rclcpp::ok()) {
|
|
||||||
rclcpp::spin(node);
|
|
||||||
}
|
|
||||||
|
|
||||||
rclcpp::shutdown();
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
378
src/transmitter_test_node.cpp
Normal file
378
src/transmitter_test_node.cpp
Normal file
@@ -0,0 +1,378 @@
|
|||||||
|
/**
|
||||||
|
* @file transmitter_test_node.cpp
|
||||||
|
* @brief 收发模块测试节点 - 测试 USB2CAN 收发功能
|
||||||
|
*
|
||||||
|
* 完全按照官方示例编写，SDK 自动查找 USB 设备
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <rclcpp/rclcpp.hpp>
|
||||||
|
#include <std_msgs/msg/bool.hpp>
|
||||||
|
#include <thread>
|
||||||
|
#include <cstring>
|
||||||
|
|
||||||
|
// 包含达妙 SDK 头文件
|
||||||
|
extern "C" {
|
||||||
|
#include "lib/transmitter_sdk/inc/pub_user.h"
|
||||||
|
}
|
||||||
|
|
||||||
|
// 默认 CAN ID 定义
|
||||||
|
constexpr uint32_t DEFAULT_CAN_ID_MOTION = 0x149; // 运动控制
|
||||||
|
constexpr uint32_t DEFAULT_CAN_ID_ATTACK = 0x189; // 攻击控制
|
||||||
|
constexpr int CAN_BAUDRATE = 1000000; // 1Mbps
|
||||||
|
|
||||||
|
class TransmitterTestNode : public rclcpp::Node
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
TransmitterTestNode() : Node("transmitter_test_node")
|
||||||
|
{
|
||||||
|
// 声明参数 - 通道设置
|
||||||
|
this->declare_parameter("send_frequency", 50.0); // Hz
|
||||||
|
this->declare_parameter("tx_channel", 0); // 发送通道
|
||||||
|
this->declare_parameter("rx_channel", 1); // 接收通道
|
||||||
|
|
||||||
|
// 声明参数 - CAN ID 设置 (发送)
|
||||||
|
this->declare_parameter("can_id_motion", 0x149); // 运动控制 ID (默认 0x149)
|
||||||
|
this->declare_parameter("can_id_attack", 0x189); // 攻击控制 ID (默认 0x189)
|
||||||
|
|
||||||
|
// 声明参数 - CAN 帧格式设置
|
||||||
|
this->declare_parameter("use_extended_frame", false); // 是否使用扩展帧 (29位ID)
|
||||||
|
this->declare_parameter("use_canfd", false); // 是否使用 CANFD
|
||||||
|
this->declare_parameter("use_brs", false); // CANFD 是否启用波特率切换
|
||||||
|
|
||||||
|
// 声明参数 - CAN ID 过滤 (接收)
|
||||||
|
this->declare_parameter("rx_filter_enabled", false); // 是否启用接收过滤
|
||||||
|
this->declare_parameter("rx_filter_id_min", 0x000); // 接收 ID 最小值
|
||||||
|
this->declare_parameter("rx_filter_id_max", 0x7FF); // 接收 ID 最大值
|
||||||
|
|
||||||
|
// 获取参数 - 通道
|
||||||
|
send_frequency_ = this->get_parameter("send_frequency").as_double();
|
||||||
|
tx_channel_ = static_cast<uint8_t>(this->get_parameter("tx_channel").as_int());
|
||||||
|
rx_channel_ = static_cast<uint8_t>(this->get_parameter("rx_channel").as_int());
|
||||||
|
|
||||||
|
// 获取参数 - CAN ID
|
||||||
|
can_id_motion_ = static_cast<uint32_t>(this->get_parameter("can_id_motion").as_int());
|
||||||
|
can_id_attack_ = static_cast<uint32_t>(this->get_parameter("can_id_attack").as_int());
|
||||||
|
|
||||||
|
// 获取参数 - CAN 帧格式
|
||||||
|
use_extended_frame_ = this->get_parameter("use_extended_frame").as_bool();
|
||||||
|
use_canfd_ = this->get_parameter("use_canfd").as_bool();
|
||||||
|
use_brs_ = this->get_parameter("use_brs").as_bool();
|
||||||
|
|
||||||
|
// 获取参数 - 接收过滤
|
||||||
|
rx_filter_enabled_ = this->get_parameter("rx_filter_enabled").as_bool();
|
||||||
|
rx_filter_id_min_ = static_cast<uint32_t>(this->get_parameter("rx_filter_id_min").as_int());
|
||||||
|
rx_filter_id_max_ = static_cast<uint32_t>(this->get_parameter("rx_filter_id_max").as_int());
|
||||||
|
|
||||||
|
RCLCPP_INFO(this->get_logger(), "=================================");
|
||||||
|
RCLCPP_INFO(this->get_logger(), "收发模块测试节点启动");
|
||||||
|
RCLCPP_INFO(this->get_logger(), "发送频率: %.1f Hz", send_frequency_);
|
||||||
|
RCLCPP_INFO(this->get_logger(), "发送通道: %d, 接收通道: %d", tx_channel_, rx_channel_);
|
||||||
|
RCLCPP_INFO(this->get_logger(), "发送 CAN ID - 运动: 0x%03X, 攻击: 0x%03X", can_id_motion_, can_id_attack_);
|
||||||
|
RCLCPP_INFO(this->get_logger(), "帧格式: %s, %s",
|
||||||
|
use_extended_frame_ ? "扩展帧(29位)" : "标准帧(11位)",
|
||||||
|
use_canfd_ ? "CANFD" : "CAN2.0");
|
||||||
|
if (rx_filter_enabled_) {
|
||||||
|
RCLCPP_INFO(this->get_logger(), "接收过滤: 启用 [0x%03X - 0x%03X]", rx_filter_id_min_, rx_filter_id_max_);
|
||||||
|
} else {
|
||||||
|
RCLCPP_INFO(this->get_logger(), "接收过滤: 禁用 (接收所有 ID)");
|
||||||
|
}
|
||||||
|
RCLCPP_INFO(this->get_logger(), "=================================");
|
||||||
|
|
||||||
|
// 初始化设备
|
||||||
|
if (!initDevice()) {
|
||||||
|
RCLCPP_ERROR(this->get_logger(), "设备初始化失败，节点退出");
|
||||||
|
rclcpp::shutdown();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 创建发布者
|
||||||
|
connection_status_pub_ = this->create_publisher<std_msgs::msg::Bool>("transmitter/connection_status", 10);
|
||||||
|
|
||||||
|
// 创建定时器 - 发送测试数据
|
||||||
|
auto send_period = std::chrono::duration<double>(1.0 / send_frequency_);
|
||||||
|
send_timer_ = this->create_wall_timer(
|
||||||
|
std::chrono::duration_cast<std::chrono::milliseconds>(send_period),
|
||||||
|
std::bind(&TransmitterTestNode::sendTestData, this));
|
||||||
|
|
||||||
|
// 创建定时器 - 发布连接状态
|
||||||
|
status_timer_ = this->create_wall_timer(
|
||||||
|
std::chrono::seconds(1),
|
||||||
|
std::bind(&TransmitterTestNode::publishStatus, this));
|
||||||
|
|
||||||
|
RCLCPP_INFO(this->get_logger(), "测试节点初始化完成");
|
||||||
|
}
|
||||||
|
|
||||||
|
~TransmitterTestNode()
|
||||||
|
{
|
||||||
|
cleanup();
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
bool initDevice()
|
||||||
|
{
|
||||||
|
RCLCPP_INFO(this->get_logger(), "正在初始化 USB2CAN 设备...");
|
||||||
|
|
||||||
|
// 初始化模块句柄
|
||||||
|
damiao_handle_ = damiao_handle_create(DEV_USB2CANFD_DUAL);
|
||||||
|
if (!damiao_handle_) {
|
||||||
|
RCLCPP_ERROR(this->get_logger(), "创建句柄失败");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 打印 SDK 版本
|
||||||
|
damiao_print_version(damiao_handle_);
|
||||||
|
|
||||||
|
// 查找对应类型模块的设备数量
|
||||||
|
RCLCPP_INFO(this->get_logger(), "正在查找设备...");
|
||||||
|
int device_cnt = damiao_handle_find_devices(damiao_handle_);
|
||||||
|
|
||||||
|
if (device_cnt == 0) {
|
||||||
|
RCLCPP_ERROR(this->get_logger(), "未找到 USB2CAN 设备！");
|
||||||
|
damiao_handle_destroy(damiao_handle_);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
RCLCPP_INFO(this->get_logger(), "找到 %d 个设备", device_cnt);
|
||||||
|
|
||||||
|
int handle_cnt = 0;
|
||||||
|
device_handle* dev_list[16] = {nullptr};
|
||||||
|
|
||||||
|
// 获取设备信息
|
||||||
|
RCLCPP_INFO(this->get_logger(), "正在获取设备列表...");
|
||||||
|
damiao_handle_get_devices(damiao_handle_, dev_list, &handle_cnt);
|
||||||
|
|
||||||
|
if (handle_cnt == 0 || dev_list[0] == nullptr) {
|
||||||
|
RCLCPP_ERROR(this->get_logger(), "获取设备列表失败");
|
||||||
|
damiao_handle_destroy(damiao_handle_);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 使用第一个设备
|
||||||
|
device_ = dev_list[0];
|
||||||
|
|
||||||
|
// 打开设备
|
||||||
|
RCLCPP_INFO(this->get_logger(), "正在打开设备...");
|
||||||
|
if (device_open(device_)) {
|
||||||
|
RCLCPP_INFO(this->get_logger(), "设备已打开");
|
||||||
|
} else {
|
||||||
|
RCLCPP_ERROR(this->get_logger(), "打开设备失败！");
|
||||||
|
damiao_handle_destroy(damiao_handle_);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 获取模块版本信息
|
||||||
|
char strBuf[256] = {0};
|
||||||
|
device_get_version(device_, strBuf, sizeof(strBuf));
|
||||||
|
RCLCPP_INFO(this->get_logger(), "设备版本: %s", strBuf);
|
||||||
|
|
||||||
|
// 获取序列号信息
|
||||||
|
device_get_serial_number(device_, strBuf, sizeof(strBuf));
|
||||||
|
RCLCPP_INFO(this->get_logger(), "设备序列号: %s", strBuf);
|
||||||
|
|
||||||
|
// 设置发送通道波特率（简化版，自动计算采样点）
|
||||||
|
RCLCPP_INFO(this->get_logger(), "正在设置发送通道 %d 波特率 %d bps...", tx_channel_, CAN_BAUDRATE);
|
||||||
|
if (!device_channel_set_baud(device_, tx_channel_, use_canfd_, CAN_BAUDRATE, 0)) {
|
||||||
|
RCLCPP_ERROR(this->get_logger(), "设置发送通道波特率失败");
|
||||||
|
} else {
|
||||||
|
RCLCPP_INFO(this->get_logger(), "发送通道波特率设置成功");
|
||||||
|
}
|
||||||
|
|
||||||
|
// 设置接收通道波特率（简化版，自动计算采样点）
|
||||||
|
RCLCPP_INFO(this->get_logger(), "正在设置接收通道 %d 波特率 %d bps...", rx_channel_, CAN_BAUDRATE);
|
||||||
|
if (!device_channel_set_baud(device_, rx_channel_, use_canfd_, CAN_BAUDRATE, 0)) {
|
||||||
|
RCLCPP_ERROR(this->get_logger(), "设置接收通道波特率失败");
|
||||||
|
} else {
|
||||||
|
RCLCPP_INFO(this->get_logger(), "接收通道波特率设置成功");
|
||||||
|
}
|
||||||
|
|
||||||
|
// 开启发送通道
|
||||||
|
RCLCPP_INFO(this->get_logger(), "正在打开发送通道 %d...", tx_channel_);
|
||||||
|
device_open_channel(device_, tx_channel_);
|
||||||
|
|
||||||
|
// 开启接收通道
|
||||||
|
RCLCPP_INFO(this->get_logger(), "正在打开接收通道 %d...", rx_channel_);
|
||||||
|
device_open_channel(device_, rx_channel_);
|
||||||
|
|
||||||
|
// 获取并验证波特率设置（在通道打开后）
|
||||||
|
device_baud_t baud = {0};
|
||||||
|
if (device_channel_get_baudrate(device_, tx_channel_, &baud)) {
|
||||||
|
RCLCPP_INFO(this->get_logger(), "发送通道 CAN 波特率: %d bps, CANFD 波特率: %d bps",
|
||||||
|
baud.can_baudrate, baud.canfd_baudrate);
|
||||||
|
} else {
|
||||||
|
RCLCPP_WARN(this->get_logger(), "获取发送通道波特率失败");
|
||||||
|
}
|
||||||
|
|
||||||
|
// 注册回调
|
||||||
|
RCLCPP_INFO(this->get_logger(), "正在注册回调...");
|
||||||
|
device_hook_to_rec(device_, &TransmitterTestNode::receiveCallback);
|
||||||
|
device_hook_to_sent(device_, &TransmitterTestNode::sentCallback);
|
||||||
|
device_hook_to_err(device_, &TransmitterTestNode::errorCallback);
|
||||||
|
|
||||||
|
RCLCPP_INFO(this->get_logger(), "设备初始化成功");
|
||||||
|
RCLCPP_INFO(this->get_logger(), "=== 环路测试模式 ===");
|
||||||
|
RCLCPP_INFO(this->get_logger(), "请将通道%d和通道%d的CAN总线连接在一起", tx_channel_, rx_channel_);
|
||||||
|
is_connected_ = true;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void cleanup()
|
||||||
|
{
|
||||||
|
if (device_) {
|
||||||
|
// 关闭 CAN 通道
|
||||||
|
device_close_channel(device_, tx_channel_);
|
||||||
|
device_close_channel(device_, rx_channel_);
|
||||||
|
// 关闭设备
|
||||||
|
device_close(device_);
|
||||||
|
}
|
||||||
|
if (damiao_handle_) {
|
||||||
|
// 销毁模块句柄
|
||||||
|
damiao_handle_destroy(damiao_handle_);
|
||||||
|
}
|
||||||
|
RCLCPP_INFO(this->get_logger(), "设备已关闭");
|
||||||
|
}
|
||||||
|
|
||||||
|
void sendTestData()
|
||||||
|
{
|
||||||
|
static int test_counter = 0;
|
||||||
|
test_counter++;
|
||||||
|
|
||||||
|
// 发送运动控制指令 - 使用配置的运动控制 CAN ID
|
||||||
|
{
|
||||||
|
int16_t x_move = 0; // 平动 X 轴 (左右)
|
||||||
|
int16_t y_move = 0; // 平动 Y 轴 (前后)
|
||||||
|
int16_t yaw = 0; // 云台偏航
|
||||||
|
int16_t pitch = 0; // 云台俯仰
|
||||||
|
|
||||||
|
uint16_t data[8];
|
||||||
|
data[0] = (x_move >> 8) & 0xFF;
|
||||||
|
data[1] = x_move & 0xFF;
|
||||||
|
data[2] = (y_move >> 8) & 0xFF;
|
||||||
|
data[3] = y_move & 0xFF;
|
||||||
|
data[4] = (yaw >> 8) & 0xFF;
|
||||||
|
data[5] = yaw & 0xFF;
|
||||||
|
data[6] = (pitch >> 8) & 0xFF;
|
||||||
|
data[7] = pitch & 0xFF;
|
||||||
|
|
||||||
|
device_channel_send_fast(device_, tx_channel_, can_id_motion_, 1,
|
||||||
|
use_extended_frame_, use_canfd_, use_brs_, 8, data);
|
||||||
|
|
||||||
|
if (test_counter % 50 == 0) {
|
||||||
|
RCLCPP_INFO(this->get_logger(),
|
||||||
|
"通道%d 发送运动控制 [0x%03X]: X=%d, Y=%d, Yaw=%d, Pitch=%d",
|
||||||
|
tx_channel_, can_id_motion_, x_move, y_move, yaw, pitch);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 发送攻击控制指令 - 使用配置的攻击控制 CAN ID
|
||||||
|
{
|
||||||
|
int16_t feed = 0; // 拨弹轮
|
||||||
|
int16_t spin = 2; // 小陀螺开启
|
||||||
|
int16_t friction = 2; // 摩擦轮开启
|
||||||
|
|
||||||
|
uint16_t data[8];
|
||||||
|
data[0] = (feed >> 8) & 0xFF;
|
||||||
|
data[1] = feed & 0xFF;
|
||||||
|
data[2] = (spin >> 8) & 0xFF;
|
||||||
|
data[3] = spin & 0xFF;
|
||||||
|
data[4] = (friction >> 8) & 0xFF;
|
||||||
|
data[5] = friction & 0xFF;
|
||||||
|
data[6] = 0;
|
||||||
|
data[7] = 0;
|
||||||
|
|
||||||
|
device_channel_send_fast(device_, tx_channel_, can_id_attack_, 1,
|
||||||
|
use_extended_frame_, use_canfd_, use_brs_, 8, data);
|
||||||
|
|
||||||
|
if (test_counter % 50 == 0) {
|
||||||
|
RCLCPP_INFO(this->get_logger(),
|
||||||
|
"通道%d 发送攻击控制 [0x%03X]: Feed=%d, Spin=%d, Friction=%d",
|
||||||
|
tx_channel_, can_id_attack_, feed, spin, friction);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void publishStatus()
|
||||||
|
{
|
||||||
|
auto msg = std_msgs::msg::Bool();
|
||||||
|
msg.data = is_connected_;
|
||||||
|
connection_status_pub_->publish(msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 发送帧回传回调函数
|
||||||
|
static void sentCallback(usb_rx_frame_t* frame)
|
||||||
|
{
|
||||||
|
if (!frame) return;
|
||||||
|
printf("[发送回调] ID: 0x%X\n", frame->head.can_id);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 接收帧回传回调函数
|
||||||
|
static void receiveCallback(usb_rx_frame_t* frame)
|
||||||
|
{
|
||||||
|
if (!frame) return;
|
||||||
|
printf("[接收回调] 通道%d ID: 0x%X, 长度: %d, 数据: ",
|
||||||
|
frame->head.channel, frame->head.can_id, frame->head.dlc);
|
||||||
|
for (int i = 0; i < frame->head.dlc; i++) {
|
||||||
|
printf("%02X ", frame->payload[i]);
|
||||||
|
}
|
||||||
|
printf("\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
// 错误回调函数
|
||||||
|
static void errorCallback(usb_rx_frame_t* frame)
|
||||||
|
{
|
||||||
|
if (!frame) return;
|
||||||
|
printf("[错误回调] 通道%d ID: 0x%X, 错误类型: ",
|
||||||
|
frame->head.channel, frame->head.can_id);
|
||||||
|
// 根据错误码判断错误类型
|
||||||
|
if (frame->head.esi) {
|
||||||
|
printf("错误状态指示(ESI) ");
|
||||||
|
}
|
||||||
|
if (frame->head.dir) {
|
||||||
|
printf("发送方向 ");
|
||||||
|
} else {
|
||||||
|
printf("接收方向 ");
|
||||||
|
}
|
||||||
|
printf("\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
// 成员变量 - 通道和频率
|
||||||
|
double send_frequency_;
|
||||||
|
uint8_t tx_channel_;
|
||||||
|
uint8_t rx_channel_;
|
||||||
|
|
||||||
|
// 成员变量 - CAN ID
|
||||||
|
uint32_t can_id_motion_;
|
||||||
|
uint32_t can_id_attack_;
|
||||||
|
|
||||||
|
// 成员变量 - CAN 帧格式
|
||||||
|
bool use_extended_frame_;
|
||||||
|
bool use_canfd_;
|
||||||
|
bool use_brs_;
|
||||||
|
|
||||||
|
// 成员变量 - 接收过滤
|
||||||
|
bool rx_filter_enabled_;
|
||||||
|
uint32_t rx_filter_id_min_;
|
||||||
|
uint32_t rx_filter_id_max_;
|
||||||
|
|
||||||
|
// 成员变量 - 设备句柄
|
||||||
|
damiao_handle* damiao_handle_ = nullptr;
|
||||||
|
device_handle* device_ = nullptr;
|
||||||
|
bool is_connected_ = false;
|
||||||
|
|
||||||
|
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr connection_status_pub_;
|
||||||
|
rclcpp::TimerBase::SharedPtr send_timer_;
|
||||||
|
rclcpp::TimerBase::SharedPtr status_timer_;
|
||||||
|
};
|
||||||
|
|
||||||
|
int main(int argc, char* argv[])
|
||||||
|
{
|
||||||
|
rclcpp::init(argc, argv);
|
||||||
|
|
||||||
|
auto node = std::make_shared<TransmitterTestNode>();
|
||||||
|
|
||||||
|
if (rclcpp::ok()) {
|
||||||
|
rclcpp::spin(node);
|
||||||
|
}
|
||||||
|
|
||||||
|
rclcpp::shutdown();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@@ -1,404 +0,0 @@
|
|||||||
/**
|
|
||||||
* @file uart_transmitter_node.cpp
|
|
||||||
* @brief UART 串口收发模块测试节点 (CH340)
|
|
||||||
*
|
|
||||||
* 默认波特率：115200
|
|
||||||
* 协议帧格式：
|
|
||||||
* | 0xBB | 0x77 | 平动左右 | 平动前后 | 云台偏航 | 云台俯仰 | 拨弹轮 | 拨杆 |
|
|
||||||
* CRC8 | 帧尾 | | 0xAA | 0x55 | 2 bytes | 2 bytes | 2 bytes | 2 bytes | 2
|
|
||||||
* bytes|1 byte| 1byte| 0xEE |
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <cstring>
|
|
||||||
#include <errno.h>
|
|
||||||
#include <fcntl.h>
|
|
||||||
#include <rclcpp/rclcpp.hpp>
|
|
||||||
#include <std_msgs/msg/bool.hpp>
|
|
||||||
#include <termios.h>
|
|
||||||
#include <thread>
|
|
||||||
#include <unistd.h>
|
|
||||||
#include <vector>
|
|
||||||
|
|
||||||
// 帧定义
|
|
||||||
constexpr uint8_t FRAME_HEADER_1 = 0xBB;
|
|
||||||
constexpr uint8_t FRAME_HEADER_2 = 0x77;
|
|
||||||
constexpr uint8_t FRAME_TAIL = 0xEE;
|
|
||||||
constexpr int FRAME_LENGTH = 15;
|
|
||||||
|
|
||||||
// 默认串口设备
|
|
||||||
constexpr const char *DEFAULT_SERIAL_PORT = "/dev/ttyCH340";
|
|
||||||
constexpr int DEFAULT_BAUDRATE = 115200;
|
|
||||||
|
|
||||||
class UartTransmitterNode : public rclcpp::Node {
|
|
||||||
public:
|
|
||||||
UartTransmitterNode() : Node("uart_transmitter_node") {
|
|
||||||
// 声明参数
|
|
||||||
this->declare_parameter("serial_port", DEFAULT_SERIAL_PORT);
|
|
||||||
this->declare_parameter("baudrate", DEFAULT_BAUDRATE);
|
|
||||||
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]， 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();
|
|
||||||
baudrate_ = this->get_parameter("baudrate").as_int();
|
|
||||||
send_frequency_ = this->get_parameter("send_frequency").as_double();
|
|
||||||
|
|
||||||
RCLCPP_INFO(this->get_logger(), "---------------------------------");
|
|
||||||
RCLCPP_INFO(this->get_logger(), "UART 收发节点启动");
|
|
||||||
RCLCPP_INFO(this->get_logger(), "串口: %s", serial_port_.c_str());
|
|
||||||
RCLCPP_INFO(this->get_logger(), "波特率: %d", baudrate_);
|
|
||||||
RCLCPP_INFO(this->get_logger(), "发送频率: %.1f Hz", send_frequency_);
|
|
||||||
RCLCPP_INFO(this->get_logger(), "---------------------------------");
|
|
||||||
|
|
||||||
// 初始化串口
|
|
||||||
if (!initSerial()) {
|
|
||||||
RCLCPP_ERROR(this->get_logger(), "串口初始化失败，节点退出");
|
|
||||||
rclcpp::shutdown();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 创建发布者
|
|
||||||
connection_status_pub_ = this->create_publisher<std_msgs::msg::Bool>(
|
|
||||||
"transmitter/connection_status", 10);
|
|
||||||
|
|
||||||
// 创建定时器 - 发送数据
|
|
||||||
auto send_period = std::chrono::duration<double>(1.0 / send_frequency_);
|
|
||||||
send_timer_ = this->create_wall_timer(
|
|
||||||
std::chrono::duration_cast<std::chrono::milliseconds>(send_period),
|
|
||||||
std::bind(&UartTransmitterNode::sendControlFrame, this));
|
|
||||||
|
|
||||||
// 创建定时器 - 发布连接状态
|
|
||||||
status_timer_ = this->create_wall_timer(
|
|
||||||
std::chrono::seconds(1),
|
|
||||||
std::bind(&UartTransmitterNode::publishStatus, this));
|
|
||||||
|
|
||||||
// 创建接收线程
|
|
||||||
receive_thread_ = std::thread(&UartTransmitterNode::receiveLoop, this);
|
|
||||||
|
|
||||||
RCLCPP_INFO(this->get_logger(), "UART 节点初始化完成");
|
|
||||||
}
|
|
||||||
|
|
||||||
~UartTransmitterNode() {
|
|
||||||
running_ = false;
|
|
||||||
if (receive_thread_.joinable()) {
|
|
||||||
receive_thread_.join();
|
|
||||||
}
|
|
||||||
if (serial_fd_ >= 0) {
|
|
||||||
close(serial_fd_);
|
|
||||||
}
|
|
||||||
RCLCPP_INFO(this->get_logger(), "串口已关闭");
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
bool initSerial() {
|
|
||||||
RCLCPP_INFO(this->get_logger(), "正在打开串口 %s...", serial_port_.c_str());
|
|
||||||
|
|
||||||
// 打开串口
|
|
||||||
serial_fd_ = open(serial_port_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
|
|
||||||
if (serial_fd_ < 0) {
|
|
||||||
RCLCPP_ERROR(this->get_logger(), "无法打开串口 %s: %s",
|
|
||||||
serial_port_.c_str(), strerror(errno));
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 配置串口
|
|
||||||
struct termios tty;
|
|
||||||
memset(&tty, 0, sizeof(tty));
|
|
||||||
|
|
||||||
if (tcgetattr(serial_fd_, &tty) != 0) {
|
|
||||||
RCLCPP_ERROR(this->get_logger(), "tcgetattr 错误: %s", strerror(errno));
|
|
||||||
close(serial_fd_);
|
|
||||||
serial_fd_ = -1;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 设置波特率
|
|
||||||
speed_t baud = convertBaudrate(baudrate_);
|
|
||||||
cfsetospeed(&tty, baud);
|
|
||||||
cfsetispeed(&tty, baud);
|
|
||||||
|
|
||||||
// 8N1
|
|
||||||
tty.c_cflag &= ~PARENB; // 无校验
|
|
||||||
tty.c_cflag &= ~CSTOPB; // 1位停止位
|
|
||||||
tty.c_cflag &= ~CSIZE;
|
|
||||||
tty.c_cflag |= CS8; // 8位数据
|
|
||||||
tty.c_cflag |= CREAD | CLOCAL; // 启用接收，忽略控制线
|
|
||||||
|
|
||||||
// 禁用硬件流控
|
|
||||||
tty.c_cflag &= ~CRTSCTS;
|
|
||||||
|
|
||||||
// 原始模式
|
|
||||||
tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
|
|
||||||
tty.c_iflag &= ~(IXON | IXOFF | IXANY);
|
|
||||||
tty.c_oflag &= ~OPOST;
|
|
||||||
|
|
||||||
// 设置超时
|
|
||||||
tty.c_cc[VMIN] = 0;
|
|
||||||
tty.c_cc[VTIME] = 1; // 100ms 超时
|
|
||||||
|
|
||||||
if (tcsetattr(serial_fd_, TCSANOW, &tty) != 0) {
|
|
||||||
RCLCPP_ERROR(this->get_logger(), "tcsetattr 错误: %s", strerror(errno));
|
|
||||||
close(serial_fd_);
|
|
||||||
serial_fd_ = -1;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 清空缓冲区
|
|
||||||
tcflush(serial_fd_, TCIOFLUSH);
|
|
||||||
|
|
||||||
RCLCPP_INFO(this->get_logger(), "串口打开成功");
|
|
||||||
is_connected_ = true;
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
speed_t convertBaudrate(int baudrate) {
|
|
||||||
switch (baudrate) {
|
|
||||||
case 9600:
|
|
||||||
return B9600;
|
|
||||||
case 19200:
|
|
||||||
return B19200;
|
|
||||||
case 38400:
|
|
||||||
return B38400;
|
|
||||||
case 57600:
|
|
||||||
return B57600;
|
|
||||||
case 115200:
|
|
||||||
return B115200;
|
|
||||||
case 230400:
|
|
||||||
return B230400;
|
|
||||||
case 460800:
|
|
||||||
return B460800;
|
|
||||||
case 921600:
|
|
||||||
return B921600;
|
|
||||||
default:
|
|
||||||
RCLCPP_WARN(this->get_logger(), "不支持的波特率 %d, 使用 115200",
|
|
||||||
baudrate);
|
|
||||||
return B115200;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void sendControlFrame() {
|
|
||||||
if (serial_fd_ < 0) {
|
|
||||||
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
|
|
||||||
"串口未打开");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 获取控制参数
|
|
||||||
int16_t x_move =
|
|
||||||
static_cast<int16_t>(this->get_parameter("x_move").as_int());
|
|
||||||
int16_t y_move =
|
|
||||||
static_cast<int16_t>(this->get_parameter("y_move").as_int());
|
|
||||||
int16_t yaw = static_cast<int16_t>(this->get_parameter("yaw").as_int());
|
|
||||||
int16_t pitch = static_cast<int16_t>(this->get_parameter("pitch").as_int());
|
|
||||||
int16_t feed = static_cast<int16_t>(this->get_parameter("feed").as_int());
|
|
||||||
uint8_t left_switch =
|
|
||||||
static_cast<uint8_t>(this->get_parameter("left_switch").as_int()) &
|
|
||||||
0x0F;
|
|
||||||
uint8_t right_switch =
|
|
||||||
static_cast<uint8_t>(this->get_parameter("right_switch").as_int()) &
|
|
||||||
0x0F;
|
|
||||||
|
|
||||||
// 构建数据帧
|
|
||||||
uint8_t frame[FRAME_LENGTH];
|
|
||||||
int idx = 0;
|
|
||||||
|
|
||||||
// 帧头
|
|
||||||
frame[idx++] = FRAME_HEADER_1;
|
|
||||||
frame[idx++] = FRAME_HEADER_2;
|
|
||||||
|
|
||||||
// 平动左右 (2 bytes, int16, 小端序)
|
|
||||||
frame[idx++] = x_move & 0xFF;
|
|
||||||
frame[idx++] = (x_move >> 8) & 0xFF;
|
|
||||||
|
|
||||||
// 平动前后 (2 bytes, 小端序)
|
|
||||||
frame[idx++] = y_move & 0xFF;
|
|
||||||
frame[idx++] = (y_move >> 8) & 0xFF;
|
|
||||||
|
|
||||||
// 云台偏航 (2 bytes, 小端序)
|
|
||||||
frame[idx++] = yaw & 0xFF;
|
|
||||||
frame[idx++] = (yaw >> 8) & 0xFF;
|
|
||||||
|
|
||||||
// 云台俯仰 (2 bytes, 小端序)
|
|
||||||
frame[idx++] = pitch & 0xFF;
|
|
||||||
frame[idx++] = (pitch >> 8) & 0xFF;
|
|
||||||
|
|
||||||
// 拨弹轮 (2 bytes, 小端序)
|
|
||||||
frame[idx++] = feed & 0xFF;
|
|
||||||
frame[idx++] = (feed >> 8) & 0xFF;
|
|
||||||
|
|
||||||
// 拨杆 (1 byte: 高4位左拨杆，低4位右拨杆)
|
|
||||||
frame[idx++] = (left_switch << 4) | right_switch;
|
|
||||||
|
|
||||||
// CRC8 (除帧头和帧尾外的所有数据，不包括CRC本身)
|
|
||||||
// 当前idx=13, 数据长度=11 (从frame[2]到frame[12])
|
|
||||||
// uint8_t crc_value = calculateCRC8(frame + 2, 11);
|
|
||||||
// 帧尾 1，接收没有写 CRC8，因此固定为 CC
|
|
||||||
frame[idx++] = 0xCC;
|
|
||||||
|
|
||||||
// 帧尾 2
|
|
||||||
frame[idx++] = FRAME_TAIL;
|
|
||||||
|
|
||||||
// 调试输出：打印完整帧内容
|
|
||||||
std::string frame_hex;
|
|
||||||
char buf[4];
|
|
||||||
for (int i = 0; i < FRAME_LENGTH; i++) {
|
|
||||||
snprintf(buf, sizeof(buf), "%02X ", frame[i]);
|
|
||||||
frame_hex += buf;
|
|
||||||
}
|
|
||||||
RCLCPP_INFO(this->get_logger(), "[TX](%dB): %s", FRAME_LENGTH,
|
|
||||||
frame_hex.c_str());
|
|
||||||
|
|
||||||
// 发送数据
|
|
||||||
ssize_t written = write(serial_fd_, frame, FRAME_LENGTH);
|
|
||||||
if (written != FRAME_LENGTH) {
|
|
||||||
RCLCPP_WARN(this->get_logger(), "发送数据不完整: %zd/%d", written,
|
|
||||||
FRAME_LENGTH);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
uint8_t calculateCRC8(const uint8_t *data, size_t len) {
|
|
||||||
uint8_t crc = 0xFF; // 初始值
|
|
||||||
for (size_t i = 0; i < len; i++) {
|
|
||||||
crc ^= data[i];
|
|
||||||
for (int j = 0; j < 8; j++) {
|
|
||||||
if (crc & 0x80) {
|
|
||||||
crc = (crc << 1) ^ 0x31; // CRC8-MAXIM 多项式
|
|
||||||
} else {
|
|
||||||
crc <<= 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return crc;
|
|
||||||
}
|
|
||||||
|
|
||||||
void receiveLoop() {
|
|
||||||
uint8_t buffer[256];
|
|
||||||
std::vector<uint8_t> frame_buffer;
|
|
||||||
frame_buffer.reserve(FRAME_LENGTH);
|
|
||||||
|
|
||||||
while (running_ && rclcpp::ok()) {
|
|
||||||
if (serial_fd_ < 0) {
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 读取数据
|
|
||||||
ssize_t n = read(serial_fd_, buffer, sizeof(buffer));
|
|
||||||
if (n > 0) {
|
|
||||||
for (ssize_t i = 0; i < n; i++) {
|
|
||||||
frame_buffer.push_back(buffer[i]);
|
|
||||||
|
|
||||||
// 查找帧头
|
|
||||||
if (frame_buffer.size() >= 2 && frame_buffer[0] == FRAME_HEADER_1 &&
|
|
||||||
frame_buffer[1] == FRAME_HEADER_2) {
|
|
||||||
|
|
||||||
// 等待完整帧
|
|
||||||
if (frame_buffer.size() >= FRAME_LENGTH) {
|
|
||||||
// 检查帧尾
|
|
||||||
if (frame_buffer[FRAME_LENGTH - 1] == FRAME_TAIL) {
|
|
||||||
// 验证 CRC
|
|
||||||
uint8_t rx_crc = frame_buffer[FRAME_LENGTH - 2];
|
|
||||||
uint8_t calc_crc =
|
|
||||||
calculateCRC8(frame_buffer.data() + 2, FRAME_LENGTH - 4);
|
|
||||||
|
|
||||||
if (rx_crc == calc_crc) {
|
|
||||||
// 解析数据（小端序）
|
|
||||||
int16_t x_move = frame_buffer[2] | (frame_buffer[3] << 8);
|
|
||||||
int16_t y_move = frame_buffer[4] | (frame_buffer[5] << 8);
|
|
||||||
int16_t yaw = frame_buffer[6] | (frame_buffer[7] << 8);
|
|
||||||
int16_t pitch = frame_buffer[8] | (frame_buffer[9] << 8);
|
|
||||||
int16_t feed = frame_buffer[10] | (frame_buffer[11] << 8);
|
|
||||||
uint8_t key = frame_buffer[12];
|
|
||||||
uint8_t left_switch = (key >> 4) & 0x0F;
|
|
||||||
uint8_t right_switch = key & 0x0F;
|
|
||||||
|
|
||||||
// 输出详细日志
|
|
||||||
RCLCPP_INFO(this->get_logger(),
|
|
||||||
"[RX]: %02X %02X | x:%d y:%d yaw:%d "
|
|
||||||
"pitch:%d feed:%d | L:%d R:%d | CRC:%02X | %02X",
|
|
||||||
frame_buffer[0], frame_buffer[1], x_move, y_move,
|
|
||||||
yaw, pitch, feed, left_switch, right_switch,
|
|
||||||
rx_crc, frame_buffer[FRAME_LENGTH - 1]);
|
|
||||||
} else {
|
|
||||||
RCLCPP_WARN(
|
|
||||||
this->get_logger(),
|
|
||||||
"[CRC错误] 接收=%02X, 计算=%02X, 帧内容: %s", rx_crc,
|
|
||||||
calc_crc,
|
|
||||||
bytesToHex(frame_buffer.data(), FRAME_LENGTH).c_str());
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
RCLCPP_WARN(this->get_logger(),
|
|
||||||
"[帧尾错误] 期望=%02X, 实际=%02X", FRAME_TAIL,
|
|
||||||
frame_buffer[FRAME_LENGTH - 1]);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 清空缓冲区，准备下一帧
|
|
||||||
frame_buffer.clear();
|
|
||||||
}
|
|
||||||
} else if (frame_buffer.size() > FRAME_LENGTH) {
|
|
||||||
// 缓冲区溢出，清空
|
|
||||||
frame_buffer.clear();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
} else if (n < 0 && errno != EAGAIN) {
|
|
||||||
RCLCPP_ERROR(this->get_logger(), "读取错误: %s", strerror(errno));
|
|
||||||
}
|
|
||||||
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
std::string bytesToHex(const uint8_t *data, size_t len) {
|
|
||||||
std::string result;
|
|
||||||
char buf[4];
|
|
||||||
for (size_t i = 0; i < len; i++) {
|
|
||||||
snprintf(buf, sizeof(buf), "%02X ", data[i]);
|
|
||||||
result += buf;
|
|
||||||
}
|
|
||||||
return result;
|
|
||||||
}
|
|
||||||
|
|
||||||
void publishStatus() {
|
|
||||||
auto msg = std_msgs::msg::Bool();
|
|
||||||
msg.data = is_connected_ && (serial_fd_ >= 0);
|
|
||||||
connection_status_pub_->publish(msg);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 成员变量
|
|
||||||
std::string serial_port_;
|
|
||||||
int baudrate_;
|
|
||||||
double send_frequency_;
|
|
||||||
|
|
||||||
int serial_fd_ = -1;
|
|
||||||
bool is_connected_ = false;
|
|
||||||
bool running_ = true;
|
|
||||||
std::thread receive_thread_;
|
|
||||||
|
|
||||||
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr connection_status_pub_;
|
|
||||||
rclcpp::TimerBase::SharedPtr send_timer_;
|
|
||||||
rclcpp::TimerBase::SharedPtr status_timer_;
|
|
||||||
};
|
|
||||||
|
|
||||||
int main(int argc, char *argv[]) {
|
|
||||||
rclcpp::init(argc, argv);
|
|
||||||
|
|
||||||
auto node = std::make_shared<UartTransmitterNode>();
|
|
||||||
|
|
||||||
if (rclcpp::ok()) {
|
|
||||||
rclcpp::spin(node);
|
|
||||||
}
|
|
||||||
|
|
||||||
rclcpp::shutdown();
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@@ -1,5 +0,0 @@
|
|||||||
# CH340 串口模块 (ttyUSB0 -> ttyCH340)
|
|
||||||
SUBSYSTEM=="tty", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", SYMLINK+="ttyCH340", MODE="0666", GROUP="dialout"
|
|
||||||
|
|
||||||
# IMU 串口模块 (ttyUSB1 -> ttyIMU)
|
|
||||||
SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", SYMLINK+="ttyIMU", MODE="0666", GROUP="dialout"
|
|
||||||
@@ -1,38 +0,0 @@
|
|||||||
#!/bin/bash
|
|
||||||
# USB 串口设备权限和固定名称设置脚本
|
|
||||||
|
|
||||||
set -e
|
|
||||||
|
|
||||||
echo "=================================="
|
|
||||||
echo "USB 串口设备权限设置"
|
|
||||||
echo "=================================="
|
|
||||||
|
|
||||||
# 1. 检查当前设备
|
|
||||||
echo ""
|
|
||||||
echo "当前连接的 USB 串口设备："
|
|
||||||
ls -la /dev/ttyUSB* 2>/dev/null || echo "未发现 ttyUSB 设备"
|
|
||||||
|
|
||||||
echo ""
|
|
||||||
echo "设备详细信息："
|
|
||||||
for dev in /dev/ttyUSB*; do
|
|
||||||
if [ -e "$dev" ]; then
|
|
||||||
echo "--- $dev ---"
|
|
||||||
udevadm info -a -n "$dev" | grep -E "(idVendor|idProduct|serial)" | head -5
|
|
||||||
fi
|
|
||||||
done
|
|
||||||
|
|
||||||
# 2. 安装规则文件
|
|
||||||
echo ""
|
|
||||||
echo "安装 udev 规则"
|
|
||||||
sudo cp 99-usb-serial.rules /etc/udev/rules.d/
|
|
||||||
|
|
||||||
# 3. 重新加载 udev 规则
|
|
||||||
echo "重新加载 udev 规则"
|
|
||||||
sudo udevadm control --reload-rules
|
|
||||||
sudo udevadm trigger
|
|
||||||
|
|
||||||
# 4. 添加用户到 dialout 组
|
|
||||||
echo "将当前用户添加到 dialout 组"
|
|
||||||
sudo usermod -aG dialout $USER
|
|
||||||
|
|
||||||
echo "设置完成，请重新插拔设备"
|
|
||||||
Reference in New Issue
Block a user