Compare commits
7 Commits
338ca9d2b7
...
7f4aaf78cd
| Author | SHA1 | Date | |
|---|---|---|---|
|
7f4aaf78cd | ||
| 948c0be417 | |||
| 01d236d1a0 | |||
| 860decf873 | |||
| e670fe0848 | |||
| 3cf42d0f0d | |||
| 53aa847cc9 |
47
.gitignore
vendored
47
.gitignore
vendored
@@ -0,0 +1,47 @@
|
||||
# ROS 2 / Colcon
|
||||
build/
|
||||
install/
|
||||
log/
|
||||
|
||||
# CMake
|
||||
CMakeCache.txt
|
||||
CMakeFiles/
|
||||
cmake_install.cmake
|
||||
Makefile
|
||||
CTestTestfile.cmake
|
||||
Testing/
|
||||
|
||||
# C++ 编译输出
|
||||
*.o
|
||||
*.obj
|
||||
*.so
|
||||
*.a
|
||||
*.dll
|
||||
*.exe
|
||||
*.out
|
||||
|
||||
# IDE
|
||||
.vscode/
|
||||
.idea/
|
||||
*.swp
|
||||
*.swo
|
||||
*~
|
||||
|
||||
# Python
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
*$py.class
|
||||
*.so
|
||||
.Python
|
||||
|
||||
# 系统文件
|
||||
.DS_Store
|
||||
Thumbs.db
|
||||
|
||||
# 临时文件
|
||||
*.tmp
|
||||
*.temp
|
||||
*.log
|
||||
|
||||
# 保留 compile_commands.json 给 clangd 使用
|
||||
# (不忽略 build/compile_commands.json，通过 CMake 导出到项目根目录)
|
||||
28
.vscode/settings.json
vendored
28
.vscode/settings.json
vendored
@@ -1,8 +1,28 @@
|
||||
{
|
||||
"clangd.arguments": [
|
||||
// Clangd 配置
|
||||
"clangd.path": "/usr/bin/clangd",
|
||||
"clangd.arguments": [
|
||||
"--background-index",
|
||||
"--compile-commands-dir=${workspaceFolder}/build",
|
||||
"--completion-style=detailed",
|
||||
"--query-driver=/usr/bin/clang",
|
||||
"--header-insertion=never"
|
||||
"--header-insertion=iwyu",
|
||||
"--completion-style=bundled",
|
||||
"--pch-storage=memory",
|
||||
"--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"
|
||||
}
|
||||
59
CMakeLists.txt
Normal file
59
CMakeLists.txt
Normal file
@@ -0,0 +1,59 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(amadeus_26)
|
||||
|
||||
# 使用 C++17
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
|
||||
# 导出 compile_commands.json 给 clangd 使用
|
||||
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
|
||||
|
||||
# 查找依赖
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
|
||||
# 添加 SDK 库路径
|
||||
set(TRANSMITTER_SDK_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib/transmitter_sdk)
|
||||
link_directories(${TRANSMITTER_SDK_PATH}/lib/linux/x64)
|
||||
|
||||
# 包含目录
|
||||
include_directories(
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
${TRANSMITTER_SDK_PATH}/inc
|
||||
)
|
||||
|
||||
# ==================== UART 节点 ====================
|
||||
add_executable(uart_transmitter_node src/uart_transmitter_node.cpp)
|
||||
|
||||
ament_target_dependencies(uart_transmitter_node
|
||||
rclcpp
|
||||
std_msgs
|
||||
)
|
||||
|
||||
# ==================== IMU 节点 ====================
|
||||
add_executable(imu_receiver_node src/imu_receiver_node.cpp)
|
||||
|
||||
ament_target_dependencies(imu_receiver_node
|
||||
rclcpp
|
||||
std_msgs
|
||||
geometry_msgs
|
||||
sensor_msgs
|
||||
)
|
||||
|
||||
# ==================== 安装目标 ====================
|
||||
install(TARGETS
|
||||
uart_transmitter_node
|
||||
imu_receiver_node
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
# 安装 launch 文件
|
||||
install(DIRECTORY launch
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
OPTIONAL
|
||||
)
|
||||
|
||||
ament_package()
|
||||
26
README.md
26
README.md
@@ -10,8 +10,8 @@
|
||||
|
||||
- MV-SUA133GC-T 工业相机 (USB)
|
||||
- NUC10i7 微型计算机 (主机)
|
||||
- FDI Systems DETA10 IMU 模块 (USB - CP210x 串口芯片 - UART) `/dev/ttyUSB0`
|
||||
- 达妙 USB2CAN-DUAL USB 转 CAN 模块 (USB - 模块 - CAN*2) `/dev/ttyACM0`
|
||||
- FDI Systems DETA10 IMU 模块 (USB - CP210x 串口芯片 - UART) `/dev/ttyCH340`
|
||||
- CH340 USB 转 UART 模块 (USB - 模块 - UART) `/dev/ttyIMU`
|
||||
|
||||
### 软件
|
||||
|
||||
@@ -19,12 +19,11 @@
|
||||
- OpenCV 4.13 with Contrib
|
||||
- Eigen 3
|
||||
- MVSDK (工业相机 SDK)
|
||||
- dm_device (USB 转 CAN 模块 SDK)
|
||||
- 待补充...
|
||||
|
||||
## 目标
|
||||
|
||||
感谢同济 SuperPower 战队！自瞄算法参考同济的 sp_vision_26 。项目目标为使 2026 年采用的
|
||||
感谢同济 SuperPower 战队！自瞄算法参考同济的 sp_vision_26 。
|
||||
|
||||
## 软件架构
|
||||
|
||||
@@ -37,17 +36,22 @@ ROS2 基本架构为节点。以每个节点的发布、解析、接收数据为
|
||||
- 发布：描述该节点使用 ROS 发布的数据
|
||||
- 发送：描述该节点使用直连硬件发送的数据
|
||||
|
||||
### 收发节点
|
||||
### UART 收发节点
|
||||
|
||||
- 功能：主机对外通信的封装节点。物理上读取收发模块
|
||||
- 连接：USB2CAN，协商 CAN 通信速率 1000kbps，`/dev/ttyACM0`
|
||||
IMU_CP210x，协商 UART 通信速率 921600，`/dev/ttyUSB0`
|
||||
- 接收：USB2CAN -> 裁判系统数据
|
||||
- 连接：CH340，协商 UART 通信速率 115200，`/dev/ttyCH340`
|
||||
- 接收：CH340.RX -> 裁判系统数据
|
||||
- 订阅：中央节点 -> 控制指令
|
||||
- 发送：USB2CAN.CAN2 -> 控制指令
|
||||
- 发送：CH340.TX -> 控制指令
|
||||
- 发布：裁判系统数据（血量、比赛当前状态）
|
||||
CH340 、IMU 连接 OK 标志
|
||||
|
||||
### IMU 收发节点
|
||||
|
||||
- 功能：IMU 与主机通信的封装节点。物理上读取 IMU（DETA10）数据
|
||||
- 连接：IMU_CP210x，协商 UART 通信速率 921600，`/dev/ttyUSB1`
|
||||
- 接收：IMU_CP210x -> 六轴加速度、陀螺仪数据
|
||||
- 发布：IMU 数据（地磁角、六轴角加速度、加速度、陀螺仪）
|
||||
裁判系统数据（血量、比赛当前状态）
|
||||
USB2CAN 、IMU 连接 OK 标志
|
||||
|
||||
### 中央节点
|
||||
|
||||
|
||||
Binary file not shown.
Binary file not shown.
27
docs/IMU/packet/MSG_AHRS.md
Normal file
27
docs/IMU/packet/MSG_AHRS.md
Normal file
@@ -0,0 +1,27 @@
|
||||
# MSG_AHRS | FDISYSTEMS支持中心
|
||||
|
||||
该数据包用于描述：卡尔曼滤波输出航姿参考系统数据
|
||||
|
||||
## 基本信息
|
||||
|
||||
| 属性 | 值 |
|
||||
|------|-----|
|
||||
| **Packet ID** | 0x41 |
|
||||
| **Length** | 48 |
|
||||
| **Read / Write** | Read |
|
||||
|
||||
## 数据字段
|
||||
|
||||
| Offset | Size | Format | Field | Unit | Description |
|
||||
|--------|------|--------|-------|------|-------------|
|
||||
| 0 | 4 | float32_t | RollSpeed | rad/s | 滤波修正后的横滚角速度，等于MSG_IMU数据包里的Gyroscope_X减去卡尔曼滤波估计的X轴角速度零偏 |
|
||||
| 4 | 4 | float32_t | PitchSpeed | rad/s | 滤波修正后的俯仰角速度，等于MSG_IMU数据包里的Gyroscope_Y减去卡尔曼滤波估计的Y轴角速度零偏 |
|
||||
| 8 | 4 | float32_t | HeadingSpeed | rad/s | 滤波修正后的偏航角速度，等于MSG_IMU数据包里的Gyroscope_Z减去卡尔曼滤波估计的Z轴角速度零偏 |
|
||||
| 12 | 4 | float32_t | Roll | rad | 横滚 |
|
||||
| 16 | 4 | float32_t | Pitch | rad | 俯仰 |
|
||||
| 20 | 4 | float32_t | Heading | rad | 偏航 |
|
||||
| 24 | 4 | float32_t | Q1 | / | 四元数Q1 |
|
||||
| 28 | 4 | float32_t | Q2 | / | 四元数Q2 |
|
||||
| 32 | 4 | float32_t | Q3 | / | 四元数Q3 |
|
||||
| 36 | 4 | float32_t | Q4 | / | 四元数Q4 |
|
||||
| 40 | 8 | int64_t | Timestamp | us | 数据的时间戳，从上电开始启动的微秒数。时钟源为MCU外部晶振。 |
|
||||
26
docs/IMU/packet/MSG_BODY_ACCELERATION.md
Normal file
26
docs/IMU/packet/MSG_BODY_ACCELERATION.md
Normal file
@@ -0,0 +1,26 @@
|
||||
# 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 | 当地重力加速度 |
|
||||
19
docs/IMU/packet/MSG_NED_VEL.md
Normal file
19
docs/IMU/packet/MSG_NED_VEL.md
Normal file
@@ -0,0 +1,19 @@
|
||||
# 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 | 滤波修正的地向速度 |
|
||||
@@ -1,27 +0,0 @@
|
||||
# 控制指令的发送说明
|
||||
|
||||
在主机外接收的控制板中的控制代码的实现是：
|
||||
|
||||
```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 分别是云台和底盘的控制板，在处于进点/回家状态时不应开启小陀螺，发射弹丸需要**打开摩擦轮 同时 进弹**，因此只有在处于攻击状态时开启摩擦轮，此时可以将进弹视为扳机。
|
||||
37
docs/Transmit/ctrl_frame.md
Normal file
37
docs/Transmit/ctrl_frame.md
Normal file
@@ -0,0 +1,37 @@
|
||||
# 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
|
||||
87
docs/Transmit/usb_device_setup.md
Normal file
87
docs/Transmit/usb_device_setup.md
Normal file
@@ -0,0 +1,87 @@
|
||||
# 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
|
||||
102
launch/uart_transmitter.launch.py
Normal file
102
launch/uart_transmitter.launch.py
Normal file
@@ -0,0 +1,102 @@
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# 声明启动参数
|
||||
serial_port_arg = DeclareLaunchArgument(
|
||||
'serial_port',
|
||||
default_value='/dev/ttyCH340',
|
||||
description='CH340 串口设备路径'
|
||||
)
|
||||
|
||||
baudrate_arg = DeclareLaunchArgument(
|
||||
'baudrate',
|
||||
default_value='115200',
|
||||
description='串口波特率'
|
||||
)
|
||||
|
||||
send_frequency_arg = DeclareLaunchArgument(
|
||||
'send_frequency',
|
||||
default_value='10.0',
|
||||
description='发送频率 (Hz)'
|
||||
)
|
||||
|
||||
# 控制参数
|
||||
x_move_arg = DeclareLaunchArgument(
|
||||
'x_move',
|
||||
default_value='0',
|
||||
description='平动左右 [-660, 660]'
|
||||
)
|
||||
|
||||
y_move_arg = DeclareLaunchArgument(
|
||||
'y_move',
|
||||
default_value='0',
|
||||
description='平动前后 [-660, 660]'
|
||||
)
|
||||
|
||||
yaw_arg = DeclareLaunchArgument(
|
||||
'yaw',
|
||||
default_value='0',
|
||||
description='云台偏航 [-660, 660]'
|
||||
)
|
||||
|
||||
pitch_arg = DeclareLaunchArgument(
|
||||
'pitch',
|
||||
default_value='0',
|
||||
description='云台俯仰 [-660, 660]'
|
||||
)
|
||||
|
||||
feed_arg = DeclareLaunchArgument(
|
||||
'feed',
|
||||
default_value='0',
|
||||
description='拨弹轮 [-660, 660]'
|
||||
)
|
||||
|
||||
left_switch_arg = DeclareLaunchArgument(
|
||||
'left_switch',
|
||||
default_value='3',
|
||||
description='左拨杆 [1, 3]'
|
||||
)
|
||||
|
||||
right_switch_arg = DeclareLaunchArgument(
|
||||
'right_switch',
|
||||
default_value='3',
|
||||
description='右拨杆 [1, 3]'
|
||||
)
|
||||
|
||||
# 创建节点
|
||||
uart_transmitter_node = Node(
|
||||
package='amadeus_26',
|
||||
executable='uart_transmitter_node',
|
||||
name='uart_transmitter_node',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'serial_port': LaunchConfiguration('serial_port'),
|
||||
'baudrate': LaunchConfiguration('baudrate'),
|
||||
'send_frequency': LaunchConfiguration('send_frequency'),
|
||||
'x_move': LaunchConfiguration('x_move'),
|
||||
'y_move': LaunchConfiguration('y_move'),
|
||||
'yaw': LaunchConfiguration('yaw'),
|
||||
'pitch': LaunchConfiguration('pitch'),
|
||||
'feed': LaunchConfiguration('feed'),
|
||||
'left_switch': LaunchConfiguration('left_switch'),
|
||||
'right_switch': LaunchConfiguration('right_switch'),
|
||||
}],
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
serial_port_arg,
|
||||
baudrate_arg,
|
||||
send_frequency_arg,
|
||||
x_move_arg,
|
||||
y_move_arg,
|
||||
yaw_arg,
|
||||
pitch_arg,
|
||||
feed_arg,
|
||||
left_switch_arg,
|
||||
right_switch_arg,
|
||||
uart_transmitter_node,
|
||||
])
|
||||
20
package.xml
Normal file
20
package.xml
Normal file
@@ -0,0 +1,20 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>amadeus_26</name>
|
||||
<version>0.0.1</version>
|
||||
<description>Amadeus 26 transmission and control package</description>
|
||||
<maintainer email="maintainer@example.com">maintainer</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
591
src/imu_receiver_node.cpp
Normal file
591
src/imu_receiver_node.cpp
Normal file
@@ -0,0 +1,591 @@
|
||||
/**
|
||||
* @file imu_receiver_node.cpp
|
||||
* @brief IMU 数据接收节点 (FDI DETA10) - 高实时性版本
|
||||
*
|
||||
* 通过串口读取 FDI DETA10 IMU 模块的数据
|
||||
* 波特率：921600
|
||||
* 协议：FDILink
|
||||
*
|
||||
* 转发数据包：
|
||||
* - MSG_AHRS (0x41): 航姿参考系统数据
|
||||
* - MSG_BODY_ACCELERATION (0x62): 机体系加速度数据
|
||||
*/
|
||||
|
||||
#include <atomic>
|
||||
#include <cstring>
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <geometry_msgs/msg/quaternion.hpp>
|
||||
#include <geometry_msgs/msg/twist.hpp>
|
||||
#include <geometry_msgs/msg/vector3.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/imu.hpp>
|
||||
#include <std_msgs/msg/bool.hpp>
|
||||
#include <std_msgs/msg/float32.hpp>
|
||||
#include <std_msgs/msg/float64.hpp>
|
||||
#include <termios.h>
|
||||
#include <thread>
|
||||
#include <unistd.h>
|
||||
#include <vector>
|
||||
|
||||
// FDI Link 帧定义
|
||||
constexpr uint8_t FRAME_START = 0xFC;
|
||||
constexpr uint8_t FRAME_END = 0xFD;
|
||||
|
||||
// 数据包 ID
|
||||
constexpr uint8_t MSG_AHRS = 0x41; // 航姿参考系统数据
|
||||
constexpr uint8_t MSG_BODY_ACCELERATION = 0x62; // 机体系加速度数据
|
||||
|
||||
// 默认串口设备
|
||||
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;
|
||||
|
||||
// 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};
|
||||
|
||||
class ImuReceiverNode : public rclcpp::Node {
|
||||
public:
|
||||
ImuReceiverNode() : Node("imu_receiver_node") {
|
||||
// 声明参数
|
||||
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校验（默认关闭）
|
||||
|
||||
// 获取参数
|
||||
serial_port_ = this->get_parameter("serial_port").as_string();
|
||||
baudrate_ = this->get_parameter("baudrate").as_int();
|
||||
enable_crc_ = this->get_parameter("enable_crc").as_bool();
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "=================================");
|
||||
RCLCPP_INFO(this->get_logger(), "IMU 数据接收节点启动 (高实时性版本)");
|
||||
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(), "=================================");
|
||||
|
||||
// 创建发布者
|
||||
ahrs_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("imu/ahrs", 10);
|
||||
body_acc_pub_ = this->create_publisher<geometry_msgs::msg::Vector3>(
|
||||
"imu/body_acceleration", 10);
|
||||
g_force_pub_ =
|
||||
this->create_publisher<std_msgs::msg::Float32>("imu/g_force", 10);
|
||||
connection_status_pub_ = this->create_publisher<std_msgs::msg::Bool>(
|
||||
"imu/connection_status", 10);
|
||||
|
||||
// 初始化串口
|
||||
if (!initSerial()) {
|
||||
RCLCPP_ERROR(this->get_logger(), "串口初始化失败，节点退出");
|
||||
rclcpp::shutdown();
|
||||
return;
|
||||
}
|
||||
|
||||
// 创建接收线程
|
||||
receive_thread_ = std::thread(&ImuReceiverNode::receiveLoop, this);
|
||||
|
||||
// 创建状态发布定时器
|
||||
status_timer_ = this->create_wall_timer(
|
||||
std::chrono::seconds(1),
|
||||
std::bind(&ImuReceiverNode::publishStatus, this));
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "IMU 节点初始化完成");
|
||||
}
|
||||
|
||||
~ImuReceiverNode() {
|
||||
running_ = false;
|
||||
if (receive_thread_.joinable()) {
|
||||
receive_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];
|
||||
|
||||
// 可选的CRC校验
|
||||
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; // 头+CRC16+数据+尾
|
||||
size_t remaining = total_frame_len - 5; // 已经读取了5字节
|
||||
|
||||
// 检查缓冲区中是否有足够的数据
|
||||
if (i + 5 + remaining <= rx_buffer.size()) {
|
||||
// 数据足够，直接复制剩余部分
|
||||
current_frame.insert(current_frame.end(),
|
||||
rx_buffer.begin() + i + 5,
|
||||
rx_buffer.begin() + i + 5 + remaining);
|
||||
|
||||
// 检查帧尾
|
||||
if (current_frame.back() == FRAME_END) {
|
||||
// 解析帧
|
||||
processFrame(current_frame);
|
||||
}
|
||||
// 无论是否有帧尾，都结束当前帧处理
|
||||
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) {
|
||||
// 解析帧
|
||||
processFrame(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));
|
||||
}
|
||||
// n == 0 或 EAGAIN: 没有数据可读，继续循环（无sleep，保证实时性）
|
||||
}
|
||||
}
|
||||
|
||||
// 处理完整的一帧数据
|
||||
void processFrame(const std::vector<uint8_t> &frame) {
|
||||
// 最小帧长度检查：头(5) + CRC16(2) + 尾(1) = 8
|
||||
if (frame.size() < 8)
|
||||
return;
|
||||
|
||||
// 检查帧尾
|
||||
if (frame.back() != FRAME_END)
|
||||
return;
|
||||
|
||||
uint8_t msg_id = frame[1];
|
||||
uint8_t data_len = frame[2];
|
||||
|
||||
// 验证数据长度
|
||||
size_t expected_len = 5 + 2 + data_len + 1;
|
||||
if (frame.size() != expected_len)
|
||||
return;
|
||||
|
||||
// 可选的数据CRC校验
|
||||
if (enable_crc_) {
|
||||
uint16_t data_crc = (frame[5] << 8) | frame[6];
|
||||
uint16_t calc_crc = calculateCRC16(frame.data() + 7, data_len);
|
||||
if (data_crc != calc_crc)
|
||||
return;
|
||||
}
|
||||
|
||||
// 解析数据包
|
||||
parsePacket(msg_id, frame.data() + 7, data_len);
|
||||
}
|
||||
|
||||
void parsePacket(uint8_t msg_id, const uint8_t *data, uint8_t len) {
|
||||
switch (msg_id) {
|
||||
case MSG_AHRS:
|
||||
ahrs_count_++;
|
||||
parseAHRS(data, len);
|
||||
break;
|
||||
case MSG_BODY_ACCELERATION:
|
||||
body_acc_count_++;
|
||||
parseBodyAcceleration(data, len);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// 解析 AHRS 数据 (0x41)
|
||||
void parseAHRS(const uint8_t *data, uint8_t len) {
|
||||
if (len < 48)
|
||||
return;
|
||||
|
||||
// 解析角速度 (rad/s)
|
||||
float roll_speed = *reinterpret_cast<const float *>(data);
|
||||
float pitch_speed = *reinterpret_cast<const float *>(data + 4);
|
||||
float heading_speed = *reinterpret_cast<const float *>(data + 8);
|
||||
|
||||
// 解析欧拉角 (rad)
|
||||
float roll = *reinterpret_cast<const float *>(data + 12);
|
||||
float pitch = *reinterpret_cast<const float *>(data + 16);
|
||||
float heading = *reinterpret_cast<const float *>(data + 20);
|
||||
|
||||
// 解析四元数
|
||||
float q1 = *reinterpret_cast<const float *>(data + 24);
|
||||
float q2 = *reinterpret_cast<const float *>(data + 28);
|
||||
float q3 = *reinterpret_cast<const float *>(data + 32);
|
||||
float q4 = *reinterpret_cast<const float *>(data + 36);
|
||||
|
||||
// 解析时间戳 (int64_t, us)
|
||||
int64_t timestamp_us = *reinterpret_cast<const int64_t *>(data + 40);
|
||||
|
||||
// 将 IMU 时间戳转换为 ROS 时间
|
||||
// 使用当前 ROS 时间作为基准，计算相对时间
|
||||
rclcpp::Time current_time = this->now();
|
||||
rclcpp::Time imu_time =
|
||||
imu_base_time_ + rclcpp::Duration(0, timestamp_us * 1000);
|
||||
|
||||
// 如果是第一个数据包，初始化基准时间
|
||||
if (!imu_time_initialized_) {
|
||||
imu_base_time_ = current_time - rclcpp::Duration(0, timestamp_us * 1000);
|
||||
imu_time = current_time;
|
||||
imu_time_initialized_ = true;
|
||||
}
|
||||
|
||||
// 保存最新的 IMU 时间戳，供 BodyAcceleration 使用
|
||||
latest_imu_timestamp_ = imu_time;
|
||||
|
||||
// 发布 IMU 消息
|
||||
sensor_msgs::msg::Imu imu_msg;
|
||||
imu_msg.header.stamp = imu_time;
|
||||
imu_msg.header.frame_id = "imu_link";
|
||||
|
||||
// 角速度 (转换为 ROS 坐标系: x=roll, y=pitch, z=heading)
|
||||
imu_msg.angular_velocity.x = roll_speed;
|
||||
imu_msg.angular_velocity.y = pitch_speed;
|
||||
imu_msg.angular_velocity.z = heading_speed;
|
||||
|
||||
// 四元数 (FDI: Q1=w, Q2=x, Q3=y, Q4=z -> ROS: x,y,z,w)
|
||||
imu_msg.orientation.x = q2;
|
||||
imu_msg.orientation.y = q3;
|
||||
imu_msg.orientation.z = q4;
|
||||
imu_msg.orientation.w = q1;
|
||||
|
||||
// 线性加速度暂时设为0（从 MSG_BODY_ACCELERATION 获取）
|
||||
imu_msg.linear_acceleration.x = 0;
|
||||
imu_msg.linear_acceleration.y = 0;
|
||||
imu_msg.linear_acceleration.z = 0;
|
||||
|
||||
ahrs_pub_->publish(imu_msg);
|
||||
|
||||
// 根据 verbose 参数输出日志
|
||||
bool verbose = this->get_parameter("verbose").as_bool();
|
||||
if (verbose) {
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
"AHRS - Roll: %.3f, Pitch: %.3f, Heading: %.3f rad, Time: %ld us",
|
||||
roll, pitch, heading, timestamp_us);
|
||||
} else {
|
||||
RCLCPP_DEBUG(this->get_logger(),
|
||||
"AHRS - Roll: %.3f, Pitch: %.3f, Heading: %.3f rad", roll,
|
||||
pitch, heading);
|
||||
}
|
||||
}
|
||||
|
||||
// 解析机体系加速度数据 (0x62)
|
||||
void parseBodyAcceleration(const uint8_t *data, uint8_t len) {
|
||||
if (len < 16)
|
||||
return;
|
||||
|
||||
// 解析机体系加速度 (m/s²)
|
||||
float acc_x = *reinterpret_cast<const float *>(data);
|
||||
float acc_y = *reinterpret_cast<const float *>(data + 4);
|
||||
float acc_z = *reinterpret_cast<const float *>(data + 8);
|
||||
|
||||
// 解析重力加速度
|
||||
float g_force = *reinterpret_cast<const float *>(data + 12);
|
||||
|
||||
// 使用与 AHRS 相同的时间戳（如果可用），否则使用当前时间
|
||||
rclcpp::Time msg_time = latest_imu_timestamp_;
|
||||
if (msg_time.nanoseconds() == 0) {
|
||||
msg_time = this->now();
|
||||
}
|
||||
|
||||
// 发布机体系加速度
|
||||
geometry_msgs::msg::Vector3 acc_msg;
|
||||
acc_msg.x = acc_x;
|
||||
acc_msg.y = acc_y;
|
||||
acc_msg.z = acc_z;
|
||||
body_acc_pub_->publish(acc_msg);
|
||||
|
||||
// 发布重力加速度
|
||||
std_msgs::msg::Float32 g_msg;
|
||||
g_msg.data = g_force;
|
||||
g_force_pub_->publish(g_msg);
|
||||
|
||||
// 根据 verbose 参数输出日志
|
||||
bool verbose = this->get_parameter("verbose").as_bool();
|
||||
if (verbose) {
|
||||
RCLCPP_INFO(this->get_logger(),
|
||||
"BodyAcc - X: %.3f, Y: %.3f, Z: %.3f m/s2, G: %.3f", acc_x,
|
||||
acc_y, acc_z, g_force);
|
||||
} else {
|
||||
RCLCPP_DEBUG(this->get_logger(),
|
||||
"BodyAcc - X: %.3f, Y: %.3f, Z: %.3f m/s2, G: %.3f", acc_x,
|
||||
acc_y, acc_z, g_force);
|
||||
}
|
||||
}
|
||||
|
||||
void publishStatus() {
|
||||
auto msg = std_msgs::msg::Bool();
|
||||
msg.data = is_connected_ && (serial_fd_ >= 0);
|
||||
connection_status_pub_->publish(msg);
|
||||
|
||||
if (is_connected_) {
|
||||
RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
|
||||
"IMU 接收正常 - AHRS: %zu, BodyAcc: %zu",
|
||||
ahrs_count_.load(), body_acc_count_.load());
|
||||
}
|
||||
}
|
||||
|
||||
// 成员变量
|
||||
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::atomic<size_t> ahrs_count_;
|
||||
std::atomic<size_t> body_acc_count_;
|
||||
|
||||
// IMU 时间戳同步
|
||||
bool imu_time_initialized_ = false;
|
||||
rclcpp::Time imu_base_time_;
|
||||
rclcpp::Time latest_imu_timestamp_;
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr ahrs_pub_;
|
||||
rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr body_acc_pub_;
|
||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr g_force_pub_;
|
||||
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr connection_status_pub_;
|
||||
rclcpp::TimerBase::SharedPtr status_timer_;
|
||||
};
|
||||
|
||||
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;
|
||||
}
|
||||
400
src/uart_transmitter_node.cpp
Normal file
400
src/uart_transmitter_node.cpp
Normal file
@@ -0,0 +1,400 @@
|
||||
/**
|
||||
* @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]
|
||||
this->declare_parameter("left_switch", 0); // 左拨杆 [1, 3]
|
||||
this->declare_parameter("right_switch", 0); // 右拨杆 [1, 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);
|
||||
frame[idx++] = 0xCC; // crc_value
|
||||
|
||||
// 帧尾
|
||||
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;
|
||||
}
|
||||
5
tool/99-usb-serial.rules
Normal file
5
tool/99-usb-serial.rules
Normal file
@@ -0,0 +1,5 @@
|
||||
# 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"
|
||||
38
tool/setup_usb_permissions.sh
Executable file
38
tool/setup_usb_permissions.sh
Executable file
@@ -0,0 +1,38 @@
|
||||
#!/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 "设置完成，请重新插拔设备"
|
||||
@@ -1,149 +0,0 @@
|
||||
//
|
||||
// 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
|
||||
Binary file not shown.
Binary file not shown.
Reference in New Issue
Block a user