NCLink ROS 2 Bridge

这是针对 Ti/eletr_comp 飞控 NCLink 协议的 ROS 2 桥接和地面站工程。桥接节点通过 UART/USB 串口接入飞控，地面站监听桥接 Topic 进行可视化，并通过 ROS 2 Topic/Service 下发控制命令。

构建

当前环境已验证 ROS 2 Jazzy、Python 3.12 和 colcon 可用：

source /opt/ros/jazzy/setup.bash
sudo apt install python3-pyqt5 python3-pyqt5.qtsvg
python3 -m pip install --user pyserial
colcon build --symlink-install
source install/setup.bash

Humble 边缘计算机部署注意事项

如果诊断信息出现 pyserial is not installed，需要在运行 ROS 2 节点的同一个 Python 环境中安装串口依赖：

sudo apt install python3-serial
python3 -c "import serial; print(serial.__version__)"

如果 /nclink_bridge/diagnostics 中交替出现不同串口，例如 /dev/ttyUSB0 和 /dev/ttyTHS3，说明有多个旧的 nclink_bridge 进程同时运行。先查看并停止旧进程：

pgrep -af 'nclink_bridge|ros2 launch'
kill <旧进程PID>

同一个 Topic 存在不同版本的消息发布者时，ros2 topic echo 可能出现 sequence size exceeds remaining buffer。停止旧进程后，若问题仍存在，清理本工作区中两个包的构建产物并重新生成接口：

cd ~/dev/NUEDC
rm -rf build/nclink_msgs build/nclink_ros2_bridge \
       install/nclink_msgs install/nclink_ros2_bridge
source /opt/ros/humble/setup.bash
colcon build --symlink-install --packages-select nclink_msgs nclink_ros2_bridge
source install/setup.bash

确认只有一个发布者：

ros2 topic info /nclink_bridge/diagnostics -v

节点与模块职责

当前工程的生产运行时包含两个 ROS 2 节点：nclink_bridge 和 nclink_ground_station；另提供任务示例和场地遥测测试节点。消息接口包和普通 Python 模块不会单独启动为 ROS 2 节点。

名称	类型	作用
nclink_bridge	ROS 2 节点	管理飞控串口连接，接收并解析 NCLink 遥测，发布 ROS 2 Topic；同时订阅控制 Topic、处理 Service 请求并发送 NCLink 控制帧。
nclink_ground_station	ROS 2 + PyQt5 节点	订阅桥接节点的遥测，提供仪表盘、姿态/位置可视化、起飞前检查、校准指导、参数表和指令中心。通过桥接节点的 Topic/Service 下发控制。
field_telemetry_demo	ROS 2 测试节点	同时发布无人机 telemetry/pos 和小车 telemetry/carstate，用于验证协同场地页面；不连接或控制真实设备。
nclink_msgs	ROS 2 接口包	提供飞行状态、原始传感器、GPS、RC、PID、导航控制以及参数读写所需的自定义消息和 Service，不负责运行逻辑。
protocol.py	协议模块	实现 NCLink 组帧、解帧、异或校验、流式解析、字段缩放和大小端转换。
serial_transport.py	串口模块	在独立线程中执行串口读写、发送队列、串口断线检测和自动重连，避免阻塞 ROS 2 回调。
nclink_bridge.launch.py	启动文件	加载 YAML 参数并启动 nclink_bridge 节点。

nclink_bridge 内部的数据流如下：

