diff --git a/.gitignore b/.gitignore index e69de29..5079921 100644 --- a/.gitignore +++ b/.gitignore @@ -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 导出到项目根目录) \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index aa46b6d..2a31b6f 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -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" } \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..92a135f --- /dev/null +++ b/CMakeLists.txt @@ -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() \ No newline at end of file diff --git a/README.md b/README.md index 3c56262..b75c444 100644 --- a/README.md +++ b/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 标志 ### 中央节点 diff --git a/camera_sdk/lib/linux/arm64/libMVSDK.so b/camera_sdk/lib/linux/arm64/libMVSDK.so deleted file mode 100644 index 402ea21..0000000 Binary files a/camera_sdk/lib/linux/arm64/libMVSDK.so and /dev/null differ diff --git a/camera_sdk/lib/linux/x64/libMVSDK.so b/camera_sdk/lib/linux/x64/libMVSDK.so deleted file mode 100644 index 625adcb..0000000 Binary files a/camera_sdk/lib/linux/x64/libMVSDK.so and /dev/null differ diff --git a/docs/IMU/packet/MSG_AHRS.md b/docs/IMU/packet/MSG_AHRS.md new file mode 100644 index 0000000..8b0e3cc --- /dev/null +++ b/docs/IMU/packet/MSG_AHRS.md @@ -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外部晶振。 | \ No newline at end of file diff --git a/docs/IMU/packet/MSG_BODY_ACCELERATION.md b/docs/IMU/packet/MSG_BODY_ACCELERATION.md new file mode 100644 index 0000000..65e2d83 --- /dev/null +++ b/docs/IMU/packet/MSG_BODY_ACCELERATION.md @@ -0,0 +1,26 @@ +# MSG_BODY_ACCELERATION | FDISYSTEMS支持中心 + +![FDI SYSTEMS 支持中心](https://doc.fdisystems.cn/logo.png) + +[Back to FDISYSTEMS](https://fdisystems.cn) + +Developers / FDILink协议 / Data Packets / MSG_BODY_ACCELERATION + +该数据包用于描述：滤波修正后的机体系加速度，不包括重力加速度。 + +## 基本信息 + +| 属性 | 值 | +|------|-----| +| **Packet ID** | 0x62 | +| **Length** | 16 | +| **Read / Write** | Read | + +## 数据字段 + +| Offset | Size | Format | Field | Unit | Description | +|--------|------|--------|-------|------|-------------| +| 0 | 4 | float32_t | Body_acceleration_X | m/s/s | 滤波修正后的机体系X轴方向加速度 | +| 4 | 4 | float32_t | Body_acceleration_Y | m/s/s | 滤波修正后的机体系Y轴方向加速度 | +| 8 | 4 | float32_t | Body_acceleration_Z | m/s/s | 滤波修正后的机体系Z轴方向加速度 | +| 12 | 4 | float32_t | G_force | m/s/s | 当地重力加速度 | diff --git a/docs/IMU/packet/MSG_NED_VEL.md b/docs/IMU/packet/MSG_NED_VEL.md new file mode 100644 index 0000000..91dd296 --- /dev/null +++ b/docs/IMU/packet/MSG_NED_VEL.md @@ -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 | 滤波修正的地向速度 | \ No newline at end of file diff --git a/docs/Transmit/ctrl_command.md b/docs/Transmit/ctrl_command.md deleted file mode 100644 index a1bdb81..0000000 --- a/docs/Transmit/ctrl_command.md +++ /dev/null @@ -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 分别是云台和底盘的控制板，在处于进点/回家状态时不应开启小陀螺，发射弹丸需要**打开摩擦轮 同时 进弹**，因此只有在处于攻击状态时开启摩擦轮，此时可以将进弹视为扳机。 \ No newline at end of file diff --git a/docs/Transmit/ctrl_frame.md b/docs/Transmit/ctrl_frame.md new file mode 100644 index 0000000..b066656 --- /dev/null +++ b/docs/Transmit/ctrl_frame.md @@ -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 \ No newline at end of file diff --git a/docs/Transmit/usb_device_setup.md b/docs/Transmit/usb_device_setup.md new file mode 100644 index 0000000..a96ac44 --- /dev/null +++ b/docs/Transmit/usb_device_setup.md @@ -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 \ No newline at end of file diff --git a/launch/uart_transmitter.launch.py b/launch/uart_transmitter.launch.py new file mode 100644 index 0000000..1e52044 --- /dev/null +++ b/launch/uart_transmitter.launch.py @@ -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, + ]) \ No newline at end of file diff --git a/camera_sdk/inc/CameraApi.h b/lib/camera_sdk/inc/CameraApi.h similarity index 100% rename from camera_sdk/inc/CameraApi.h rename to lib/camera_sdk/inc/CameraApi.h diff --git a/camera_sdk/inc/CameraDefine.h b/lib/camera_sdk/inc/CameraDefine.h similarity index 100% rename from camera_sdk/inc/CameraDefine.h rename to lib/camera_sdk/inc/CameraDefine.h diff --git a/camera_sdk/inc/CameraStatus.h b/lib/camera_sdk/inc/CameraStatus.h similarity index 100% rename from camera_sdk/inc/CameraStatus.h rename to lib/camera_sdk/inc/CameraStatus.h diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..9430c9b --- /dev/null +++ b/package.xml @@ -0,0 +1,20 @@ + + + + amadeus_26 + 0.0.1 + Amadeus 26 transmission and control package + maintainer + MIT + + ament_cmake + + rclcpp + std_msgs + geometry_msgs + sensor_msgs + + + ament_cmake + + \ No newline at end of file diff --git a/src/imu_receiver_node.cpp b/src/imu_receiver_node.cpp new file mode 100644 index 0000000..49c39d8 --- /dev/null +++ b/src/imu_receiver_node.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// 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("imu/ahrs", 10); + body_acc_pub_ = this->create_publisher( + "imu/body_acceleration", 10); + g_force_pub_ = + this->create_publisher("imu/g_force", 10); + connection_status_pub_ = this->create_publisher( + "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 rx_buffer; + rx_buffer.reserve(SERIAL_READ_BUF_SIZE * 2); + + // 临时读取缓冲区 + uint8_t read_buf[SERIAL_READ_BUF_SIZE]; + + // 帧解析状态 + bool in_frame = false; + std::vector 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 &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(data); + float pitch_speed = *reinterpret_cast(data + 4); + float heading_speed = *reinterpret_cast(data + 8); + + // 解析欧拉角 (rad) + float roll = *reinterpret_cast(data + 12); + float pitch = *reinterpret_cast(data + 16); + float heading = *reinterpret_cast(data + 20); + + // 解析四元数 + float q1 = *reinterpret_cast(data + 24); + float q2 = *reinterpret_cast(data + 28); + float q3 = *reinterpret_cast(data + 32); + float q4 = *reinterpret_cast(data + 36); + + // 解析时间戳 (int64_t, us) + int64_t timestamp_us = *reinterpret_cast(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(data); + float acc_y = *reinterpret_cast(data + 4); + float acc_z = *reinterpret_cast(data + 8); + + // 解析重力加速度 + float g_force = *reinterpret_cast(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 running_{true}; + std::thread receive_thread_; + + // 统计信息 + std::atomic ahrs_count_; + std::atomic body_acc_count_; + + // IMU 时间戳同步 + bool imu_time_initialized_ = false; + rclcpp::Time imu_base_time_; + rclcpp::Time latest_imu_timestamp_; + + rclcpp::Publisher::SharedPtr ahrs_pub_; + rclcpp::Publisher::SharedPtr body_acc_pub_; + rclcpp::Publisher::SharedPtr g_force_pub_; + rclcpp::Publisher::SharedPtr connection_status_pub_; + rclcpp::TimerBase::SharedPtr status_timer_; +}; + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + if (rclcpp::ok()) { + rclcpp::spin(node); + } + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/uart_transmitter_node.cpp b/src/uart_transmitter_node.cpp new file mode 100644 index 0000000..9413ffd --- /dev/null +++ b/src/uart_transmitter_node.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include + +// 帧定义 +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( + "transmitter/connection_status", 10); + + // 创建定时器 - 发送数据 + auto send_period = std::chrono::duration(1.0 / send_frequency_); + send_timer_ = this->create_wall_timer( + std::chrono::duration_cast(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(this->get_parameter("x_move").as_int()); + int16_t y_move = + static_cast(this->get_parameter("y_move").as_int()); + int16_t yaw = static_cast(this->get_parameter("yaw").as_int()); + int16_t pitch = static_cast(this->get_parameter("pitch").as_int()); + int16_t feed = static_cast(this->get_parameter("feed").as_int()); + uint8_t left_switch = + static_cast(this->get_parameter("left_switch").as_int()) & + 0x0F; + uint8_t right_switch = + static_cast(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 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::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(); + + if (rclcpp::ok()) { + rclcpp::spin(node); + } + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/tool/99-usb-serial.rules b/tool/99-usb-serial.rules new file mode 100644 index 0000000..a798e46 --- /dev/null +++ b/tool/99-usb-serial.rules @@ -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" \ No newline at end of file diff --git a/tool/setup_usb_permissions.sh b/tool/setup_usb_permissions.sh new file mode 100755 index 0000000..fdac62e --- /dev/null +++ b/tool/setup_usb_permissions.sh @@ -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 "设置完成，请重新插拔设备" \ No newline at end of file diff --git a/transmitter_sdk/example.c b/transmitter_sdk/example.c deleted file mode 100644 index e69de29..0000000 diff --git a/transmitter_sdk/inc/pub_user.h b/transmitter_sdk/inc/pub_user.h deleted file mode 100755 index 27fc139..0000000 --- a/transmitter_sdk/inc/pub_user.h +++ /dev/null @@ -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 - -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 \ No newline at end of file diff --git a/transmitter_sdk/lib/linux/arm64/libdm_device.so b/transmitter_sdk/lib/linux/arm64/libdm_device.so deleted file mode 100755 index ed22cbb..0000000 Binary files a/transmitter_sdk/lib/linux/arm64/libdm_device.so and /dev/null differ diff --git a/transmitter_sdk/lib/linux/x64/libdm_device.so b/transmitter_sdk/lib/linux/x64/libdm_device.so deleted file mode 100755 index 3727af8..0000000 Binary files a/transmitter_sdk/lib/linux/x64/libdm_device.so and /dev/null differ