飞控串口
   │
   ├── serial_transport.py：读写与重连
   │
   ├── protocol.py：NCLink 解析与编码
   │       │
   │       ├── 遥测 → /nclink_bridge/telemetry/*
   │       └── ROS 2 命令/Service → 控制帧 → 飞控
   │
   └── nclink_ground_station：监听 Topic、可视化和控制面板

地面站功能

nclink_ground_station 按功能分为八个板块：

• 总览仪表盘：姿态卡片、航向/电池/GPS 状态、人工地平线、起飞模式选择和起飞/降落入口。
• 姿态与位置：实时姿态仪、角速度显示、GPS 局部轨迹和飞控三维轨迹显示。
• 协同场地态势：以比赛 SVG 为固定底图，同时显示无人机实时位置/航向/轨迹和小车 A→B→C→D→A 的循迹位置、速度、里程与姿态。
• 遥测数据：集中查看桥接遥测以及场地无人机、小车状态。
• 起飞前检查：通信链路、状态新鲜度、电池电压、未解锁、GPS Fix、RC 输入六项检查；全部通过前起飞授权按钮保持禁用。
• 校准指导：陀螺仪/加速度计、磁力计和结果确认步骤，带安全确认、开始/停止指令和飞控 ACK 反馈。
• 参数中心：12 个基础参数、6 组 PID、0~199 预留参数表，支持读取、写入、恢复默认和恢复出厂入口。
• 指令中心：虚拟遥控、相对移动、导航/速度、GPS 目标点、三维目标、预留参数和校准指令分板块发送。

起飞流程由地面站执行安全门控：先接收有效状态和 RC，再确认链路、电压、GPS 和未解锁状态；用户二次确认后才发布 unlock=1、takeoff=1 的 RC 指令。具体飞行模式仍由飞控 RC/协议定义，界面中的模式用于任务流程标记。

消息与接口含义

飞控与场地遥测 Topic

Topic	消息类型	含义
~/telemetry/status	nclink_msgs/FlightStatus	飞行姿态、角速度、IMU 温度、电池电压、飞行模式和解锁状态。姿态单位为度，角速度为度/秒，电压为伏特。
~/telemetry/sensor_raw	nclink_msgs/RawSensor	原始加速度、原始陀螺仪和磁场数据。加速度/陀螺仪保持飞控原始计数，协议没有提供标定系数。
~/telemetry/rc	nclink_msgs/RcChannels	飞控当前收到的 8 路遥控通道值，以及 unlock、takeoff 状态。
~/telemetry/gps	sensor_msgs/NavSatFix	GPS 经度、纬度、高度和基本定位状态；经纬度为度，高度为米。完整 PDOP、定位类型和卫星数见 gps_raw。
~/telemetry/gps_raw	nclink_msgs/GpsTelemetry	完整 GPS 帧字段：经纬度、高度、PDOP、定位类型和卫星数。
~/telemetry/observation_ne	nclink_msgs/FusionState	水平观测数据，values 顺序为水平位置和水平速度，具体坐标轴由飞控 NE 坐标定义。
~/telemetry/observation_uop	nclink_msgs/FusionState	垂直/超声波/光流观测数据，values 对应气压高度、超声波高度和光流速度字段。
~/telemetry/fusion_ne	nclink_msgs/FusionState	水平融合位置、速度和加速度；values 顺序为位置 N/E、速度 N/E、加速度 N/E。
~/telemetry/fusion_u	nclink_msgs/FusionState	垂直融合位置、速度和加速度；values 顺序为高度、垂直速度、垂直加速度。
~/telemetry/user	nclink_msgs/UserData	NCLink 0x09 USER 数据；当前固件发送滚转、俯仰、偏航及其观测值，共 6 个浮点数。
~/telemetry/calibration	nclink_msgs/CalibrationTelemetry	校准相关遥测帧（0x11~0x15）及其浮点数组；用于查看飞控返回的校准数据。
~/telemetry/pid	nclink_msgs/PidGroup	一组 3 个 PID 控制器的 9 个参数，values 顺序为每个 PID 的 Kp、Ki、Kd。
~/telemetry/parameters	nclink_msgs/FlightParameters	高度限制、安全电压、速度限制、串口功能、避障等 12 个飞控参数。
~/telemetry/reserved_parameter	nclink_msgs/ReservedParameter	预留参数编号和浮点值；地面站用它填充 0~199 预留参数表。
~/telemetry/track3d	nclink_msgs/Track3D	三维位置和姿态四元数 q0~q3。位置为飞控协议坐标，四元数由 NCLink 定标。
~/telemetry/pos	nclink_msgs/DronePosition	场地无人机位置：x/y/z 为以 H 点为原点的局部米制坐标，yaw 为弧度。该 Topic 由定位节点或测试节点发布，不由串口桥自动生成。
~/telemetry/carstate	nclink_msgs/CarState	循线小车状态：speed 单位 m/s，mileage 是从 A 点起沿 A→B→C→D→A 的累计里程，单位 m。该 Topic 由小车状态节点或测试节点发布。
~/telemetry/ack	nclink_msgs/Ack	飞控对控制命令的应答，response 为 NCLink 应答码，response_name 为可读名称。
~/telemetry/raw_frame	std_msgs/ByteMultiArray	原始完整 NCLink 帧，仅在 publish_raw_frame=true 时发布。
~/diagnostics	nclink_msgs/NcLinkDiagnostics	串口连接状态、收发帧数量、校验错误数量、未知帧数量和收发频率。

地面站的“遥测数据”页面会集中显示上述已接入的状态、原始传感器、RC、GPS、观测量、融合量、USER、校准、PID、参数、预留参数、三维轨迹、ACK、诊断和可选原始帧。当前下位机的光流观测帧只包含气压高度、下视测距和 P/R 方向速度；原始光流积分、valid、quality 等字段没有在 NCLink 帧中发送，因此无法仅靠地面站补出。

其中 FusionState.axis 的取值为 0 或 1，0 表示水平 NE 方向，1 表示垂直 U 方向。消息固定提供 6 个 values 元素：观测帧实际发送 4 个值、垂直融合帧实际发送 3 个值，不足部分由桥接层补 0.0；水平融合帧发送完整 6 个值。协议中部分观测/融合字段使用飞控源码定义的缩放值，桥接层会完成协议缩放，但不会改变坐标系方向。

协同场地坐标约定

场地左下角为场地坐标 (0, 0)，单位为米；图纸右向为 +x，图纸上向为 +y。SVG 的 y 轴向下，因此显示层统一使用 svg_x=100*x、svg_y=500-100*y 完成转换。图中关键点的场地坐标为：

点位	场地坐标 (m)
A	(1.500, 2.000)
B	(1.500, 3.500)
C	(3.000, 3.500)
D	(3.000, 2.000)
H	(1.125, 1.125)

DronePosition 采用 H 点局部坐标：消息 (0,0) 正好显示在 H，yaw=0 朝图纸右方，正值逆时针旋转。CarState.mileage=0 对应 A；两段 1.5 m 直线和两个半径 0.75 m 的半圆组成一圈，总长 3+1.5π=7.71238898 m。显示逻辑会将负里程限制在 A，并将达到或超过一圈的里程限制在终点 A，标记任务完成。

ROS 2 控制 Topic

Topic	消息类型	含义
~/cmd/rc	nclink_msgs/RcCommand	发送 10 路遥控通道和解锁/起飞标志。需要持续发布，默认超过 300 ms 未更新会发送一次失效保护帧。
~/cmd/move	nclink_msgs/MoveCommand	SDK 相对移动命令。direction：0 前、1 后、2 左、3 右、4 上、5 下；distance_cm 为距离厘米数。
~/cmd/nav_ctrl	nclink_msgs/NavControlCommand	发送导航或速度控制命令，包括序号、XYZ、导航模式、坐标系标志和执行时长。速度模式下遵循飞控源码中的 ROS 坐标转换。
~/cmd/calibration	nclink_msgs/CalibrationCommand	传感器校准控制，字段为校准标志、步骤和命令码。
~/cmd/gps_point	nclink_msgs/GpsPointCommand	发送目标 GPS 点，经纬度单位为度。
~/cmd/track3d	nclink_msgs/Track3DCommand	发送三维位置、四元数和更新标志。
~/cmd/reserved_parameter	nclink_msgs/ReservedParameterCommand	写入指定编号的预留浮点参数。

Service 接口

Service	类型	含义
~/get_status	nclink_msgs/GetStatus	按 command 请求 PID、参数或预留参数。常用命令：1 读 PID，3 读参数，6 读指定预留参数，7 请求全部预留参数。
~/get_fc_parameters	nclink_msgs/GetParameters	reset_to_default=false 读取当前参数；为 true 请求恢复默认参数。
~/get_reserved_parameter	nclink_msgs/GetReservedParameter	根据 param_id 读取预留参数。
~/set_fc_parameters	nclink_msgs/SetParameters	一次写入 12 个飞控基础参数。
~/set_pid	nclink_msgs/SetPid	写入一组 PID；group 使用逻辑编号 0~5，每组包含 9 个 Kp、Ki、Kd 值。

启动

ros2 launch nclink_ros2_bridge nclink_bridge.launch.py \
  serial_port:=/dev/ttyUSB0 baudrate:=460800

另开一个终端启动地面站：

source /opt/ros/jazzy/setup.bash
source install/setup.bash
ros2 launch nclink_ground_station nclink_ground_station.launch.py

地面站默认监听 /nclink_bridge。如果桥接节点使用了其他命名空间，可通过参数覆盖：

ros2 run nclink_ground_station nclink_ground_station \
  --ros-args -p bridge_namespace:=/my_bridge

地面站起飞前定位检查可通过 position_source 选择：gps 要求 GPS Fix， optical_flow 适用于光流+下视测距，slam 适用于 T265/激光雷达 SLAM，none 不检查位置源。 当前配置默认为 optical_flow；光流和测距是否真的有效仍由飞控传感器状态保证。

不接飞控也可以先查看界面和可视化效果：

ros2 run nclink_ground_station nclink_ground_station \
  --ros-args -p demo_mode:=true

demo_mode 只生成本地演示数据，不会向飞控发送控制命令；接入真实桥接节点时保持为 false。

若要按真实 ROS 2 Topic 验证新页面，另开终端运行双话题演示发布器：

source /opt/ros/jazzy/setup.bash
source install/setup.bash
ros2 run nclink_ground_station field_telemetry_demo

它以 10 Hz 发布 /nclink_bridge/telemetry/pos 和 /nclink_bridge/telemetry/carstate：无人机首帧位于 H 并起飞，小车从 A 出发，完成一圈后在 A 停止。可通过参数改变命名空间和小车速度，例如：

ros2 run nclink_ground_station field_telemetry_demo --ros-args \
  -p bridge_namespace:=/my_bridge -p car_speed_m_s:=0.8

节点默认使用 460800 8N1，会自动重连串口。所有 Topic 和 Service 都位于 /nclink_bridge 命名空间下，例如：

ros2 topic echo /nclink_bridge/telemetry/status
ros2 topic echo /nclink_bridge/telemetry/sensor_raw
ros2 topic echo /nclink_bridge/telemetry/gps
ros2 topic echo /nclink_bridge/telemetry/pos --qos-reliability best_effort
ros2 topic echo /nclink_bridge/telemetry/carstate --qos-reliability best_effort
ros2 topic echo /nclink_bridge/diagnostics

遥测和诊断 Topic 使用 BEST_EFFORT QoS，地面站已使用相同配置订阅。手工查看遥测时也应指定：

ros2 topic echo /nclink_bridge/telemetry/status --qos-reliability best_effort

控制命令使用以下 Topic：

/nclink_bridge/cmd/rc
/nclink_bridge/cmd/move
/nclink_bridge/cmd/nav_ctrl
/nclink_bridge/cmd/calibration
/nclink_bridge/cmd/gps_point
/nclink_bridge/cmd/track3d
/nclink_bridge/cmd/reserved_parameter

读取和写入飞控参数使用 /nclink_bridge/get_fc_parameters、 /nclink_bridge/get_reserved_parameter、/nclink_bridge/set_fc_parameters 和 /nclink_bridge/set_pid。get_fc_parameters、set_fc_parameters 特意避开 ROS 2 节点自带的 /get_parameters、/set_parameters 服务名。

sensor_raw 对应飞控 NCLink 0x02 原始传感器帧；加速度和陀螺仪字段保持飞控原始 计数，磁场字段按协议的 raw * 1000 反向还原。由于飞控协议没有提供单位和标定系数， 节点不会将它们伪装成 SI 单位的 sensor_msgs/Imu。

需要抓取原始 NCLink 帧时，将配置中的 publish_raw_frame 设为 true，节点会发布 /nclink_bridge/telemetry/raw_frame (std_msgs/ByteMultiArray)。

控制安全

RC 命令需要持续发布；桥接默认 300 ms 没有收到新的 RC 命令后，只发送一次解锁/起飞均为 0 的失效保护帧。地面站点击“开始/更新持续 RC”或起飞授权后会以 20 Hz 持续发布，点击“降落 / 撤销授权”会停止并发送锁定帧。节点不会自行发送解锁或起飞命令。首次实机测试应断开电机或使用安全架。

解锁方式是 NCLink RC 帧中的 unlock=1，不是单独的 ROS 2 解锁服务。地面站“虚拟遥控”勾选“解锁”后点击“开始/更新持续 RC”，或者通过起飞前检查后点击起飞授权，都会持续发送 unlock=1；下位机收到后将 unlock_flag 置为 1 并进入解锁控制状态。当前下位机代码虽然接收 takeoff 字段，但没有使用 takeoff_flag 参与控制，实际解锁由 unlock_flag 控制，油门和飞行模式仍由 RC 通道决定。

协议来源

协议字段以飞控端 FCS/WP_Src/NCLink.c 为准，尤其是方向帧头、大小端、缩放系数和 0x17 三维轨迹帧。GS/nclink.py 作为 Linux 端实现参考。

测试

不依赖 ROS 运行时的协议测试：

cd src/nclink_ros2_bridge
python3 -m unittest -v

场地几何、坐标转换和地面站测试：

source /opt/ros/jazzy/setup.bash
source install/setup.bash
python3 -m unittest -v src/nclink_ground_station/test_ground_station.py