duduuu.xyz

[DeepSeek-V4正式版灰度测试]Claude Code中一句话生成网页版我的世界游戏

Sat, 18 Jul 2026 00:00:00 GMT

7月17日，根据笔者帐号API的特征，确定已经被灰度测试DeepSeek-V4正式版，故模仿前期网络上的相关测试，使用简单prompt生成我的世界网页版游戏，测试该模型的“许愿式编程”和Plan-Build-Debug能力，验证网络上的测试是否为炒作。在文末，笔者将相关代码部署到了服务器，大家可以在浏览器进行试玩进行评价。

整体情况及配置

环境： Claude Code 2.1.117 + DeepSeek-V4-Pro + max 思考强度
提示词：

为我还原一个网页版我的世界，越还原越好。如果有不确定的地方请进行联网搜索
手动操作： Plan环节接受所有“推荐”的建议，不进行额外输入
费用及Tokens：
- 费用总消耗：1.78RMB（实际可能更少，同时间笔者在进行其他科研任务，仍在使用缓存命中较低的预览版）
- Token(缓存命中-未命中-输出）:56105280-329411-225048
- 缓存命中率：99.42%

测评体验

思维链密度更高，整段内容更充实，多为"I'm doing"结构（这也是用来验证正式版API的一个标准），与之相比，预览版的提示词多为"Let me"结构，且段落较短；思维链不再像预览版在思维链写代码。
工具调用和Skill触发更多，在Plan阶段很快的触发了brainstorming和wrting-plans，执行阶段会开subagent（同时，有部分测试者反应执行阶段思维链会切换回"Let me"结构，但笔者测试没有出现，我认为为模型路由不稳定导致）
思维链返回会等待一轮思考结束完全返回，而非预览版一段一段返回，体现在Claude Code里就是thinking时间过长，一度让笔者以为卡了，不过最终思维过程输出很严谨
出现编译过程timeout的情况，不再像预览版多轮命令检查一直timeout，会主动检查环境配置和依赖问题，Debug能力增强
虽然思维链密度很大，但是没有出现雷霆大思考和左右脑互搏，后训练效果极佳
缓存命中率超级无敌高，单论本项目的缓存命中率，有可能接近99.7%，正式版上线调价后大概率价格还更低

注：在整个过程中，笔者进行过一次手动输入，那是因为笔者忘了我的世界怎么操作了，以为下面的物品框是空的是bug，最后DeepSeek为我加了一个操作说明

测试视频

视频 1：测试生成视频

视频 2：游戏画面截图

试玩地址

https://duduuu.xyz/mc/

总结

因为号被A畜封了，笔者无法给出与Opus 4.8的比较。但是，在“许愿式编程”的流畅实现上，DeepSeek-V4已经越来越近。同时，在笔者的科研任务（有很多软硬件的Debug）中，这版模型已经强于笔者使用过的所有模型。至于缓存命中率，已经无人可敌。也许低价高质的AI模型即将触手可及。

声明：本测评仅代表个人观点。

ROS2+PX4 Real-Hardware: UXRCE-DDS Deployment (Pixhawk 6C)

Thu, 16 Jul 2026 00:00:00 GMT

Note

This section involves relatively complex operations. If anything is unclear, jump to the companion video tutorial at the end of the article.

Introduction

In our previous simulation series and MAVROS2 real-hardware articles, we covered simulation environment setup and MAVROS2-based offboard control. Starting from PX4 v1.14, the official recommendation is to use uXRCE-DDS middleware instead of MAVROS as the communication bridge between ROS2 and PX4.

This article takes the Pixhawk 6C (official flight controller) and Nvidia Jetson Orin NX (companion computer) as an example, providing a complete walkthrough of deploying the uXRCE-DDS middleware on real hardware. Topics include:

uXRCE-DDS architecture (Client/Agent model, DDS Global Data Space)
PX4 firmware flashing (command-line make upload + QGC ground station)
Flight controller serial port parameter configuration and uXRCE-DDS Client activation
Companion computer WiFi hotspot auto-start configuration
Agent startup and connectivity testing
Troubleshooting common issues

Prerequisites: This article assumes you have completed the simulation environment setup per the ROS2+PX4 Simulation Environment Development Tutorial, and have flashed Ubuntu 22.04 onto your companion computer per ROS2 Hands-On: Flashing the Companion Computer. If this is your first encounter with PX4 on real hardware, we recommend reading Controlling PX4 Drones with a Companion Computer (MAVROS2) first to understand the basic real-hardware workflow.

uXRCE-DDS Architecture

Why Replace MAVROS with DDS?

During the ROS1 era and early ROS2 days, MAVROS was the standard solution for connecting ROS with PX4 flight controllers. However, MAVROS is essentially a protocol bridge — it "translates" the MAVLink protocol from the flight controller into ROS topics. This means:

You must publish/subscribe to topics under the dedicated /mavros namespace (e.g., /mavros/setpoint_position/local)
A centralized MAVROS node is required to maintain all communication mappings (equivalent to ROS_Master in the ROS1 era)
Scalability is limited in multi-drone scenarios

uXRCE-DDS takes a fundamentally different approach. It is built on DDS (Data Distribution Service) — a decentralized real-time data distribution protocol that directly maps PX4's internal uORB topics to ROS2 topics. The benefits:

Native ROS2 topics: Sensor data from the flight controller is published directly as standard ROS2 topics, with no MAVROS translation layer. Offboard control code written during simulation can be transferred to real hardware without changing a single topic name.
Decentralized architecture: All ROS2 nodes automatically discover each other within the same DDS data space — no single point of failure (no ROS1-era Master node).
Low latency: DDS uses UDP multicast and shared memory for data transmission, with latency far lower than MAVLink serialization overhead.

Client/Agent Communication Model

uXRCE-DDS adopts a Client-Agent architecture:

Flight Controller Side (Client): Runs inside PX4, responsible for serializing uORB topic messages into DDS format. It does exactly one thing — bridge the flight controller's internal uORB messages to the Agent. The Client itself does not participate in DDS discovery or maintain any topology.

Companion Computer Side (Agent): Runs Micro-XRCE-DDS-Agent (the same one we launched with MicroXRCEAgent udp4 -p 8888 during simulation), responsible for:

Receiving serialized uORB messages from the Client
Publishing them into ROS2's DDS Global Data Space
Receiving ROS2 control commands from the DDS space and forwarding them to the Client

DDS Global Data Space: As long as ROS2 nodes run under the same ROS_DOMAIN_ID, they automatically discover each other's topics, services, and actions. No manual connection configuration is needed — publishers and subscribers are matched automatically.

The Role of ROS_DOMAIN_ID

DDS treats all nodes using the same ROS_DOMAIN_ID on the same local network as being "in the same data space." Check your current domain ID:

echo $ROS_DOMAIN_ID

If the output is empty, the default value is 0. In multi-drone formation scenarios, you can assign different domain IDs to each drone-companion-computer pair for isolated communication. As long as the domain ID is consistent and the nodes are on the same local network, ROS2 nodes discover each other automatically — this is the core advantage of the decentralized architecture.

Hardware Requirements

Required hardware:

Pixhawk 6C (or other PX4-native flight controller, firmware version ≥ v1.14)
Nvidia Jetson Orin NX (or other companion computer, running Ubuntu 22.04)
USB-TypeC cable (connect flight controller to local computer for flashing and parameter configuration)
Dupont wires ×3 (connect flight controller TELEM2 to companion computer UART)
Companion computer power cable (12V)
Flight controller battery
Telemetry radio module (for ground station; optional but strongly recommended)

Firmware version requirement: The uXRCE-DDS middleware is only included in PX4 source code v1.14 and above. The latest stable version has reached v1.16 (v1.17 appeared as of July 2026); we recommend using the latest version.

Flashing PX4 Firmware

Ensure the flight controller firmware version is ≥ v1.14. If your existing firmware meets this requirement, you may skip this section. Otherwise, choose one of the two methods below to re-flash.

Method 1: Command-Line Build & Upload (Recommended)

Clone the PX4 source and compile + upload. This method suits readers who already have a PX4 development environment set up.

cd ~/PX4-Autopilot

Before flashing, confirm the board target for your flight controller. The target for Pixhawk 6C is px4_fmu-v6c:

# List all available build targets
make list_config_targets | grep -i "v6c"

After confirming the flight controller is connected to your computer via USB-TypeC, check the serial port:

ls -l /dev/ttyACM*
# Should output something like /dev/ttyACM0

Use make upload to compile and flash in one step:

make px4_fmu-v6c upload

The terminal will show build progress. After completion, the flight controller reboots automatically. You can verify a successful flash by checking the NSH (NuttShell) messages:

# Check serial port settings; confirm baud rate is 115200
stty -F /dev/ttyACM0 115200 raw -echo
cat /dev/ttyACM0
# Press Enter; seeing the nsh> prompt means the flight controller is running normally

Common issue: If make upload reports a permission error, run sudo usermod -a -G dialout $USER and log in again.

Method 2: QGroundControl Ground Station Flashing

If you prefer not to compile from the command line, you can flash a pre-built firmware directly using QGC.

Open QGroundControl and connect the flight controller via USB
Go to "Vehicle Setup" → "Firmware"
Unplug the USB and plug it back in; QGC will automatically detect the flight controller model
Confirm the displayed version is ≥ v1.14, then click "OK" to start flashing
Wait for the progress bar to complete

Note: Ground station flashing resets all parameters. Export a backup first if you have previously configured parameters.

Flight Controller Serial Port Parameter Configuration

This section is the most error-prone part of the deployment — we need to accurately understand the flight controller's internal serial port naming and mapping.

Serial Port Mapping

The Pixhawk 6C has three TELEM ports, each corresponding to a different TTY device internally:

| Physical Port | Internal TTY | Default Use | Default Baud Rate | |--------------|-------------|-------------|-------------------| | TELEM1 | /dev/ttyS5 | Telemetry (MAVLink) | 57600 | | TELEM2 | /dev/ttyS3 | Companion Computer | 921600 | | TELEM3 | /dev/ttyS0 | Spare | — |

This article uses TELEM2 to connect to the companion computer. The TELEM2 6-pin layout is: VCC (5V), TX, RX, PWM, GPIO, GND. We only need three wires:

| TELEM2 Pin | Connects To | Jetson Orin NX UART2 Pin | |-----------|-------------|--------------------------| | TX | → | Pin 10 (RX) | | RX | → | Pin 8 (TX) | | GND | → | Pin 6 (GND) |

Do NOT connect VCC! The flight controller and companion computer each have independent power supplies.

Different companion computers have different UART pinouts and TTY device names. For the Jetson Orin NX:

| UART | TX Pin | RX Pin | GND Pin | TTY Device | |------|--------|--------|---------|------------| | UART1 (default debug console) | Pin 8 | Pin 10 | Pin 6 | /dev/ttyTHS0 | | UART2 | Pin 12 | Pin 14 | Pin 6 | /dev/ttyTHS1 |

We use UART2 (/dev/ttyTHS1) in this article, reserving UART1 as the debug console. Please consult the pinout diagram for your specific companion computer model.

Entering the Flight Controller Terminal via NSH

With the flight controller connected to your local computer via USB, use the mavlink_shell tool to enter the NSH terminal:

cd ~/PX4-Autopilot
./Tools/mavlink_shell.py /dev/ttyACM0:9600

Note: The baud rate for entering NSH is 9600 (not 115200 nor 921600). If QGC has occupied the serial port, close the ground station first.

After entering, you should see the nsh> prompt. First, check the current MAVLink instance allocation with mavlink status:

nsh> mavlink status
instance #0:
    TX queue: ...
    ...
    UART: /dev/ttyS5, 57600 baud    ← TELEM1 (Telemetry)
instance #1:
    ...
    UART: /dev/ttyS3, 921600 baud   ← TELEM2 (Companion Computer)
instance #2:
    ...
    UART: /dev/ttyACM0, 9600 baud   ← USB (Current Connection)

You can see the flight controller has automatically assigned MAVLink instance #1 to TELEM2 at 921600 baud. However, we need TELEM2 to carry uXRCE-DDS instead of MAVLink, otherwise the two will conflict.

Disabling the MAVLink Instance on TELEM2

Check the current MAVLink parameter configuration:

nsh> param show MAV_1_CONFIG
MAV_1_CONFIG = 102    ← 102 corresponds to TELEM2

nsh> param show MAV_1_MODE
MAV_1_MODE = 0        ← 0 = Normal (MAVLink)

The parameter value 102 is the hardware port identifier for TELEM2. We need to:

Set MAV_1_CONFIG to 0 (disable this MAVLink instance)
Stop the currently running MAVLink instance on TELEM2 (ttyS3)

# 1. Disable MAVLink on TELEM2
param set MAV_1_CONFIG 0

# 2. Stop the running MAVLink instance on ttyS3
mavlink stop -d /dev/ttyS3

Check mavlink status again — only two instances should remain (TELEM1 + USB), with TELEM2's MAVLink now stopped.

Enabling the uXRCE-DDS Client

Now start the uXRCE-DDS Client on TELEM2:

nsh> uxrce_dds_client start -d /dev/ttyS3 -b 921600

Parameter explanation:

-d /dev/ttyS3: Specifies the serial device corresponding to TELEM2
-b 921600: Baud rate; must match MAV_1_RATE

Optional namespace parameter: Adding -n <namespace> prefixes all published topics with a namespace (e.g., /px4_1/fmu/...). This is very useful in multi-drone scenarios to distinguish between different flight controller instances. Single-drone setups can omit this.

Set the DDS configuration parameter to ensure the DDS Client binds to the correct port:

nsh> param set UXRCE_DDS_CFG 102

Why 102? This parameter specifies which hardware port the uXRCE-DDS Client should bind to. 102 = TELEM2 (the same port as MAV_1_CONFIG=102). The default value is 0 (unconfigured); it must be set explicitly.

Verify the Client is running:

nsh> uxrce_dds_client status
# Output should show "running" status, but not yet connected to the Agent ("not connected" is normal at this stage)

The flight controller configuration is now complete. To recap the key commands:

# Complete command sequence executed in the flight controller NSH
param set MAV_1_CONFIG 0                                             # Disable MAVLink on TELEM2
mavlink stop -d /dev/ttyS3                                           # Stop the existing MAVLink instance
uxrce_dds_client start -d /dev/ttyS3 -b 921600                       # Start uXRCE-DDS Client
param set UXRCE_DDS_CFG 102                                          # Bind DDS to TELEM2
uxrce_dds_client status                                              # Verify status

Hardware Wiring

Connect the TELEM2 TX, RX, and GND pins from the flight controller to the corresponding UART2 pins on the companion computer:

| TELEM2 Pin | Connects To | Jetson Orin NX UART2 Pin | |-----------|-------------|--------------------------| | TX | → | Pin 10 (RX) | | RX | → | Pin 8 (TX) | | GND | → | Pin 6 (GND) |

If you are using a USB-TTL adapter (similar to Method 1 in the MAVROS article), the companion computer side will use /dev/ttyUSB0. The following steps use a direct UART connection (/dev/ttyTHS1) as the example.

Companion Computer WiFi Hotspot Configuration

During actual flights, we cannot connect a monitor, keyboard, and mouse to the companion computer — all operations must be performed remotely via SSH. This requires the companion computer and your local computer to be on the same local network. The most reliable approach is to have the companion computer create its own WiFi hotspot, which your local computer connects to, enabling SSH via a fixed IP address.

Creating the Hotspot

Open a terminal on the companion computer (Jetson Orin NX):

nm-connection-editor

In the GUI that appears:

Click "+" → choose "Wi-Fi" → click "Create"
Connection name: choose any name, e.g., drone-hotspot
SSID: same as the connection name
Mode: select Hotspot
Wi-Fi Security: choose "WPA & WPA2 Personal", set a password of at least 8 characters
Switch to the IPv4 Settings tab, set Method to "Manual":
- Address: 192.168.10.1
- Netmask: 24
- Gateway: 192.168.10.1
IPv6 Settings: set Method to "Ignore"
Click "Save"

Setting the Hotspot to Auto-Start on Boot

First, delete previously saved WiFi connections (to prevent the companion computer from connecting to old WiFi networks on power-up):

# List all saved connections
nmcli connection show

# Delete unwanted connections (keep the hotspot you just created)
sudo nmcli connection delete "<old WiFi name>"

Set the hotspot to auto-connect:

nmcli connection modify "drone-hotspot" connection.autoconnect yes
nmcli connection modify "drone-hotspot" connection.autoconnect-priority 10

Activate the hotspot:

nmcli connection up "drone-hotspot"

Now, on your local computer, search for WiFi networks — you should see the drone-hotspot signal. After connecting, verify:

ping 192.168.10.1

From now on, each time: power the flight controller → power the companion computer → hotspot starts automatically → connect from local computer → SSH:

ssh jetson@192.168.10.1

Starting the Agent and Connectivity Testing

Pre-Connection Checks

After SSH-ing into the companion computer, do not power the flight controller yet. Perform the following preparation steps:

# 1. Add your user to the dialout group (prevents serial port permission issues)
sudo usermod -a -G dialout $USER
# Log out and back in for this to take effect

# 2. Check available serial devices
ls -l /dev/ttyTHS*
# Should output /dev/ttyTHS1 (UART2)

Confirm that Micro-XRCE-DDS-Agent is installed on the companion computer (it should already be present from your simulation environment; if not, refer to the MicroXRCE-DDS Agent installation section in the ROS2+PX4 Simulation Tutorial).

Verifying Hardware Connectivity

First, test whether the flight controller → companion computer serial communication is working. After powering the flight controller, use cat to check for a data stream:

stty -F /dev/ttyTHS1 921600 raw -echo
cat /dev/ttyTHS1

If you see garbled output (MAVLink or uXRCE-DDS binary data), the physical connection and serial port configuration are correct:

If there is no output, check:

Whether the flight controller TELEM2 TX ↔ companion computer UART RX are correctly cross-connected
Whether the baud rates match (both the flight controller and companion computer should use 921600)
Whether uxrce_dds_client start has been executed on the flight controller
Whether another process (e.g., ground station) is occupying the serial port

Starting Micro-XRCE-DDS-Agent

# Activate the ROS2 environment
source /opt/ros/humble/setup.bash

# Start the Agent, connecting to the flight controller via the serial port
MicroXRCEAgent serial --dev /dev/ttyTHS1 -b 921600

On a successful connection, the terminal will output something like:

[info] Serial agent starting...
[info] Connect to serial device: /dev/ttyTHS1, baudrate: 921600
[info] Connection established
[info] Client connected: 0x00000001
[info] Topic created: /fmu/out/vehicle_odometry
[info] Topic created: /fmu/out/imu
[info] Topic created: /fmu/out/...
...

The uXRCE-DDS middleware deployment is now complete. The Agent will automatically publish the flight controller's uORB topics as ROS2 topics. Verify in another terminal:

source /opt/ros/humble/setup.bash
ros2 topic list | grep fmu

You should see a large number of topics prefixed with /fmu/out/... carrying flight controller data. Your ROS2 offboard control code can now run directly on the companion computer, with topic names identical to those used in the simulation environment!

Complete Startup Sequence

Before each real flight, follow this sequence:

Connect your local computer to the companion computer's hotspot drone-hotspot
Confirm you can ping 192.168.10.1
Power the flight controller (via battery) and wait for it to finish booting
Confirm the ground station (QGC) is connected to the flight controller via telemetry
SSH into the companion computer: ssh jetson@192.168.10.1
In the SSH terminal, start the Agent:

source /opt/ros/humble/setup.bash
MicroXRCEAgent serial --dev /dev/ttyTHS1 -b 921600

Open a second SSH terminal and run the ROS2 control node:

source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
ros2 run offboard_control simple_offboard   # Using the earlier Offboard node as an example

Important tip: Based on experience, once the Agent establishes a successful connection, try to keep the flight controller powered and connected. Repeatedly disconnecting and reconnecting the serial port mid-flight sometimes causes subsequent connection failures. It is recommended to connect once when everything is ready and avoid changes in between.

Parameter Quick Reference

| Parameter | Value | Description | |-----------|-------|-------------| | MAV_1_CONFIG | 0 (disabled) | Decommission MAVLink on TELEM2 | | MAV_2_CONFIG | 101 (TELEM1) | Keep telemetry on TELEM1 | | UXRCE_DDS_CFG | 102 (TELEM2) | Bind DDS Client to TELEM2 | | SER_TEL2_BAUD | 921600 | TELEM2 baud rate |

uXRCE-DDS vs. MAVROS Comparison

| | uXRCE-DDS | MAVROS | |---|---|---| | PX4 Recommendation | Recommended since v1.14+ | Legacy, still maintained | | Topic Naming | Native ROS2 topics | /mavros/ namespace | | Architecture | Decentralized DDS | Centralized bridge | | Multi-Drone Scaling | DDS auto-discovery | Manual configuration required | | Sim-to-Real Code Reuse | Identical topics; no code changes needed | Topic prefix substitution required | | Stability | Under active development | Mature and stable (from ROS1 era) | | Real-Hardware Latency | Low (UDP multicast) | Moderate (MAVLink serialization) |

Recommendation: Prefer uXRCE-DDS for new projects. If your flight controller firmware version is < v1.14 and cannot be upgraded, or if you have a large body of legacy MAVROS code that needs compatibility, MAVROS remains a viable choice. See Controlling PX4 Drones with a Companion Computer (MAVROS2).

Troubleshooting Common Issues

| Symptom | Likely Cause | Troubleshooting | |---------|-------------|-----------------| | NSH terminal unreachable; serial port occupied | QGC or another process has occupied ACM0 | Close QGC ground station and retry | | uxrce_dds_client start reports an error | Baud rate mismatch or TELEM2 still in use by MAVLink | Confirm MAV_1_CONFIG=0 and that mavlink stop -d /dev/ttyS3 has been executed | | Agent always shows "not connected" after startup | UXRCE_DDS_CFG not correctly set | param set UXRCE_DDS_CFG 102 | | cat /dev/ttyTHS1 produces no output | Wiring error or baud rate mismatch | Check TX/RX cross-connection and that the baud rate is 921600 | | Agent is connected but ros2 topic list shows no /fmu/ topics | ROS2 environment not sourced, or domain ID mismatch | source /opt/ros/humble/setup.bash, check echo $ROS_DOMAIN_ID | | Serial device is not named ttyTHS1 | A different UART is in use | ls /dev/ttyTHS* to list available devices |

Summary

This article provided a complete walkthrough of deploying the uXRCE-DDS middleware on a Pixhawk 6C + Jetson Orin NX setup — covering firmware flashing, flight controller NSH terminal operations, MAVLink decommissioning and DDS Client activation, hardware wiring, companion computer hotspot configuration, and Agent startup. As the PX4-recommended ROS2 communication solution, uXRCE-DDS offers significant advantages in code portability (simulation code works directly on real hardware), scalability (automatic DDS discovery), and latency.

That said, MAVROS, as a time-tested solution, still holds value in terms of stability and community documentation. Readers can choose flexibly based on their flight controller firmware version and project requirements. If you experiment with both approaches and compare their performance, we welcome you to share your experience in the comments.

Thank you for reading! Corrections and feedback are always appreciated.

Companion Video Tutorial

If the video below loads slowly, watch it on Zhihu: https://www.zhihu.com/zvideo/1981084557504701099

References

ROS2+PX4 真机部署：UXRCE-DDS 中间件的部署（以 Pixhawk 6C 为例）

Thu, 16 Jul 2026 00:00:00 GMT

注意

本节操作较为复杂，如有不理解的地方可以跳转到文末的配套讲解视频进行辅助学习。

简介

在前面的仿真系列和 MAVROS2 真机文章中，我们分别介绍了仿真环境搭建和基于 MAVROS2 的真机Offboard 控制。PX4 从 v1.14 版本开始，官方推荐使用 uXRCE-DDS 中间件替代 MAVROS，作为 ROS2 与 PX4 之间的通信桥梁。

本文以 Pixhawk 6C（官方飞控板）和 Nvidia Jetson Orin NX（机载电脑）为例，完整演示如何在真机上部署 uXRCE-DDS 中间件。内容包括：

uXRCE-DDS 架构原理（Client/Agent 模型、DDS 全局数据空间）
PX4 固件烧录（命令行 make upload + 地面站两种方式）
飞控端串口参数配置与 uXRCE-DDS Client 启用
机载电脑 WiFi 热点自动启动配置
Agent 启动与连通性测试
常见问题排查

前提条件：本文假设读者已按照 ROS2+PX4 仿真环境开发教程完成仿真环境搭建，并按 ROS2 真机实践：为机载电脑刷机完成机载电脑的 Ubuntu 22.04 刷机。如果你是第一次接触 PX4 真机，建议先阅读使用机载电脑控制 PX4 无人机（MAVROS2）了解真机操作的基本流程。

uXRCE-DDS 架构原理

为什么用 DDS 替代 MAVROS？

在 ROS1 时代和 ROS2 早期，MAVROS 是连接 ROS 与 PX4 飞控的标准方案。但 MAVROS 本质上是一个协议桥接器——它将飞控端的 MAVLink 协议"翻译"成 ROS 话题，这意味着：

你需要发布/订阅 mavros 命名空间下的专用话题（如 /mavros/setpoint_position/local）
系统中需要一个中心化的 MAVROS 节点来维护所有通信映射（在ROS1时代的ROS_Master)
在多机场景中扩展性受限

uXRCE-DDS 采用完全不同的思路。它基于 DDS（Data Distribution Service）——一种去中心化的实时数据分发协议，让 PX4 内部的 uORB 话题直接映射为 ROS2 话题。这带来的好处：

原生 ROS2 话题：飞控的传感器数据直接以 ROS2 标准话题发布，无需 MAVROS 转译层。之前在仿真中写的 Offboard 控制代码可以直接搬到真机上用，话题名都不需要改。
分布式架构：所有 ROS2 节点在同一个 DDS 数据空间中自动发现彼此，不存在单点瓶颈（没有 ROS1 时代的 Master 节点）。
低延迟：DDS 使用 UDP 多播和共享内存进行数据传输，延迟远低于 MAVLink 串行化开销。

Client/Agent 通信模型

uXRCE-DDS 采用 Client-Agent 架构：

飞控端（Client）：运行在 PX4 内部，负责将 uORB 话题的消息序列化为 DDS 格式。它只做一件事——把飞控内部的 uORB 消息桥接到 Agent。Client 本身不参与 DDS 发现、不维护拓扑。

机载电脑端（Agent）：运行 Micro-XRCE-DDS-Agent（之前我们仿真中用 MicroXRCEAgent udp4 -p 8888 启动的那个），负责：

接收来自 Client 的串行化 uORB 消息
将它们发布到 ROS2 的 DDS 全局数据空间
从 DDS 空间接收 ROS2 控制指令并转发给 Client

DDS 全局数据空间（Global Data Space）：只要 ROS2 节点运行在同一个 ROS_DOMAIN_ID 下，它们就能自动发现彼此的话题、服务和 action。你不需要手动配置任何连接关系——发布者和订阅者自动匹配。

ROS_DOMAIN_ID 的作用

DDS 将同一个局域网内使用相同 ROS_DOMAIN_ID 的所有节点视为"在同一数据空间内"。可以用以下命令查看当前 domain ID：

echo $ROS_DOMAIN_ID

如果输出为空，默认值为 0。在多机编队场景中，你可以为每组无人机-机载电脑分配不同的 domain ID 来隔离通信。只要 domain ID 一致且在同一局域网下，ROS2 节点之间就能互相发现——这正是分布式架构的核心优势。

硬件准备

需要的硬件设备：

Pixhawk 6C（或其他 PX4 原生飞控板，固件版本 ≥ v1.14）
Nvidia Jetson Orin NX（或其他机载电脑，已刷 Ubuntu 22.04）
USB-TypeC 线（连接飞控到本地电脑，用于烧录和参数配置）
杜邦线 ×3（连接飞控 TELEM2 到机载电脑 UART）
机载电脑供电线（12V）
飞控供电电池
数传模块（用于地面站，可选但强烈建议保留）

飞控固件版本要求：uXRCE-DDS 中间件在 PX4 v1.14 及以上版本的源码中才包含。目前最新稳定版已到 v1.16（26年7月已出现v1.17），建议使用最新版本。

PX4 固件烧录

确保飞控固件版本 ≥ v1.14。如果已有的固件版本满足要求可以跳过本节，否则选择以下两种方式之一重新烧录。

方式一：命令行编译上传（推荐）

克隆 PX4 源码并编译上传。这种方式适合已经搭建好 PX4 开发环境的读者。

cd ~/PX4-Autopilot

在烧录之前，先确认飞控板对应的编译目标（board target）。Pixhawk 6C 的目标为 px4_fmu-v6c：

# 查看所有可用的编译目标
make list_config_targets | grep -i "v6c"

确认飞控通过 USB-TypeC 连接到电脑后，查看串口：

ls -l /dev/ttyACM*
# 应输出类似 /dev/ttyACM0

使用 make upload 一键编译并烧录：

make px4_fmu-v6c upload

# 查看串口参数，确认波特率为 115200
stty -F /dev/ttyACM0 115200 raw -echo
cat /dev/ttyACM0
# 按回车后出现 nsh> 提示符表示飞控正常运行

常见问题：如果 make upload 报权限错误，执行 sudo usermod -a -G dialout $USER 后重新登录。

方式二：QGroundControl 地面站烧录

如果不想通过命令行编译，也可以用 QGC 直接烧录预编译固件。

打开 QGroundControl，通过 USB 连接飞控
进入 "Vehicle Setup" → "Firmware" 页面
拔掉 USB 再重新插入，QGC 会自动识别飞控型号
确认显示的版本号 ≥ v1.14，点击 "OK" 开始烧录
等待进度条完成

注意：地面站烧录会重置所有参数，如果之前配置过参数请先导出备份。

飞控端串口参数配置

这一节是部署中最容易出错的环节——我们需要准确理解飞控内部串口的命名映射关系。

串口映射关系

Pixhawk 6C 有三个 TELEM 端口，每个端口在飞控 NSH 内部对应不同的 TTY 设备：

| 物理端口 | 飞控内部 TTY | 默认用途 | 默认波特率 | |---------|-------------|---------|-----------| | TELEM1 | /dev/ttyS5 | 数传（MAVLink） | 57600 | | TELEM2 | /dev/ttyS3 | 机载电脑 | 921600 | | TELEM3 | /dev/ttyS0 | 备用 | — |

本文使用 TELEM2 连接机载电脑。TELEM2 的六针引脚排列为：VCC(5V)、TX、RX、PWM、GPIO、GND。我们只需要接三根线：

| TELEM2 引脚 | 连接到 | Jetson Orin NX UART2 引脚 | |------------|--------|--------------------------| | TX | → | Pin 10 (RX) | | RX | → | Pin 8 (TX) | | GND | → | Pin 6 (GND) |

不要连接 VCC！飞控和机载电脑各自独立供电。

不同机载电脑的 UART 引脚和 TTY 设备名不同。以 Jetson Orin NX 为例：

| UART | TX 引脚 | RX 引脚 | GND 引脚 | TTY 设备 | |------|--------|--------|---------|---------| | UART1（默认调试口） | Pin 8 | Pin 10 | Pin 6 | /dev/ttyTHS0 | | UART2 | Pin 12 | Pin 14 | Pin 6 | /dev/ttyTHS1 |

本文使用 UART2（/dev/ttyTHS1），以保留 UART1 作为调试口。读者请根据自己的机载电脑型号查阅对应引脚图。

飞控通过 USB 连到本地电脑后，使用 mavlink_shell 工具进入飞控的 NSH 终端：

cd ~/PX4-Autopilot
./Tools/mavlink_shell.py /dev/ttyACM0:9600

注意：进入 NSH 的波特率是 9600（不是 115200 也不是 921600）。如果之前 QGC 占用了串口，需要先关闭地面站。

进入后出现 nsh> 提示符。先用 mavlink status 查看当前 MAVLink 实例分配情况：

nsh> mavlink status
instance #0:
    TX queue: ...
    ...
    UART: /dev/ttyS5, 57600 baud    ← TELEM1 (数传)
instance #1:
    ...
    UART: /dev/ttyS3, 921600 baud   ← TELEM2 (机载电脑)
instance #2:
    ...
    UART: /dev/ttyACM0, 9600 baud   ← USB (当前连接)

可以看到飞控已自动为 TELEM2 分配了 MAVLink 实例 #1，波特率 921600。但我们需要让 TELEM2 走 uXRCE-DDS 而不是 MAVLink，否则两者会冲突。

查看当前 MAVLink 参数配置：

nsh> param show MAV_1_CONFIG
MAV_1_CONFIG = 102    ← 102 对应 TELEM2

nsh> param show MAV_1_MODE
MAV_1_MODE = 0        ← 0 = Normal (MAVLink)

参数 102 是 TELEM2 的硬件端口编号。我们需要：

将 MAV_1_CONFIG 设为 0（禁用该 MAVLink 实例）
停止当前已在 TELEM2 (ttyS3) 上运行的 MAVLink 实例

# 1. 禁用 TELEM2 上的 MAVLink
param set MAV_1_CONFIG 0

# 2. 停止正在运行的 ttyS3 上的 MAVLink 实例
mavlink stop -d /dev/ttyS3

再次检查 mavlink status，应只剩两个实例（TELEM1 + USB），TELEM2 的 MAVLink 已停下。

启用 uXRCE-DDS Client

现在在 TELEM2 上启动 uXRCE-DDS Client：

nsh> uxrce_dds_client start -d /dev/ttyS3 -b 921600

参数说明：

-d /dev/ttyS3：指定 TELEM2 对应的串口设备
-b 921600：波特率，需与 MAV_1_RATE 保持一致

可选的命名空间参数：加 -n <namespace> 可在所有发布的话题前添加命名空间前缀（如 /px4_1/fmu/...），这在多机场景中区分不同飞控实例时非常有用。单机可以不指定。

设置 DDS 配置参数，确保 DDS Client 绑定正确的端口：

nsh> param set UXRCE_DDS_CFG 102

为什么设为 102？ 这个参数指定 uXRCE-DDS Client 应该绑定到哪个硬件端口。102 = TELEM2（与 MAV_1_CONFIG=102 对应同一个端口）。默认值为 0（未配置），需要显式设置。

验证 Client 已启动：

nsh> uxrce_dds_client status
# 输出应显示 running 状态，但此时尚未连接到 Agent（"not connected" 是正常的）

# 飞控 NSH 中执行的完整命令序列
param set MAV_1_CONFIG 0                                             # 禁用 MAVLink on TELEM2
mavlink stop -d /dev/ttyS3                                           # 停止现有 MAVLink 实例
uxrce_dds_client start -d /dev/ttyS3 -b 921600                       # 启动 uXRCE-DDS Client
param set UXRCE_DDS_CFG 102                                          # 绑定 DDS 到 TELEM2
uxrce_dds_client status                                              # 验证状态

硬件接线

将飞控 TELEM2 的 TX、RX、GND 三根杜邦线连接到机载电脑 UART2 对应引脚：

| TELEM2 引脚 | 连接到 | Jetson Orin NX UART2 引脚 | |------------|--------|--------------------------| | TX | → | Pin 10 (RX) | | RX | → | Pin 8 (TX) | | GND | → | Pin 6 (GND) |

如果你使用 USB-TTL 转接线（方式类似 MAVROS 文章中的方法一），则机载电脑端走 /dev/ttyUSB0。下面以 UART 直连（/dev/ttyTHS1）为例。

机载电脑 WiFi 热点配置

在实际飞行中，我们无法给机载电脑接显示器和键鼠，必须通过远程 SSH 操作。这就要求机载电脑和本地电脑在同一个局域网内。最可靠的方式是让机载电脑自己创建一个 WiFi 热点，本地电脑连接这个热点后通过固定 IP SSH 过去。

创建热点

在机载电脑（Jetson Orin NX）上打开终端：

nm-connection-editor

在弹出的图形界面中：

点击 "+" → 选择 "Wi-Fi" → 点击 "Create"
Connection name：任取，如 北航中法工程师学院是XX
SSID：与 Connection name 相同即可
Mode：选择 Hotspot（热点模式）
Wi-Fi Security：选择 "WPA & WPA2 Personal"，设置一个 8 位以上密码
切换到 IPv4 Settings 选项卡，Method 选择 "Manual"：
- Address: 192.168.10.1
- Netmask: 24
- Gateway: 192.168.10.1
IPv6 Settings：Method 选择 "Ignore"
点击 "Save"

设置开机自动启动热点

首先删除之前保存的 WiFi 连接记录（防止上电后自动连到其他 WiFi）：

# 列出所有已保存的连接
nmcli connection show

# 删除不需要的连接（保留刚创建的热点）
sudo nmcli connection delete "<旧 WiFi 名称>"

将热点设为自动连接：

nmcli connection modify "北航中法工程师学院是XX" connection.autoconnect yes
nmcli connection modify "北航中法工程师学院是XX" connection.autoconnect-priority 10

激活热点：

nmcli connection up "北航中法工程师学院是XX"

此时在本地电脑上搜索 WiFi，应能看到 北航中法工程师学院是XX 信号。连接后验证：

ping 192.168.10.1

之后每次飞控上电 → 机载电脑供电 → 自动启动热点 → 本地电脑连接 → SSH：

ssh jetson@192.168.10.1

启动 Agent 与连通测试

连接前检查

在 SSH 进入机载电脑后，先不要给飞控上电。执行以下准备步骤：

# 1. 将用户加入 dialout 组（防止串口权限问题）
sudo usermod -a -G dialout $USER
# 重新登录使生效

# 2. 查看可用的串口设备
ls -l /dev/ttyTHS*
# 应输出 /dev/ttyTHS1（UART2）

确认机载电脑上已安装 Micro-XRCE-DDS-Agent（仿真环境中应该已装好，若未安装参考 ROS2+PX4 仿真环境开发教程中 MicroXRCE-DDS Agent 安装部分）。

验证硬件连通性

先测试飞控 → 机载电脑的串口通信是否正常。给飞控上电后，用 cat 检测是否有数据流：

stty -F /dev/ttyTHS1 921600 raw -echo
cat /dev/ttyTHS1

如果出现乱码（MAVLink 或 uXRCE-DDS 二进制数据），说明物理连接和串口配置正确：

如果没有任何输出，请检查：

飞控 TELEM2 TX ↔ 机载电脑 UART RX 是否正确交叉连接
波特率是否匹配（飞控端和机载电脑端都应为 921600）
飞控端 uxrce_dds_client start 是否已执行
是否被其他进程（如地面站）占用了串口

启动 Micro-XRCE-DDS-Agent

# 激活 ROS2 环境
source /opt/ros/humble/setup.bash

# 启动 Agent，通过串口连接飞控
MicroXRCEAgent serial --dev /dev/ttyTHS1 -b 921600

成功连接时，终端会输出类似以下信息：

[info] Serial agent starting...
[info] Connect to serial device: /dev/ttyTHS1, baudrate: 921600
[info] Connection established
[info] Client connected: 0x00000001
[info] Topic created: /fmu/out/vehicle_odometry
[info] Topic created: /fmu/out/imu
[info] Topic created: /fmu/out/...
...

至此，uXRCE-DDS 中间件部署完成。 Agent 会自动将飞控的 uORB 话题发布为 ROS2 话题。在另一个终端中验证：

source /opt/ros/humble/setup.bash
ros2 topic list | grep fmu

应看到大量 /fmu/out/... 开头的飞控数据话题。现在你的 ROS2 Offboard 控制代码可以直接在机载电脑上运行，话题名和仿真环境完全一致！

完整启动流程

每次真机飞行前，按以下顺序操作：

本地电脑连接机载电脑热点 北航中法工程师学院是XX
确认本地电脑能 ping 通 192.168.10.1
给飞控上电（通过电池），等待飞控启动完成
确认地面站（QGC）通过数传连接到飞控
SSH 进入机载电脑：ssh jetson@192.168.10.1
在 SSH 终端中启动 Agent：

source /opt/ros/humble/setup.bash
MicroXRCEAgent serial --dev /dev/ttyTHS1 -b 921600

开另一个 SSH 终端，运行 ROS2 控制节点：

source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
ros2 run offboard_control simple_offboard   # 以之前的 Offboard 节点为例

重要提示：根据经验，Agent 第一次连接成功后尽量保持飞控通电不断开。如果在飞行中反复断开重连串口，有时会导致后续连接失败。建议准备好后一次连通，中间不要改动。

参数速查表

| 参数 | 值 | 说明 | |------|-----|------| | MAV_1_CONFIG | 0（禁用） | 将 TELEM2 上的 MAVLink 卸任 | | MAV_2_CONFIG | 101（TELEM1） | 数传保留在 TELEM1 | | UXRCE_DDS_CFG | 102（TELEM2） | DDS Client 绑定 TELEM2 | | SER_TEL2_BAUD | 921600 | TELEM2 波特率 |

uXRCE-DDS vs MAVROS 对比

| | uXRCE-DDS | MAVROS | |---|---|---| | PX4 官方推荐 | v1.14+ 推荐 | 旧方案，仍在维护 | | 话题命名 | 原生 ROS2 话题 | /mavros/ 命名空间 | | 架构 | 去中心化 DDS | 中心化桥接 | | 多机扩展 | DDS 自动发现 | 需手动配置 | | 仿真/真机代码复用 | 话题完全一致，代码无需修改 | 需替换话题前缀 | | 稳定性 | 仍在活跃开发 | 成熟稳定（ROS1 时代积累） | | 真机延迟 | 低（UDP 多播） | 较低（MAVLink 串行化） |

建议：新项目优先选择 uXRCE-DDS。如果飞控固件版本 < v1.14 无法升级，或有大量遗留 MAVROS 代码需要兼容，可以继续使用 MAVROS。参考使用机载电脑控制 PX4 无人机（MAVROS2）。

常见问题排查

| 现象 | 可能原因 | 排查方法 | |------|---------|---------| | NSH 终端连接不上，串口被占用 | QGC 或其他进程占用了 ACM0 | 关闭 QGC 地面站后重试 | | uxrce_dds_client start 报错 | 波特率不匹配或 TELEM2 仍被 MAVLink 占用 | 确认 MAV_1_CONFIG=0，确认 mavlink stop -d /dev/ttyS3 已执行 | | Agent 启动后一直是 "not connected" | UXRCE_DDS_CFG 未正确设置 | param set UXRCE_DDS_CFG 102 | | cat /dev/ttyTHS1 无输出 | 接线错误或波特率不对 | 检查 TX/RX 是否交叉连接，波特率是否 921600 | | Agent 连上但 ros2 topic list 无 /fmu/ 话题 | ROS2 环境未 source 或 domain ID 不匹配 | source /opt/ros/humble/setup.bash，检查 echo $ROS_DOMAIN_ID | | 串口设备名不是 ttyTHS1 | 使用了 UART 编号不同 | ls /dev/ttyTHS* 查看可用设备 |

总结

本文完整演示了 Pixhawk 6C + Jetson Orin NX 上部署 uXRCE-DDS 中间件的全流程——从固件烧录、飞控 NSH 终端操作、MAVLink 卸任与 DDS Client 启用、硬件接线，到机载电脑热点配置和 Agent 启动。uXRCE-DDS 作为 PX4 官方推荐的 ROS2 通信方案，在代码复用性（仿真代码直接上真机）、扩展性（DDS 自动发现）和延迟方面都有显著优势。

当然，MAVROS 作为久经考验的老方案，在稳定性和社区文档积累上仍有其价值。读者可以根据自己的飞控固件版本和项目需求灵活选择。如果你同时使用两种方案进行对比测试，欢迎在评论区分享经验。

最后，感谢大家的耐心阅读，如有错漏请批评指正！

完整讲解视频

如果下方视频加载较慢，可前往知乎观看：https://www.zhihu.com/zvideo/1981084557504701099

参考资料

Controlling PX4 with an Onboard Computer via MAVROS2

Mon, 13 Jul 2026 00:00:00 GMT

Introduction

In the previous real-hardware article, we covered flashing the onboard computer. This guide builds on that work to walk through the complete ROS2 + PX4 real-flight takeoff workflow, using the onboard computer together with the low-level flight controller. If you're new to PX4 simulation, start with the ROS2+PX4 Simulation Setup Guide.

The author uses an AMOVLAB JCV-600 research quadrotor (with a CodevDynamics Codev-autopilot based on PX4) and an Nvidia Jetson Orin NX as the onboard computer for ROS2 control. Readers can follow this tutorial and adapt it to their own hardware.

Required hardware (adjust according to your setup):

A complete drone kit or a drone with a self-flashed flight controller
A pair of telemetry radios
A local PC and an onboard computer
Appropriate connecting cables (refer to the I/O port specifications of your onboard computer and drone)
Pinout diagrams / manuals for the relevant drone interfaces

Process Overview

The diagram below gives a high-level summary of the real-flight takeoff workflow used in this article. First, establish a stable connection between the ground station and the flight controller via the telemetry radio, ensuring that sensor data, GPS signals, and RC commands are transmitted properly. Next, the onboard computer communicates with the flight controller over a serial port -- install drivers, configure permissions, and match baud rates. After completing pre-flight checks in the ground station, connect to the onboard computer remotely via SSH, launch the MAVROS2 node to bridge ROS2 and PX4, and finally control the drone in Offboard mode. Throughout the entire process, keep the telemetry link active, monitor ground station data, and be ready to take manual control at any moment to guarantee flight safety.

Establishing a Connection with the Ground Station

Throughout the entire experiment, PX4 requires a stable connection to a ground station. The author uses a pair of RFD900x telemetry radios. The radio on the drone connects to the flight controller's I/O port (GND, RX-to-TX, TX-to-RX cross-connected); the flight controller provides a 5V power pin for the radio (internal power distribution is already handled).

On your local PC, open QGroundControl. Under "Application Settings" -> "Comm Links" -> "Add New Link", set Type to "Serial", select the serial port containing "USB0", and use the default baud rate of 57600. Once connected, the "ACT" LED on both radios stays solid green (power normal) and the "COM" LED blinks (link established). The ground station should display the drone's status.

Connecting the Onboard Computer to the Flight Controller

First, connect the flight controller to your PC via USB-TypeC. In the Parameters page, add a MAVLINK instance for the onboard computer (the default serial port is usually TELEM2):

MAV_1_CONFIG = TELEM2
MAV_1_MODE = Onboard
MAV_1_RATE = 115200

Three connection methods are described below.

Method 1: USB-TTL Cable

Use the flight controller's TELEM2 interface, which provides six pins: VCC (5V), TX, RX, PWM, GPIO, and GND. Connect the USB end to the onboard computer, and use Dupont-to-terminal wires to cross-connect GND, TX, and RX between the flight controller and the adapter. Do not supply power through this connection. The onboard computer should be powered separately from the reserved 12V power port.

# Check USB connection
lsusb
ls -l /dev/tty*
# You should see a CH340 USB-Serial adapter and /dev/ttyS0

# Add user to the dialout group
sudo usermod -a -G dialout $USER

Download and compile the CH340 driver:

sudo apt install build-essential git linux-headers-$(uname -r)
git clone https://github.com/juliagoda/CH341SER
cd CH341SER
make && sudo make install
sudo insmod ch341.ko
sudo cp ch341.ko /lib/modules/$(uname -r)/kernel/drivers/usb/serial/
sudo depmod -a
echo "ch341" | sudo tee /etc/modules-load.d/ch341.conf

Create the udev rule at /etc/udev/rules.d/99-ch341.rules:

SUBSYSTEM=="tty", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", MODE="0666", SYMLINK+="ttyCH341"

sudo udevadm control --reload-rules && sudo udevadm trigger
ls -l /dev/ttyCH341  # Verify the driver is installed

Test communication (MAVLINK garbled output means success):

stty -F /dev/ttyUSB0 115200 raw -echo
cat /dev/ttyUSB0

If you encounter issues with the USB-TTL connection method above, refer to the following link for driver installation guidance: https://blog.csdn.net/qq_52102933/article/details/126839474

Method 2: UART Serial Cable

The Jetson Orin NX has a 40-pin UART header, where Pin 8 (TX) and Pin 10 (RX) serve as the default UART1 debug port. We will use UART2 for this connection: Pin 14 (RX), Pin 12 (TX), Pin 6 (GND). Do not connect VCC; power the onboard computer separately.

# List available UART devices
ls /dev/ttyTHS*

# If ttyTHS1 is missing, edit /boot/extlinux/extlinux.conf
# Append console=ttyTHS1,115200 to the kernel boot line
sudo reboot

# Configure permissions and baud rate
sudo usermod -aG dialout $USER
stty -F /dev/ttyTHS1 115200 raw -echo
cat /dev/ttyTHS1  # Test communication

Method 3: USB-TypeC Cable

On the author's JCV-600, the TypeC port can connect directly to the onboard computer (for reference only; may not apply to all hardware).

lsusb  # Should show ID 26ac:0032 The Autopilot PX4 CODEV DP1000
sudo usermod -aG dialout $USER
stty -F /dev/ttyACM0 115200 raw -echo
cat /dev/ttyACM0  # Test communication

Pre-Flight Checks with the Ground Station

Perform the following checks on the "Vehicle Setup" page:

Sensor calibration: accelerometer, gyroscope, magnetometer, and level-horizon calibration; verify GPS signal quality (typically 6 or more satellites).

Parameter checks:

SYS_COMPANION = 921600 (ROS2 communication baud rate)
MAV_1_CONFIG = TELEM2 (matches the serial port connected to the onboard computer)
BAT_* parameters match the actual battery specifications
Verify that the emergency stop switch functions correctly
Confirm that the RC transmitter channel mapping is correct

Important: When using Offboard mode in real flights, always be ready to take over manually with the RC transmitter to ensure safety. Once all checks are complete, QGroundControl should display "Ready for Fly".

Remote Connection to the Onboard Computer via SSH

SSH (Secure Shell) is a network security protocol that provides secure remote access and file transfer through encryption and authentication. Because the onboard computer is attached to the drone and flies with it while issuing real-time ROS control commands to the flight controller, we cannot operate it directly. Instead, we use SSH to connect from the local PC to the remote onboard computer, enabling remote control of the flight controller through the onboard computer.

On the onboard computer, enable SSH and set it to start automatically at boot:

sudo systemctl start ssh
sudo systemctl enable ssh
sudo systemctl status ssh  # Should show active (running)

Ensure both computers are on the same local network. Using a personal hotspot is recommended to avoid client isolation on public networks. Look up the IP address:

ifconfig  # Look for output such as inet 10.192.90.46 netmask 255.255.0.0 broadcast 10.192.255.255
# Here 10.192.90.46 is the IPv4 address; netmask and broadcast are the subnet mask and broadcast address respectively
# Verify the IPv4 addresses of both computers to ensure they are on the same large subnet
ping 10.192.90.46  # From the local PC, ping the onboard computer to verify connectivity

SSH connection:

ssh jetson@10.192.90.46  # First time: type "yes" to confirm, then enter the password
# Once the terminal prompt changes to jetson@ubuntu:~$, the connection is successful
# Type exit to close the SSH session

Offboard Mode Control with MAVROS

In the simulation articles we used MicroXRCEAgent for ROS2-PX4 communication. The author's flight controller firmware includes MAVROS (rather than the XRCE middleware). MAVROS is a crucial tool bridging ROS and the MAVLINK protocol. The MAVROS2 package for ROS2 is still under active development and maintenance, but it is already usable for simple tasks.

In the future, the author plans to explore development with the XRCE middleware, which will require re-flashing the PX4 firmware. Update: ROS2+PX4 Drone Swarm Real-Flight (Part 3): Deploying the UXRCE-DDS Middleware (with Pixhawk 6C)

Installing MAVROS2

sudo apt install ros-humble-mavros ros-humble-mavros-msgs

# Install the GeographicLib geographic datasets (required dependency)
wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
chmod +x install_geographiclib_datasets.sh
sudo ./install_geographiclib_datasets.sh

Creating the Offboard Node

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python offboard_control \
    --dependencies rclpy geometry_msgs mavros_msgs
cd offboard_control/offboard_control
touch simple_offboard.py && chmod +x simple_offboard.py

Complete source code of simple_offboard.py (Offboard takeoff and hover at 5 meters):

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from rclpy.clock import Clock
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, SetMode

class SimpleOffboard(Node):

    def __init__(self):
        super().__init__('simple_offboard')

        # mavros
        self.declare_parameter('mavros_ns', '/mavros')
        self.mavros_ns = self.get_parameter('mavros_ns').get_parameter_value().string_value

        # publishers
        self.local_pos_pub = self.create_publisher(
            PoseStamped,
            f'{self.mavros_ns}/setpoint_position/local',
            10
        )

        # subscribers
        self.state_sub = self.create_subscription(
            State,
            f'{self.mavros_ns}/state',
            self.state_callback,
            10
        )

        # service clients
        self.arming_client = self.create_client(
            CommandBool,
            f'{self.mavros_ns}/cmd/arming'
        )
        while not self.arming_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('mavros/arming service not available, waiting...')

        self.set_mode_client = self.create_client(
            SetMode,
            f'{self.mavros_ns}/set_mode'
        )
        while not self.set_mode_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('mavros/set_mode service not available, waiting...')

        self.current_state = State()  
        self.timer = self.create_timer(0.05, self.control_loop)  
        self.offboard_setpoint_counter = 0
        self.last_call_time = self.get_clock().now()
        self.target_altitude = 5.0 

        self.get_logger().info("Offboard Node Initialized. Waiting for MAVROS state...")

    def state_callback(self, msg):
        self.current_state = msg

    def arm_drone(self):
        self.get_logger().info("Attempting to ARM drone...")
        arm_request = CommandBool.Request()
        arm_request.value = True
        future = self.arming_client.call_async(arm_request)
        future.add_done_callback(self.arm_response_callback)

    def arm_response_callback(self, future):
        try:
            response = future.result()
            if response.success:
                self.get_logger().info("Arming successful!")
            else:
                self.get_logger().warn(f"Arming failed: {response.result}")
        except Exception as e:
            self.get_logger().error(f"Arming service call failed: {str(e)}")

    def set_offboard_mode(self):
        self.get_logger().info("Attempting to set OFFBOARD mode...")
        mode_request = SetMode.Request()
        mode_request.custom_mode = "OFFBOARD"
        future = self.set_mode_client.call_async(mode_request)
        future.add_done_callback(self.set_mode_response_callback)

    def set_mode_response_callback(self, future):
        try:
            response = future.result()
            if response.mode_sent:
                self.get_logger().info("OFFBOARD mode enabled!")
            else:
                self.get_logger().warn(f"Failed to set OFFBOARD mode.")
        except Exception as e:
            self.get_logger().error(f"SetMode service call failed: {str(e)}")

    def control_loop(self):
        now = self.get_clock().now()

        self.get_logger().info(
            f"[State] Connected: {self.current_state.connected}, "
            f"Armed: {self.current_state.armed}, "
            f"Mode: {self.current_state.mode}",
            throttle_duration_sec=1.0  
        )

        pose = PoseStamped()
        pose.header.stamp = now.to_msg()
        pose.header.frame_id = "map"  
        pose.pose.position.x = 0.0
        pose.pose.position.y = 0.0
        pose.pose.position.z = float(self.target_altitude)
        self.local_pos_pub.publish(pose)

        if not self.current_state.connected:
            return

        time_since_last_call = (now - self.last_call_time).nanoseconds / 1e9  

        if time_since_last_call > 5.0:  
            if not self.current_state.armed:  
                self.arm_drone()
                self.last_call_time = now
            elif self.current_state.mode != "OFFBOARD": 
                self.set_offboard_mode()
                self.last_call_time = now

def main(args=None):
    rclpy.init(args=args)
    offboard_node = SimpleOffboard()

    try:
        rclpy.spin(offboard_node)
    except KeyboardInterrupt:
        offboard_node.get_logger().info('Offboard control interrupted by user.')
    finally:
        offboard_node.destroy_node()
        rclpy.shutdown()
        print("Offboard Node Shutdown.")

if __name__ == '__main__':
    main()

Add the following to entry_points in setup.py:

'console_scripts': [
    'simple_offboard = offboard_control.simple_offboard:main',
],

Build:

cd ~/ros2_ws
colcon build --symlink-install --packages-select offboard_control
source install/setup.bash

Takeoff

With all hardware and software preparations complete, execute the full takeoff procedure:

Connect the drone to the ground station via the telemetry radio
Connect the onboard computer to the flight controller using any of the three methods
Complete the pre-flight checks in the ground station
Use SSH to remotely connect from the local PC to the onboard computer
In the SSH terminal, execute:

# Activate the ROS2 environment
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash

# Launch the MAVROS2 node (serial port and baud rate must match the flight controller configuration)
ros2 run mavros mavros_node --ros-args -p fcu_url:="serial:///dev/ttyACM0:115200"
# When you see [INFO] [mavros]: FCU connection established, the link is up

# Launch the Offboard control node
ros2 run offboard_control simple_offboard

At this point you should see the drone arm, take off, and ascend to 5 meters. Due to the inherent risk of Offboard mode, always be ready to take over with the RC transmitter.

Summary

Practice makes perfect -- get hands-on and iterate. This article has presented one complete workflow: from telemetry radio connection, through three hardware connection methods between the onboard computer and flight controller, pre-flight checks, SSH remote access, and finally MAVROS2 Offboard takeoff. Corrections and feedback are welcome. Thank you for reading!

References

AMOVLAB JCV-600 User Manual
PX4 Official Documentation
ROS2+PX4 Drone Swarm Real-Flight (Part 3): Deploying the UXRCE-DDS Middleware

使用机载电脑控制PX4无人机（基于MAVROS2）

Mon, 13 Jul 2026 00:00:00 GMT

简介

在真机部分的上篇文章中，我们完成了机载电脑的刷机。本文基于已有工作，利用机载电脑和底层飞控，跑通 ROS2 + PX4 的真机起飞流程。如果你是第一次接触 PX4 仿真，建议先从 ROS2+PX4 仿真环境开发教程开始。

笔者使用阿木实验室（AMOVLAB）的 JCV-600 科研实验四旋翼飞机（底层飞控为 CodevDynamics 基于 PX4 的 Codev-autopilot），使用 Nvidia Jetson Orin NX 作为机载电脑进行 ROS2 控制。读者可以仿照本教程，利用自己的硬件设备进行相似实验。

需要的硬件设备（根据自己的情况调整）：

无人机整机套件或自行刷入飞控的无人机
一对数传
本地电脑与机载电脑
相应的连接线若干（参考机载电脑与无人机 I/O 口的具体说明）
飞机相关接口的示意图/说明书

流程总述

下图是本文采用的真机起飞流程的简要示意图。首先通过数传建立地面站与飞控的稳定连接，确保传感器数据、GPS 信号和遥控指令正常传输。随后，机载电脑通过串口与飞控通信，完成驱动安装、权限配置及波特率匹配。在地面站进行飞行前检查通过后，通过 SSH 远程连接机载电脑，启动 MAVROS2 节点，建立 ROS2 与 PX4 的通信链路。最后，使用 Offboard 模式控制无人机。整个过程需全程连接数传，关注地面站数据，随时准备手动接管，确保飞行安全。

与地面站建立连接

在整个实验过程中，PX4 飞控严格要求与地面站的稳定连接。笔者使用一对 RFD900x 数传进行连接。无人机上的数传与飞控的 I/O 口连接（GND、RX-TX、TX-RX 交叉连接），飞控预留了 5V 电源口为数传供电，这是内部已经完成了电源分配。

在本地电脑中，打开 QGroundControl 地面站，在 "Application Settings" → "Comm Links" → "Add New Link" 中，Type 选择 "Serial"，Serial Port 选择含 "USB0" 的选项，波特率默认 57600。连接后，两个数传 "ACT" 灯常绿（供电正常），"COM" 灯闪烁（连接成功），地面站可以看到飞机各项状态。

机载电脑与飞控连接

首先，用 USB-TypeC 线将飞控连接到电脑地面站，在参数页面为机载电脑添加 MAVLINK 实例（一般默认为 TELEM2 串口）：

MAV_1_CONFIG = TELEM2
MAV_1_MODE = Onboard
MAV_1_RATE = 115200

以下介绍三种连接方式。

方式一：USB-TTL 线

使用 PX4 的 TELEM2 接口，用于连接机载电脑、数传等，有 VCC(5V)、TX、RX、PWM、GPIO、GND 六个引脚可供选择。将 USB 连接在机载电脑端，用杜邦转端子线交叉连接飞控的 GND、TX、RX，不要进行供电。机载电脑使用预留的 12V 供电口单独供电。

# 检查 USB 连接
lsusb
ls -l /dev/tty*
# 应出现 CH340 USB-Serial adapter 和 /dev/ttyS0

# 将用户加入 dialout 组
sudo usermod -a -G dialout $USER

下载并编译 CH340 驱动：

sudo apt install build-essential git linux-headers-$(uname -r)
git clone https://github.com/juliagoda/CH341SER
cd CH341SER
make && sudo make install
sudo insmod ch341.ko
sudo cp ch341.ko /lib/modules/$(uname -r)/kernel/drivers/usb/serial/
sudo depmod -a
echo "ch341" | sudo tee /etc/modules-load.d/ch341.conf

创建 udev 规则 /etc/udev/rules.d/99-ch341.rules：

SUBSYSTEM=="tty", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", MODE="0666", SYMLINK+="ttyCH341"

sudo udevadm control --reload-rules && sudo udevadm trigger
ls -l /dev/ttyCH341  # 验证驱动安装

测试通信（如出现 MAVLINK 乱码则说明成功）：

stty -F /dev/ttyUSB0 115200 raw -echo
cat /dev/ttyUSB0

如果上述 USB-TTL 连接方式出现问题，可以参考以下链接进行驱动安装：https://blog.csdn.net/qq_52102933/article/details/126839474

方式二：UART 串口线

Jetson Orin NX 有 40 针 UART 引脚，其中 Pin8(TX)、Pin10(RX) 为 UART1 默认调试口。我们使用 UART2 进行连接：Pin 14(RX)、Pin 12(TX)、Pin 6(GND)，不连 VCC，单独供电。

# 查看可用 UART
ls /dev/ttyTHS*

# 如缺少 ttyTHS1，编辑 /boot/extlinux/extlinux.conf
# 在内核启动行末尾添加 console=ttyTHS1,115200
sudo reboot

# 配置权限和波特率
sudo usermod -aG dialout $USER
stty -F /dev/ttyTHS1 115200 raw -echo
cat /dev/ttyTHS1  # 测试通信

方式三：USB-TypeC 线

笔者的 JCV-600 的 TypeC 接口可直接连接机载电脑（仅供参考，不一定适用于所有设备）。

lsusb  # 应出现 ID 26ac:0032 The Autopilot PX4 CODEV DP1000
sudo usermod -aG dialout $USER
stty -F /dev/ttyACM0 115200 raw -echo
cat /dev/ttyACM0  # 测试通信

利用地面站做预检查

在 "Vehicle Setup" 页面进行飞行前检查：

传感器校准：加速度计、陀螺仪、磁罗盘、水平校准；确认 GPS 信号质量（一般 6 颗星以上）。

参数检查：

SYS_COMPANION = 921600（ROS2 通信波特率）
MAV_1_CONFIG = TELEM2（对应机载电脑连接的串口）
BAT_* 参数与实际电池规格匹配
检查紧急停止开关功能正常
验证遥控器各通道映射正确

重要：在实机飞行中使用 Offboard 模式时，需随时准备用遥控器手动接管飞行，以确保安全。检查完成后，QGC 地面站应显示 "Ready for Fly"。

使用 SSH 远程连接机载电脑

SSH（Secure Shell）是一种网络安全协议，通过加密和认证机制实现安全的访问和文件传输等业务。由于机载电脑连接在飞控上，并且直接向飞控实时发布 ROS 控制指令，与无人机一起飞行，因此我们不能够直接在机载电脑上发布控制指令。在这里我们使用 SSH 协议将本地电脑与远程的机载电脑进行连接，实现本地电脑远程控制机载电脑从而实现对飞控的控制。

在机载电脑的终端启用 SSH 并且设置开机自启：

sudo systemctl start ssh
sudo systemctl enable ssh
sudo systemctl status ssh  # 应显示 active (running)

确保两台电脑位于同一局域网内，使用个人热点避免公网的客户端隔离。查询 IP 地址：

ifconfig  # 查找形如 inet 10.192.90.46 netmask 255.255.0.0 broadcast 10.192.255.255 的输出
# 其中 10.192.90.46 为 IPv4 地址，netmask 和 broadcast 分别为子网掩码和广播地址
# 验证两台电脑的 IPv4 地址，确保在同一大子网下
ping 10.192.90.46  # 在本地电脑 ping 机载电脑，验证网络连通

SSH 连接：

ssh jetson@10.192.90.46  # 首次连接需输入 yes 确认，然后输入密码
# 终端用户名变为 jetson@ubuntu:~$ 说明连接成功
# 输入 exit 退出 SSH 连接

使用 MAVROS 进行 Offboard 模式控制

在仿真篇章中我们使用 MicroXRCEAgent 进行 ROS2 与 PX4 的通信。笔者的飞控固件中刷入了 MAVROS（而非 XRCE 中间件）。MAVROS 是连接 ROS 与 MAVLINK 协议的重要工具。针对 ROS2 的 MAVROS2 功能包仍在开发维护中，但对于简单任务已可使用。

后续，笔者会针对 XRCE 中间件的开发进行探索，这需要重新刷写 PX4 固件。已更新：ROS2+PX4 无人机编队实机（三）UXRCE-DDS 中间件的部署（以 Pixhawk 6C 为例）

安装 MAVROS2

sudo apt install ros-humble-mavros ros-humble-mavros-msgs

# 安装 GeographicLib 地理数据集（必需依赖）
wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
chmod +x install_geographiclib_datasets.sh
sudo ./install_geographiclib_datasets.sh

创建 Offboard 节点

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python offboard_control \
    --dependencies rclpy geometry_msgs mavros_msgs
cd offboard_control/offboard_control
touch simple_offboard.py && chmod +x simple_offboard.py

simple_offboard.py 完整源码（Offboard 起飞并悬停 5 米）：

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from rclpy.clock import Clock
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, SetMode

class SimpleOffboard(Node):

    def __init__(self):
        super().__init__('simple_offboard')

        # mavros
        self.declare_parameter('mavros_ns', '/mavros')
        self.mavros_ns = self.get_parameter('mavros_ns').get_parameter_value().string_value

        # publishers
        self.local_pos_pub = self.create_publisher(
            PoseStamped,
            f'{self.mavros_ns}/setpoint_position/local',
            10
        )

        # subscribers
        self.state_sub = self.create_subscription(
            State,
            f'{self.mavros_ns}/state',
            self.state_callback,
            10
        )

        # service clients
        self.arming_client = self.create_client(
            CommandBool,
            f'{self.mavros_ns}/cmd/arming'
        )
        while not self.arming_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('mavros/arming service not available, waiting...')

        self.set_mode_client = self.create_client(
            SetMode,
            f'{self.mavros_ns}/set_mode'
        )
        while not self.set_mode_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('mavros/set_mode service not available, waiting...')

        self.current_state = State()  
        self.timer = self.create_timer(0.05, self.control_loop)  
        self.offboard_setpoint_counter = 0
        self.last_call_time = self.get_clock().now()
        self.target_altitude = 5.0 

        self.get_logger().info("Offboard Node Initialized. Waiting for MAVROS state...")

    def state_callback(self, msg):
        self.current_state = msg

    def arm_drone(self):
        self.get_logger().info("Attempting to ARM drone...")
        arm_request = CommandBool.Request()
        arm_request.value = True
        future = self.arming_client.call_async(arm_request)
        future.add_done_callback(self.arm_response_callback)

    def arm_response_callback(self, future):
        try:
            response = future.result()
            if response.success:
                self.get_logger().info("Arming successful!")
            else:
                self.get_logger().warn(f"Arming failed: {response.result}")
        except Exception as e:
            self.get_logger().error(f"Arming service call failed: {str(e)}")

    def set_offboard_mode(self):
        self.get_logger().info("Attempting to set OFFBOARD mode...")
        mode_request = SetMode.Request()
        mode_request.custom_mode = "OFFBOARD"
        future = self.set_mode_client.call_async(mode_request)
        future.add_done_callback(self.set_mode_response_callback)

    def set_mode_response_callback(self, future):
        try:
            response = future.result()
            if response.mode_sent:
                self.get_logger().info("OFFBOARD mode enabled!")
            else:
                self.get_logger().warn(f"Failed to set OFFBOARD mode.")
        except Exception as e:
            self.get_logger().error(f"SetMode service call failed: {str(e)}")

    def control_loop(self):
        now = self.get_clock().now()

        self.get_logger().info(
            f"[State] Connected: {self.current_state.connected}, "
            f"Armed: {self.current_state.armed}, "
            f"Mode: {self.current_state.mode}",
            throttle_duration_sec=1.0  
        )

        pose = PoseStamped()
        pose.header.stamp = now.to_msg()
        pose.header.frame_id = "map"  
        pose.pose.position.x = 0.0
        pose.pose.position.y = 0.0
        pose.pose.position.z = float(self.target_altitude)
        self.local_pos_pub.publish(pose)

        if not self.current_state.connected:
            return

        time_since_last_call = (now - self.last_call_time).nanoseconds / 1e9  

        if time_since_last_call > 5.0:  
            if not self.current_state.armed:  
                self.arm_drone()
                self.last_call_time = now
            elif self.current_state.mode != "OFFBOARD": 
                self.set_offboard_mode()
                self.last_call_time = now

def main(args=None):
    rclpy.init(args=args)
    offboard_node = SimpleOffboard()

    try:
        rclpy.spin(offboard_node)
    except KeyboardInterrupt:
        offboard_node.get_logger().info('Offboard control interrupted by user.')
    finally:
        offboard_node.destroy_node()
        rclpy.shutdown()
        print("Offboard Node Shutdown.")

if __name__ == '__main__':
    main()

在 setup.py 的 entry_points 中添加：

'console_scripts': [
    'simple_offboard = offboard_control.simple_offboard:main',
],

编译：

cd ~/ros2_ws
colcon build --symlink-install --packages-select offboard_control
source install/setup.bash

起飞

所有软硬件准备完成，执行完整起飞流程：

通过数传将无人机与地面站连接
任选一种方式将机载电脑与飞控连接
地面站完成飞行前预检查
使用 SSH 将本地电脑与机载电脑远程连接
在 SSH 终端中执行：

# 激活 ROS2 环境
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash

# 启动 MAVROS2 节点（串口和波特率需与飞控配置一致）
ros2 run mavros mavros_node --ros-args -p fcu_url:="serial:///dev/ttyACM0:115200"
# 出现 [INFO] [mavros]: FCU connection established 说明连接成功

# 启动 Offboard 控制节点
ros2 run offboard_control simple_offboard

此时应看到无人机解锁、起飞、飞至 5 米高度。由于 Offboard 模式的危险性，需随时准备使用遥控器接管飞行。

总结

勤能补拙，多动手尝试。本文给出了笔者自己的一种操作方式——从数传连接、机载电脑与飞控的三种硬件连接方式、飞行前检查、SSH 远程连接，到最终的 MAVROS2 Offboard 起飞。如有错漏请指正，感谢各位读者的耐心阅读！

参考资料

AMOVLAB JCV-600 使用手册
PX4 官方文档
ROS2+PX4 无人机编队实机（三）UXRCE-DDS 中间件的部署

Betaflight + Gazebo Software-In-The-Loop Simulation Tutorial

Sat, 11 Jul 2026 00:00:00 GMT

About ROS2: This tutorial's simulation link (Gazebo ↔ SITL ↔ Controller) uses direct UDP communication without ROS2. ROS2's ros_gz_bridge can be added for multi-drone, distributed communication, or ROS2 ecosystem integration.

This tutorial is fairly complex and focuses on explaining the overall framework. Full source files are available at the GitHub repository — contributions and stars appreciated!

Introduction

Betaflight is the most popular open-source flight controller firmware in FPV Racing and Freestyle drone communities, widely deployed on STM32/AT32 MCUs. Betaflight provides SITL (Software-In-The-Loop) mode, enabling developers to run the complete Betaflight firmware within the Gazebo physics simulation engine.

The core idea behind SITL: compile the Betaflight firmware as an x86_64 Linux executable. The flight controller's sensor inputs (IMU, barometer, GPS) come from the Gazebo simulation environment via UDP packets containing simulated sensor data. The flight controller's motor outputs (PWM commands) are likewise sent back to Gazebo via UDP to drive the propellers in the physics engine. This enables developers to fully validate flight controller logic and control algorithms on a PC, with no real hardware required.

This article provides a complete walkthrough for setting up a Betaflight SITL + Gazebo Harmonic + Python autonomous control simulation environment on Ubuntu 22.04. Before we begin, credit goes to the OSRF vehicle_gateway project and the Betaflight community for their open-source code and documentation — this tutorial draws heavily on those resources.

Prerequisites

Following the author's earlier ROS2+PX4 simulation tutorials, the following foundational environment should already be configured:

Ubuntu 22.04
ROS2 Humble (recommended: FishROS one-line installer — wget http://fishros.com/install -O fishros && . fishros)
Gazebo Harmonic (gz-sim8, version 8.9.0)
ROS2-Gazebo communication bridge ros-humble-ros-gzharmonic

Verify in a terminal:

ros2 --version       # Output should contain "humble"
gz sim --versions    # Output should be "Gazebo Sim 8.9.0"
dpkg -l | grep ros-humble-ros-gzharmonic  # Should show installed

Install required tools and Gazebo dev packages (needed for compiling plugins — the gz-harmonic runtime does NOT include these headers):

pip3 install --user websockify
sudo apt-get install -y socat

# Gazebo Harmonic dev packages — required for compiling custom Gazebo plugins!
sudo apt-get install -y \
    libgz-sim8-dev \
    libgz-transport13-dev \
    libgz-msgs10-dev \
    libgz-math7-dev \
    libgz-plugin2-dev \
    libsdformat14-dev

websockify: provides WebSocket → TCP proxying, used to connect to the Betaflight Configurator ground-station tool in a browser
socat: creates virtual serial ports (pty), mapping Betaflight's TCP UART to a local virtual serial device

Ensure your network can reliably access GitHub, otherwise large repository clones may fail partway through.

Simulation Link Architecture

Before diving into configuration, let us examine the full simulation link — this helps with diagnosing issues later. The simulation link consists of four processes that communicate via UDP ports:

Port allocation and data flow (Betaflight source sitl.c lines 192–195):

| Port | Direction | Purpose | Packet Type | |------|-----------|---------|-------------| | 9001 | SITL → External | Raw PWM (reserved for RealFlightBridge) | servo_packet_raw | | 9002 | SITL → Gazebo | Motor speed commands [0.0, 1.0] (3D mode: [-1.0, 1.0]) | servo_packet | | 9003 | Gazebo → SITL | Flight state data (IMU, position, velocity, attitude, pressure) | fdm_packet | | 9004 | Controller → SITL | 16-channel RC control signals [1000–2000] | rc_packet | | TCP 5761 | Bidirectional | MSP protocol (configuration + telemetry feedback) | N/A |

Packet structures (Betaflight source target.h lines 224–246):

// fdm_packet: Gazebo plugin → SITL
typedef struct {
    double timestamp;
    double imu_angular_velocity_rpy[3];    // Angular velocity (rad/s)
    double imu_linear_acceleration_xyz[3]; // Linear acceleration (m/s²)
    double imu_orientation_quat[4];        // Attitude quaternion (w,x,y,z)
    double velocity_xyz[3];                // Velocity (m/s, ENU frame)
    double position_xyz[3];                // Position
    double pressure;                       // Pressure (Pa)
} fdm_packet;

// servo_packet: SITL → Gazebo plugin
typedef struct {
    float motor_speed[4];   // [0.0, 1.0] or 3D mode [-1.0, 1.0]
} servo_packet;

// rc_packet: Controller → SITL
typedef struct {
    double timestamp;
    uint16_t channels[16];   // PWM [1000, 2000]
} rc_packet;

The Gazebo plugin (BetaflightPlugin) runs two phases per simulation update cycle:

PreUpdate (before simulation step): Calls ReceiveServoPacket() to read servo_packet from UDP 9002, converts motor speeds into joint force/velocity commands
PostUpdate (after simulation step): Reads angular velocity and acceleration from the IMU sensor, obtains pose and linear velocity from the Gazebo Entity Component Manager, populates an fdm_packet, applies coordinate frame transformations, and sends it via UDP to 127.0.0.1:9003

Compiling Betaflight SITL

cd ~
git clone https://github.com/betaflight/betaflight.git
cd betaflight

Note: We use the master branch (version 2026.6.0-alpha), not the 4.5.x release, because the 4.5.x stable release's SITL code lacks native Gazebo support — the ENABLE_GAZEBO_BRIDGE feature was only introduced into master after 4.5.4.

Unlike compiling PX4 which requires an ARM cross-compilation toolchain, the SITL mode compiles directly with the host GCC:

make TARGET=SITL -j$(nproc)

Successful build output:

Linking SITL
   text    data     bss     dec     hex  filename
 389784   21804   77376  488964   77604  ./obj/main/betaflight_SITL.elf

Verify:

./obj/main/betaflight_SITL.elf --help
# Output: Betaflight SITL Usage: ./obj/main/betaflight_SITL.elf [options]

Why can SITL run standalone? Because Betaflight SITL emulates the real flight controller's hardware peripherals in software: virtual IMU (accgyro_virtual.c) receives angular velocity and acceleration data from Gazebo; virtual barometer (barometer_virtual.c) receives pressure/altitude data; virtual GPS (gps_virtual.c) receives position data; virtual EEPROM (file I/O) stores configuration as eeprom.bin; virtual UART (serial_tcp.c) replaces physical UART with TCP.

After compilation, it is advisable to back up: zip -r betaflight.zip betaflight/

Obtaining and Compiling the Gazebo Plugin

Betaflight itself does not include a Gazebo plugin. We need the betaflight_gazebo plugin from OSRF's vehicle_gateway project. Important: vehicle_gateway officially targets Gazebo Garden (gz-sim7), while our environment uses Gazebo Harmonic (gz-sim8), requiring compatibility modifications.

Step 1: Clone and exclude unneeded sub-packages

cd ~/ros2_ws/src
git clone https://github.com/osrf/vehicle_gateway.git vehicle_gateway

cd ~/ros2_ws/src/vehicle_gateway
for pkg in gz_aerial_plugins vehicle_gateway vehicle_gateway_betaflight \
    vehicle_gateway_demo vehicle_gateway_integration_test vehicle_gateway_multi \
    vehicle_gateway_px4 vehicle_gateway_python vehicle_gateway_python_helpers \
    vehicle_gateway_sim_performance px4_sim betaflight_configurator \
    betaflight_demo qgroundcontrol; do
    touch "$pkg/COLCON_IGNORE"
done

Step 2: Adapting for Gazebo Harmonic (2 files need version number updates; the other 3 code fixes are already upstream)

The current upstream OSRF vehicle_gateway code already includes most of the Gazebo Harmonic compatibility fixes. Only the library version numbers in two files still need manual changes.

Required changes — Update library version numbers in both CMakeLists.txt and package.xml

(1) Edit ~/ros2_ws/src/vehicle_gateway/betaflight_gazebo/CMakeLists.txt

Replace all occurrences (including find_package, target_link_libraries, and any other references). Search globally for:

All gz-sim7 → gz-sim8
All gz-transport12 → gz-transport13

(2) Edit ~/ros2_ws/src/vehicle_gateway/betaflight_gazebo/package.xml

Update the <depend> declarations:

<depend>gz-sim7</depend> → <depend>gz-sim8</depend>
<depend>gz-transport12</depend> → <depend>gz-transport13</depend>

Why? Gazebo Garden (gz-sim7, gz-transport12) and Gazebo Harmonic (gz-sim8, gz-transport13) have incompatible ABIs. The plugin must link against the same library versions as the Gazebo server process. gz-math7 and gz-plugin2 are shared across both versions. The package.xml must also be updated, otherwise colcon/rosdep will try to resolve gz-sim7 as a dependency and fail in a Harmonic-only environment.

Verification items — The following three are already fixed upstream. No manual changes needed, but verify your clone includes them:

Check 1: Header includes (BetaflightPlugin.cpp lines 35–39)

~/ros2_ws/src/vehicle_gateway/betaflight_gazebo/src/BetaflightPlugin.cpp should already contain:

#include <functional>
#include <gz/msgs/imu.pb.h>
#include <gz/msgs/fluid_pressure.pb.h>
#include <gz/msgs/double.pb.h>

Context: Gazebo Harmonic (gz-sim8)'s System.hh no longer transitively includes protobuf message headers, so explicit includes are needed. If your clone is missing these, add them manually.

Check 2: IMU and fluid pressure subscriptions (PreUpdate lines 440–443; Configure lines 296–301)

The IMU subscription is in PreUpdate (around line 440), and the pressure sensor subscription is in Configure (around line 296). Both already use the std::function + std::bind pattern — no changes needed:

IMU subscription in PreUpdate (around line 440):

std::function<void(const gz::msgs::IMU&)> imuCb =
    std::bind(&BetaFlightPluginPrivate::ImuCb,
              this->dataPtr.get(), std::placeholders::_1);
this->dataPtr->node.Subscribe(imuTopicName, imuCb);

Fluid pressure subscription in Configure (around line 296):

std::function<void(const gz::msgs::FluidPressure&)> airPressureCb =
    std::bind(&BetaFlightPluginPrivate::onAirPressureMessageReceived,
              this->dataPtr.get(), std::placeholders::_1);
this->dataPtr->node.Subscribe(
    "/world/empty_betaflight_world/model/iris_with_Betaflight/model/iris_with_standoffs/"
    "link/imu_link/sensor/air_pressure_sensor/air_pressure",
    airPressureCb);

Context: Gazebo Garden's gz-transport12 allowed Node::Subscribe to accept member function pointers directly for template deduction. gz-transport13 tightened the rules, requiring an explicit std::function wrapper. The IMU callback ImuCb has signature void(const gz::msgs::IMU&), and the pressure callback onAirPressureMessageReceived has signature void(const gz::msgs::FluidPressure&).

Check 3: Deadlock fix in PostUpdate (lines 478–484)

The PostUpdate function should NOT contain the betaflightOnline condition:

if (!_info.paused && _info.simTime > this->dataPtr->lastControllerUpdateTime)
{
    double t = ...;
    this->SendState(t, _ecm);
    ...
}

If you see && this->dataPtr->betaflightOnline, delete it. Why: Initially SITL waits for Gazebo to send an fdm_packet, while Gazebo skips sending fdm_packet because betaflightOnline == false — a deadlock that never resolves.

Step 3: Build the plugin

cd ~/ros2_ws
source /opt/ros/humble/setup.bash
colcon build --packages-select betaflight_gazebo

Verify: ls -lh ~/ros2_ws/install/betaflight_gazebo/lib/libBetaflightPlugin.so (approximately 8.7M)

Simulation World Configuration

Create the launch package directory structure:

mkdir -p ~/bf_sitl/{launch,worlds,models,config}
ln -sf ~/ros2_ws/src/vehicle_gateway/betaflight_sim/models/iris_with_standoffs \
    ~/bf_sitl/models/iris_with_standoffs

Core plugin configuration in the world file betaflight_world.sdf:

<plugin name="BetaFlightPlugin" filename="BetaflightPlugin">
    <fdm_addr>127.0.0.1</fdm_addr>
    <fdm_port_in>9002</fdm_port_in>
    <listen_addr>127.0.0.1</listen_addr>
    <modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
    <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
    <imuName>iris_with_standoffs::imu_link::imu_sensor</imuName>
    <control channel="0">
        <jointName>iris_with_standoffs::rotor_0_joint</jointName>
        <multiplier>838</multiplier>
    </control>
    <!-- channels 1-3 similarly configured, multipliers: 838, -838, -838 -->
</plugin>

Coordinate frame transform explained: Gazebo uses ENU (X=East, Y=North, Z=Up); Betaflight expects NED (X=North, Y=East, Z=Down). gazeboXYZToNED with Yaw=180° performs the heading rotation, and the plugin's internal SendState() further applies an Rz(π/2) rotation to complete the full transform.

Configuring ARM — Enabling the Flight Controller to Arm

Betaflight defaults to disarmed (ARM disabled). We need to configure AUX1 to enter ARM mode when its channel value is in the 1700–2100 range. First, start SITL:

cd ~/bf_sitl/config
~/betaflight/obj/main/betaflight_SITL.elf --ip 127.0.0.1

Wait for bind port 5761 for UART1, then run the CLI configuration script bf_cli_config.py:

#!/usr/bin/env python3
"""Configure Betaflight SITL via TCP CLI — set ARM on AUX1, save to eeprom"""
import socket, time, sys

HOST, PORT = '127.0.0.1', 5761

print("Waiting for SITL TCP port 5761...")
for i in range(30):
    try:
        s = socket.socket(); s.settimeout(1)
        s.connect((HOST, PORT)); s.close()
        print(f"Connected after {i+1}s"); break
    except:
        time.sleep(1)
else:
    print("ERROR: SITL not ready after 30s"); sys.exit(1)

sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.settimeout(10)
sock.connect((HOST, PORT))
time.sleep(2)

# Drain initial output
try: sock.recv(8192)
except: pass

# Enter CLI mode (send '#' instead of MSP header '$')
print("Entering CLI mode...")
sock.send(b'#\n')
time.sleep(2)
try:
    data = sock.recv(4096)
    print(data.decode('utf-8', errors='replace')[:300])
except: pass

# Set ARM on AUX1 (mode=0=ARM, aux=0=AUX1, range=1700-2100)
print("\nSetting ARM on AUX1 (1700-2100)...")
sock.send(b'aux 0 0 0 1700 2100\n')
time.sleep(1)

# Save to EEPROM and reboot
print("Saving to EEPROM...")
sock.send(b'save\n')
time.sleep(5)
try:
    data = sock.recv(8192)
    print(data.decode('utf-8', errors='replace')[-300:])
except Exception as e:
    print(f"Save response: {e}")

sock.close()
print("\nDone! eeprom.bin configured with ARM on AUX1.")

What aux 0 0 0 1700 2100 means: 1st 0 = configuration slot; 2nd 0 = mode ID (0 = ARM); 3rd 0 = AUX channel index (0 = AUX1); 1700 2100 = trigger range.

AUX Channel Mode Reference

aux <slot> <mode ID> <AUX channel> <range low> <range high>

Common mode IDs:

| ID | Name | Function | |----|------|----------| | 0 | ARM | Arm/disarm the flight controller | | 1 | ANGLE | Self-stabilizing mode | | 2 | HORIZON | Semi-stabilizing mode | | 5 | HEADFREE | Head-free mode | | 11 | GPS RESCUE | GPS rescue return-to-home | | 22 | AIRMODE | Air mode | | 26 | CRASH FLIP | Turtle mode (flip after crash) | | 27 | PREARM | Pre-arm |

Example — adding ANGLE mode on AUX2 (1500–2100):

aux 1 1 1 1500 2100

Launching the Simulation — Three-Terminal Workflow

Terminal 1 — Betaflight SITL:

cd ~/bf_sitl/config
~/betaflight/obj/main/betaflight_SITL.elf --ip 127.0.0.1

Terminal 2 — Gazebo Harmonic:

export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ros2_ws/install/betaflight_gazebo/lib
export GZ_SIM_RESOURCE_PATH=$HOME/ros2_ws/src/betaflight_sim/models
gz sim -r -v 4 ~/bf_sitl/worlds/betaflight_world.sdf

Terminal 3 — Closed-loop altitude controller:

python3 ~/bf_sitl/autonomy/msp_closed_loop_controller.py \
    --target-alt 2.0 --hover-base 1450 --kp 3 --ki 1 --kd 2

RC packet send frequency: The controller must send RC packets at a minimum of 50Hz. Betaflight's receiver timeout window is approximately 100ms — exceeding this triggers RXLOSS. This controller runs on a dedicated thread at 100Hz. Altitude sign convention: Betaflight's getEstimatedAltitudeCm() returns NED altitude (positive = downwards). The controller internally negates the value so that "up is positive".

Controller Code Walkthrough

The closed-loop controller msp_closed_loop_controller.py. Place this file under ~/bf_sitl/autonomy/.

File: msp_closed_loop_controller.py (closed-loop altitude hold controller)

#!/usr/bin/env python3
"""
Closed-loop altitude hold controller for Betaflight SITL.
- Reads drone state via MSP over TCP 5761 (altitude, attitude, IMU, battery)
- Sends RC via UDP 9004 (100Hz)
- PID altitude control with attitude monitoring

This is the SAME architecture used on real hardware (TCP -> UART serial).
"""
import socket, struct, time, signal, sys, argparse, threading
from msp_reader import MSPReader

BF_IP, BF_PORT = '127.0.0.1', 9004
NUM_CH = 16

class ClosedLoopController:
    def __init__(self, target_alt=2.0, hover_base=1450, kp=80, ki=5, kd=40):
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.msp = MSPReader()
        self.target_alt = target_alt
        self.hover_base = hover_base
        self.kp, self.ki, self.kd = kp, ki, kd
        self.integral = 0.0
        self.last_error = 0.0
        self.lock = threading.Lock()
        self.throttle = 1000
        self.aux1 = 1000
        self.running = True
        signal.signal(signal.SIGINT, self.stop)
        signal.signal(signal.SIGTERM, self.stop)

    def stop(self, *args):
        print("\nDisarming...")
        self.running = False
        with self.lock:
            self.throttle = 1000
            self.aux1 = 1000
        time.sleep(0.3)
        self.msp.stop()
        self.sock.close()
        sys.exit(0)

    def send_rc(self, channels):
        ts = time.time()
        pkt = struct.pack('<d' + 'H' * NUM_CH, ts, *channels)
        try:
            self.sock.sendto(pkt, (BF_IP, BF_PORT))
        except:
            pass

    def rc_thread(self):
        """Send RC at 100Hz independently of state reading"""
        while self.running:
            with self.lock:
                t, a = self.throttle, self.aux1
            ch = [1500] * NUM_CH
            ch[2], ch[4] = t, a
            self.send_rc(ch)
            time.sleep(0.01)

    def run(self):
        print("=" * 60)
        print("Betaflight SITL Closed-Loop Controller (MSP + UDP)")
        print(f"Target altitude: {self.target_alt}m")
        print(f"Hover base: {self.hover_base}  PID: kp={self.kp} ki={self.ki} kd={self.kd}")
        print("=" * 60)

        if not self.msp.start():
            print("ERROR: Failed to connect MSP reader. Is SITL running?")
            sys.exit(1)

        threading.Thread(target=self.rc_thread, daemon=True).start()

        # Phase 1: Disarmed init (3s)
        print("\nPhase 1: Init (disarmed, 3s)...")
        with self.lock:
            self.throttle = 1000
            self.aux1 = 1000
        time.sleep(3)

        # Phase 2: Arm (2s) -- throttle MUST be 1000 for arming
        print("Phase 2: Arming (2s)...")
        with self.lock:
            self.throttle = 1000
            self.aux1 = 2000
        time.sleep(2)

        # Phase 3: Altitude hold with PID
        print(f"Phase 3: Altitude hold at {self.target_alt}m...")
        last_time = time.time()

        while self.running:
            state = self.msp.get_state()
            now = time.time()
            dt = now - last_time
            if dt <= 0 or dt > 1.0:
                dt = 0.05

            # Get altitude from MSP (cm -> m).
            # Betaflight reports NED (positive=down), negate so "up" is positive.
            alt = -state.get('alt_cm', 0) / 100.0 if state else 0.0

            # PID control
            error = self.target_alt - alt
            self.integral += error * dt
            self.integral = max(-200, min(200, self.integral))
            deriv = (error - self.last_error) / dt
            pid = self.kp * error + self.ki * self.integral + self.kd * deriv
            thr = int(self.hover_base + pid)
            thr = max(1000, min(1900, thr))

            with self.lock:
                self.throttle = thr
                self.aux1 = 2000

            # Display
            roll = state.get('roll_deg', 0)
            pitch = state.get('pitch_deg', 0)
            volt = state.get('voltage_v', 0)
            rssi = state.get('rssi', 0)
            mode = state.get('flight_mode', 0)
            armed = bool(mode & (1 << 0)) if 'flight_mode' in state else False

            print(f"\r  alt={alt:.2f}m err={error:+.2f} thr={thr} "
                  f"r={roll:.0f}° p={pitch:.0f}° "
                  f"v={volt:.1f}V RSSI={rssi} armed={armed}  ",
                  end='', flush=True)

            self.last_error = error
            last_time = now
            time.sleep(0.05)

def main():
    p = argparse.ArgumentParser()
    p.add_argument('--target-alt', type=float, default=2.0)
    p.add_argument('--hover-base', type=int, default=1450)
    p.add_argument('--kp', type=float, default=80)
    p.add_argument('--ki', type=float, default=5)
    p.add_argument('--kd', type=float, default=40)
    args = p.parse_args()
    ClosedLoopController(args.target_alt, args.hover_base,
                         args.kp, args.ki, args.kd).run()

if __name__ == '__main__':
    main()

Core logic of the closed-loop controller msp_closed_loop_controller.py:

RC send thread (100Hz): Reads throttle and AUX values from shared variables, assembles a 16-channel array using AETR channel mapping, packs with struct.pack, and sends via UDP 9004
MSP telemetry thread (~20Hz): Loops sending MSPv1 request frames, parses responses to obtain altitude, attitude, voltage, etc.
Main PID loop (20Hz): Reads altitude, compares with target to get error, runs through PID computation, superimposes result onto the hover throttle baseline

MSPv1 request frame format (6 bytes): $ M < size(0) cmd CRC(cmd). Response frame: $ M > size cmd payload(N bytes) CRC.

Real-hardware correspondence: UDP 9004 → UART serial (MSP_SET_RAW_RC), TCP 5761 → UART serial (MSP telemetry). msp_reader.py and PID logic are fully reusable — simply replace the socket with a serial port.

Troubleshooting Quick Reference

| Symptom | Most Likely Cause | Diagnosis | |---------|-------------------|-----------| | No "new fdm" messages in SITL after Gazebo starts | Plugin not loaded or UDP unreachable | Run gz sim -v 4 and check for "Loaded system [BetaFlightPlugin]" | | SITL persistently shows RXLOSS | RC controller not running | Look for new rc in SITL terminal — indicates first RC packet received | | SITL shows "Arming disabled: THROTTLE" | Throttle channel value too high during arm attempt | Verify Phase 2 ch[2] is at 1000 | | SITL shows FAILSAFE RXLOSS | RC send interval exceeds 100ms | Ensure sending at ≥50Hz. Blocking operations (e.g., file I/O) must run in separate threads | | Props spinning but no takeoff | Hover throttle too low | Gradually increase --hover-base; IRIS needs approximately 1450–1600 | | Controller reads alt=0.00 and never changes | MSP response parsing failed or altitude sign error | Run msp_reader.py standalone to verify MSP communication | | Controller alt values correct but drone flies erratically | PID parameters too aggressive | Start with conservative values: --kp 2 --ki 1 --kd 1 | | Betaflight SITL compilation fails | Missing build dependencies | sudo apt install build-essential | | Plugin compilation fails with Could not find gz-sim8 | Gazebo dev packages not installed | sudo apt install -y libgz-sim8-dev libgz-transport13-dev libgz-msgs10-dev libgz-math7-dev libgz-plugin2-dev libsdformat14-dev | | Plugin compilation fails with colcon Cannot find package gz-sim7 | package.xml version numbers not updated | Change gz-sim7→gz-sim8 and gz-transport12→gz-transport13 in package.xml | | Plugin compilation fails with gz/msgs/imu.pb.h not found | gz-msgs10-dev not installed | sudo apt install libgz-msgs10-dev | | Plugin compilation fails with "no matching function for Subscribe" | std::function + std::bind not applied correctly | Cross-check against Check 2 code examples | | gz sim --versions shows multiple versions | Multiple Gazebo versions installed | Keep only gz-harmonic |

Summary

This article provided a complete walkthrough for the Betaflight SITL + Gazebo Harmonic simulation environment, covering SITL compilation, four critical Gazebo plugin compatibility adaptations, CLI arming configuration, AUX channel mode setup, and a Python closed-loop altitude controller implementation. Betaflight's SITL mode delivers hardware-free, closed-loop simulation capability for flight controller algorithm validation — when paired with the Gazebo physics engine, it can verify the complete control pipeline from sensor input to motor output. To add visual perception to your simulation, check out YOLOv8 Object Detection in PX4 Drone Simulation.

Contributions and feedback welcome at the GitHub repository!

References

Betaflight SITL Documentation
OSRF vehicle_gateway Repository
Betaflight source code -- SITL platform implementation -- sitl.c, target.h, udplink.c
Gazebo Harmonic Installation Docs
ROS2 + Gazebo bridge package -- ros-humble-ros-gzharmonic API documentation
Betaflight Configurator

Betaflight+Gazebo 软件在环仿真教程

Sat, 11 Jul 2026 00:00:00 GMT

关于 ROS2：本教程的仿真链路（Gazebo ↔ SITL ↔ Controller）通过 UDP 直连，运行时完全不需要 ROS2。ROS2 仅在编译 Gazebo 插件时用到（因为上游 vehicle_gateway 项目使用 colcon 构建系统），插件编译完成后不再依赖 ROS2。

教程较为复杂，主要是讲解其中的框架。完整代码和配置文件欢迎在 GitHub 仓库上找到，欢迎提交 commit 改进。如果喜欢，麻烦 star 一下支持作者。

简介

Betaflight 是目前无人机竞速（FPV Racing）和自由飞（Freestyle）领域最流行的开源飞控固件，广泛运行在 STM32/AT32 等 MCU 上。Betaflight 提供 SITL（Software-In-The-Loop，软件在环）模式，允许开发者在 Gazebo 物理仿真引擎中运行完整的 Betaflight 飞控固件。

SITL 的核心思路是：将 Betaflight 固件编译为 x86_64 Linux 可执行文件，飞控的传感器输入（IMU、气压计、GPS）来自 Gazebo 仿真环境通过 UDP 发送的仿真数据，飞控的电机输出（PWM 指令）同样通过 UDP 发回 Gazebo 驱动物理引擎中的螺旋桨。这样开发者可以在 PC 上完整验证飞控逻辑和控制算法，无需真实硬件。

本文提供一个在 Ubuntu 22.04 上搭建 Betaflight SITL + Gazebo Harmonic + Python 自主控制仿真环境的完整流程。在开始之前，感谢 OSRF vehicle_gateway 项目和 Betaflight 社区提供的开源代码和文档，本教程大量参考了这些资料。

前置环境

按照笔者之前的 ROS2+PX4 仿真教程完成了以下基础环境的配置：

Ubuntu 22.04 系统
ROS2 Humble（推荐使用鱼香ROS一键安装：wget http://fishros.com/install -O fishros && . fishros）
Gazebo Harmonic（gz-sim8，版本 8.9.0）
ROS2-Gazebo 通信桥 ros-humble-ros-gzharmonic

在终端中确认环境正确：

ros2 --version       # 输出应包含 "humble"
gz sim --versions    # 输出应为 "Gazebo Sim 8.9.0"
dpkg -l | grep ros-humble-ros-gzharmonic  # 应显示已安装

克隆本教程配套仓库：

cd ~
git clone https://github.com/Jinyao-Chen/bf_sitl.git ~/bf_sitl

仓库目录结构：

~/bf_sitl/
├── autonomy/
│   ├── msp_reader.py                    # MSP 遥测读取器
│   ├── msp_closed_loop_controller.py    # 闭环定高控制器
│   └── autonomous_controller.py         # 开环起飞控制器
├── worlds/
│   └── betaflight_world.sdf             # Gazebo 世界文件
├── config/
│   └── bf_cli_config.py                 # 首次配置脚本
└── betaflight_gazebo_patches/           # 插件补丁（供编译参考）

安装必要工具和 Gazebo 开发包（编译插件必需，运行时 gz-harmonic 不包含这些头文件）：

pip3 install --user websockify
sudo apt-get install -y socat

# Gazebo Harmonic 开发包——编译自定义 Gazebo 插件必需！
sudo apt-get install -y \
    libgz-sim8-dev \
    libgz-transport13-dev \
    libgz-msgs10-dev \
    libgz-math7-dev \
    libgz-plugin2-dev \
    libsdformat14-dev

websockify：提供 WebSocket → TCP 的代理功能，用于在浏览器中连接 Betaflight 地面站配置工具
socat：用于创建虚拟串口（pty），可将 Betaflight 的 TCP UART 映射为本地虚拟串口设备

确保网络可以正常访问 GitHub，否则克隆大型仓库时会中途闪退。

仿真链路架构

在开始动手配置之前，我们先介绍整个仿真链路，这有助于后续遇到问题时定位原因。仿真链路由四个进程组成，它们通过 UDP 端口互相通信：

端口分配与数据流（Betaflight 源码 sitl.c 第 192-195 行）：

| 端口 | 方向 | 用途 | 数据包类型 | |------|------|------|-----------| | 9001 | SITL → 外部 | Raw PWM（备用于 RealFlightBridge） | servo_packet_raw | | 9002 | SITL → Gazebo | 电机转速指令 [0.0, 1.0]（3D 模式为 [-1.0, 1.0]） | servo_packet | | 9003 | Gazebo → SITL | 飞行状态数据（IMU、位置、速度、姿态、气压） | fdm_packet | | 9004 | Controller → SITL | 16 通道 RC 遥控信号 [1000-2000] | rc_packet | | TCP 5761 | 双向 | MSP 协议（配置 + 遥测回传） | N/A |

数据包结构体（Betaflight 源码 target.h 第 224-246 行）：

// fdm_packet: Gazebo 插件 → SITL
typedef struct {
    double timestamp;
    double imu_angular_velocity_rpy[3];    // 角速度 (rad/s)
    double imu_linear_acceleration_xyz[3]; // 线加速度 (m/s²)
    double imu_orientation_quat[4];        // 姿态四元数 (w,x,y,z)
    double velocity_xyz[3];                // 速度 (m/s, ENU 坐标系)
    double position_xyz[3];                // 位置
    double pressure;                       // 气压 (Pa)
} fdm_packet;

// servo_packet: SITL → Gazebo 插件
typedef struct {
    float motor_speed[4];   // [0.0, 1.0] 或 3D 模式 [-1.0, 1.0]
} servo_packet;

// rc_packet: Controller → SITL
typedef struct {
    double timestamp;
    uint16_t channels[16];   // PWM [1000, 2000]
} rc_packet;

Gazebo 插件（BetaflightPlugin）在每个仿真更新周期中执行两个阶段：

PreUpdate（仿真步进前）：调用 ReceiveServoPacket() 从 UDP 9002 读取 servo_packet，将电机转速换算为关节力/速度指令
PostUpdate（仿真步进后）：从 IMU 传感器获取角速度和加速度数据，从 Gazebo Entity Component Manager 获取位姿和线速度，填充为 fdm_packet，进行坐标系变换后通过 UDP 发送到 127.0.0.1:9003

Betaflight SITL 源码下载及编译

cd ~
git clone https://github.com/betaflight/betaflight.git
cd betaflight

注意：我们使用的不是 4.5.x 版，而是 master 分支（版本号 2026.6.0-alpha），因为 4.5.x 正式版的 SITL 代码缺少 Gazebo 的原生支持（ENABLE_GAZEBO_BRIDGE 特性在 4.5.4 之后才引入 master）。

与编译 PX4 需要 ARM 交叉编译工具链不同，SITL 模式直接使用主机 GCC 编译：

make TARGET=SITL -j$(nproc)

编译成功标志：

Linking SITL
   text    data     bss     dec     hex  filename
 389784   21804   77376  488964   77604  ./obj/main/betaflight_SITL.elf

验证：

./obj/main/betaflight_SITL.elf --help
# 输出: Betaflight SITL Usage: ./obj/main/betaflight_SITL.elf [options]

为什么 SITL 可以单独运行？因为 Betaflight SITL 用软件模拟了真实飞控的硬件外设：虚拟 IMU（accgyro_virtual.c）接收 Gazebo 的角速度和加速度数据；虚拟气压计（barometer_virtual.c）接收气压/高度数据；虚拟 GPS（gps_virtual.c）接收位置数据；虚拟 EEPROM（文件读写）保存配置为 eeprom.bin；虚拟串口（serial_tcp.c）以 TCP 替代真实 UART。

编译完成后建议备份：zip -r betaflight.zip betaflight/

Gazebo 插件的获取与编译

Betaflight 本身不包含 Gazebo 插件。我们需要 OSRF 的 vehicle_gateway 项目中的 betaflight_gazebo 插件。需要注意：vehicle_gateway 官方针对 Gazebo Garden（gz-sim7），而我们的环境是 Gazebo Harmonic（gz-sim8），需要进行适配修改。

步骤 1：创建 ROS2 工作空间并克隆插件

说明：Gazebo 插件 libBetaflightPlugin.so 需要通过 colcon（ROS2 的构建工具）编译，因此需要放在一个 ROS2 工作空间中。如果你已有 ~/ros2_ws，可以直接使用；如果没有，执行以下命令创建：

mkdir -p ~/ros2_ws/src

然后克隆 vehicle_gateway 并跳过不需要的子包：

cd ~/ros2_ws/src
git clone https://github.com/osrf/vehicle_gateway.git vehicle_gateway

cd ~/ros2_ws/src/vehicle_gateway
for pkg in gz_aerial_plugins vehicle_gateway vehicle_gateway_betaflight \
    vehicle_gateway_demo vehicle_gateway_integration_test vehicle_gateway_multi \
    vehicle_gateway_px4 vehicle_gateway_python vehicle_gateway_python_helpers \
    vehicle_gateway_sim_performance px4_sim betaflight_configurator \
    betaflight_demo qgroundcontrol; do
    touch "$pkg/COLCON_IGNORE"
done

步骤 2：适配 Gazebo Harmonic（需改 2 个文件的版本号，其余 3 项代码修复已在最新上游代码中完成）

当前 OSRF vehicle_gateway 上游代码已包含了大部分 Gazebo Harmonic 兼容修复。只有 CMakeLists.txt 的库版本号仍需要手动修改。

必须修改 — 适配库版本号

需要改两个文件：CMakeLists.txt 和 package.xml。二者中的版本号必须同步，否则 colcon 依赖解析会失败。

（1）编辑 ~/ros2_ws/src/vehicle_gateway/betaflight_gazebo/CMakeLists.txt

全文搜索替换以下两对版本号。不仅 find_package 行要改，target_link_libraries 中带版本的 target 名（如 gz-sim7::gz-sim7）也要同步替换：

所有 gz-sim7 → gz-sim8
所有 gz-transport12 → gz-transport13

（2）编辑 ~/ros2_ws/src/vehicle_gateway/betaflight_gazebo/package.xml

将 <depend> 中的版本号也同步更新：

<depend>gz-sim7</depend> → <depend>gz-sim8</depend>
<depend>gz-transport12</depend> → <depend>gz-transport13</depend>

为什么？ Gazebo Garden (gz-sim7, gz-transport12) 和 Gazebo Harmonic (gz-sim8, gz-transport13) 的 ABI 不兼容，插件必须链接与 Gazebo 服务器进程相同版本的库。gz-math7 和 gz-plugin2 两个版本共享，无需修改。package.xml 不改的话 colcon 会尝试用 rosdep 解析 gz-sim7，在只装了 Harmonic 的环境中必然失败。

验证项 — 以下三项已在最新上游代码中修复，无需手动改，但建议逐项确认你的克隆包含它们：

检查一：头文件引用（BetaflightPlugin.cpp 第 35-39 行）

~/ros2_ws/src/vehicle_gateway/betaflight_gazebo/src/BetaflightPlugin.cpp 中应已有以下头文件：

#include <functional>
#include <gz/msgs/imu.pb.h>
#include <gz/msgs/fluid_pressure.pb.h>
#include <gz/msgs/double.pb.h>

背景说明：Gazebo Harmonic (gz-sim8) 的 System.hh 不再传递包含 protobuf 消息头文件，因此需要显式 include。上游已修复，如果你的克隆缺少这几行，手动补上。

检查二：IMU 和气压传感器订阅（PreUpdate 第 440-443 行，Configure 第 296-301 行）

IMU 订阅在 PreUpdate 函数中（约第 440 行），气压传感器订阅在 Configure 函数中（约第 296 行）。二者已使用 std::function + std::bind 写法，无需改动：

PreUpdate 中 IMU 订阅（约第 440 行）：

std::function<void(const gz::msgs::IMU&)> imuCb =
    std::bind(&BetaFlightPluginPrivate::ImuCb,
              this->dataPtr.get(), std::placeholders::_1);
this->dataPtr->node.Subscribe(imuTopicName, imuCb);

Configure 中气压传感器订阅（约第 296 行）：

std::function<void(const gz::msgs::FluidPressure&)> airPressureCb =
    std::bind(&BetaFlightPluginPrivate::onAirPressureMessageReceived,
              this->dataPtr.get(), std::placeholders::_1);
this->dataPtr->node.Subscribe(
    "/world/empty_betaflight_world/model/iris_with_Betaflight/model/iris_with_standoffs/"
    "link/imu_link/sensor/air_pressure_sensor/air_pressure",
    airPressureCb);

背景说明：Gazebo Garden 的 gz-transport12 允许 Node::Subscribe 直接接受成员函数指针进行模板推导。gz-transport13 收紧后，必须用 std::function 显式包装。IMU 回调 ImuCb 签名为 void(const gz::msgs::IMU&)，气压回调 onAirPressureMessageReceived 签名为 void(const gz::msgs::FluidPressure&)。

检查三：PostUpdate 死锁修复（PostUpdate 第 478-484 行）

PostUpdate 函数中不应包含 betaflightOnline 条件：

if (!_info.paused && _info.simTime > this->dataPtr->lastControllerUpdateTime)
{
    double t = ...;
    this->SendState(t, _ecm);
    ...
}

如果出现 && this->dataPtr->betaflightOnline，请删除。原因：初始时 SITL 等 Gazebo 发 fdm_packet，而 Gazebo 因为 betaflightOnline == false 不发 fdm_packet，形成死锁。

步骤 3：编译插件

cd ~/ros2_ws
source /opt/ros/humble/setup.bash
colcon build --packages-select betaflight_gazebo

验证：ls -lh ~/ros2_ws/install/betaflight_gazebo/lib/libBetaflightPlugin.so（约 8.7M）

仿真世界的配置

世界文件和无人机模型已包含在本教程配套仓库中。只需软链接 IRIS 四旋翼模型（由 vehicle_gateway 提供）：

ln -sf ~/ros2_ws/src/vehicle_gateway/betaflight_sim/models/iris_with_standoffs \
    ~/ros2_ws/src/vehicle_gateway/betaflight_sim/models/

世界文件位于 ~/bf_sitl/worlds/betaflight_world.sdf，核心插件配置：

<plugin name="BetaFlightPlugin" filename="BetaflightPlugin">
    <fdm_addr>127.0.0.1</fdm_addr>
    <fdm_port_in>9002</fdm_port_in>
    <listen_addr>127.0.0.1</listen_addr>
    <modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
    <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
    <imuName>iris_with_standoffs::imu_link::imu_sensor</imuName>
    <control channel="0">
        <jointName>iris_with_standoffs::rotor_0_joint</jointName>
        <multiplier>838</multiplier>
    </control>
    <!-- channels 1-3 similarly configured, multipliers: 838, -838, -838 -->
</plugin>

坐标系变换详解：Gazebo 使用 ENU（X=East, Y=North, Z=Up），Betaflight 期望 NED（X=North, Y=East, Z=Down）。gazeboXYZToNED 的 Yaw=180° 进行朝向旋转，插件内部的 SendState() 进一步应用 Rz(π/2) 旋转完成完整变换。

配置解锁 —— 让飞控能够 ARM

Betaflight 的安全设计原则是"默认禁止一切飞行操作"——出厂固件没有配置 ARM（解锁）开关，即使收到 RC 信号中 AUX1=2000，飞控也不知道这表示"解锁请求"。我们需要告诉飞控：当 AUX1 通道值在 1700-2100 范围时，激活 ARM 模式。

配置保存在 eeprom.bin 文件中（相当于飞控的"设置记忆"），通过 TCP 端口 5761 的 CLI（命令行接口）进行设置。

第一步：清除旧配置（重要！）

如果你之前运行过 SITL，旧的 eeprom.bin 可能包含错误配置（典型问题是接收机模式被改成了串口 RX_SERIAL 而非 UDP），导致飞控永久无法解锁。首次配置前务必删除旧文件：

cd ~/bf_sitl/config
rm -f eeprom.bin

为什么要删？ eeprom.bin 是飞控的持久化存储。SITL 每次启动时优先读取 eeprom.bin，只有在文件不存在时才使用出厂默认值。出厂默认已正确启用 UDP 接收机模式（FEATURE_RX_UDP），确保飞控能通过 UDP 端口 9004 接收 RC 遥控信号。如果旧的 eeprom.bin 里存了错误的接收机设置，SITL 就会忽略 RC 信号，飞控永远显示 RXLOSS、永远无法解锁。

第二步：启动 SITL 并运行配置脚本

在终端 1 中启动 SITL（此时不需要 Gazebo）：

cd ~/bf_sitl/config
~/betaflight/obj/main/betaflight_SITL.elf --ip 127.0.0.1

等待出现 bind port 5761 for UART1，然后打开终端 2 运行配置脚本：

python3 ~/bf_sitl/config/bf_cli_config.py

脚本内容（bf_cli_config.py，仓库 config/ 目录下）：

#!/usr/bin/env python3
"""Configure Betaflight SITL via CLI — set ARM on AUX1"""
import socket, time, sys

HOST, PORT = '127.0.0.1', 5761

def recv_all(sock, timeout=1.0):
    """读取所有可用数据，超时后返回"""
    sock.settimeout(timeout)
    data = b''
    try:
        while True:
            chunk = sock.recv(4096)
            if not chunk: break
            data += chunk
    except socket.timeout: pass
    except Exception: pass
    return data

def main():
    # 1. 等待 SITL 的 TCP 端口就绪（最多等 30 秒）
    print("Waiting for SITL TCP port 5761...")
    for i in range(30):
        try:
            s = socket.socket(); s.settimeout(1)
            s.connect((HOST, PORT)); s.close()
            print(f"Connected after {i+1}s"); break
        except Exception:
            time.sleep(1)
    else:
        print("ERROR: SITL not ready after 30s"); sys.exit(1)

    # 2. 建立 TCP 连接，排空初始欢迎信息
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    sock.settimeout(5)
    sock.connect((HOST, PORT))
    time.sleep(1)
    recv_all(sock, 0.5)

    # 3. 发送单独一个 '#' 进入 CLI 模式
    #    注意：只发 '#'，不带 \n。Betaflight 收到 '#'（而非 MSP 帧头 '$'）时进入 CLI
    print("Entering CLI mode...")
    sock.sendall(b'#')
    time.sleep(2)
    resp = recv_all(sock, 1.0)
    if resp:
        print(resp.decode('utf-8', errors='replace')[:300])

    # 4. 配置 ARM 到 AUX1（通道值 1700-2100 时触发 ARM 模式）
    #    aux 0 0 0 1700 2100 参数：
    #      槽位0, 模式0(ARM), AUX通道0(AUX1=第5通道), 触发范围1700-2100μs
    print("\nSetting ARM on AUX1 (1700-2100)...")
    sock.sendall(b'aux 0 0 0 1700 2100\n')
    time.sleep(0.5)
    resp = recv_all(sock, 1.0)
    if resp:
        print(resp.decode('utf-8', errors='replace')[-200:])

    # 5. 保存到 eeprom.bin 并重启飞控固件
    print("Saving to EEPROM...")
    try:
        sock.sendall(b'save\n')
        time.sleep(1)
        resp = recv_all(sock, 3.0)
        if resp:
            print(resp.decode('utf-8', errors='replace')[-300:])
    except Exception as e:
        print(f"Save: {e} (reboot is normal)")

    try: sock.close()
    except: pass
    print("\nDone! ARM on AUX1 configured.")

if __name__ == '__main__':
    main()

第三步：重启 SITL 使配置生效

save 命令会让飞控内部重新初始化，但 Linux 进程本身不会自动退出。你需要在终端 1 按 Ctrl+C 手动停止，然后重新启动：

~/betaflight/obj/main/betaflight_SITL.elf --ip 127.0.0.1

为什么需要手动重启？ save 让飞控固件内部重新初始化并加载新配置到内存，但终端中运行的 Linux 进程仍然占用着端口。Ctrl+C 杀掉进程再重启，确保端口绑定和运行状态都是干净的。

重启后日志中应出现 [FLASH_Unlock] loaded 'eeprom.bin'，表示配置已正确加载。至此 ARM 配置完成，后续正常启动仿真时直接跳到下一步即可。

aux 0 0 0 1700 2100 命令含义：第 1 个 0 = 配置槽位编号；第 2 个 0 = 模式 ID（0 = ARM）；第 3 个 0 = AUX 通道索引（0 = AUX1）；1700 2100 = 触发范围（当 AUX1 通道值在此范围内时激活 ARM 模式）。

AUX 通道模式对照表

aux <槽位> <模式ID> <AUX通道> <范围低> <范围高>

常用模式 ID：

| ID | 名称 | 功能 | |----|------|------| | 0 | ARM | 解锁/上锁飞控 | | 1 | ANGLE | 自稳模式 | | 2 | HORIZON | 半自稳模式 | | 5 | HEADFREE | 无头模式 | | 11 | GPS RESCUE | GPS 救援返航 | | 22 | AIRMODE | 空中模式 | | 26 | CRASH FLIP | 反乌龟模式 | | 27 | PREARM | 预解锁 |

示例——添加 ANGLE 模式在 AUX2（1500-2100）：

aux 1 1 1 1500 2100

启动仿真 —— 三个终端流程

终端 1 —— Betaflight SITL：

cd ~/bf_sitl/config
~/betaflight/obj/main/betaflight_SITL.elf --ip 127.0.0.1

终端 2 —— Gazebo Harmonic：

export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ros2_ws/install/betaflight_gazebo/lib
export GZ_SIM_RESOURCE_PATH=$HOME/ros2_ws/src/vehicle_gateway/betaflight_sim/models
gz sim -r -v 4 ~/bf_sitl/worlds/betaflight_world.sdf

终端 3 —— 闭环定高控制器：

# 闭环定高控制器（推荐，需 MSP 遥测）
python3 ~/bf_sitl/autonomy/msp_closed_loop_controller.py \
    --target-alt 2.0 --hover-base 1450 --kp 80 --ki 5 --kd 40

# 或开环起飞控制器（无需 MSP 遥测，仅 RC 控制）
python3 ~/bf_sitl/autonomy/autonomous_controller.py --hover-throttle 1450

RC 包发送频率：控制器必须以 ≥50Hz 的频率发送 RC 包。Betaflight 接收机超时窗口约 100ms，超时即触发 RXLOSS。本控制器在独立线程中以 100Hz 运行。 高度符号约定：Betaflight 的 getEstimatedAltitudeCm() 返回 NED 高度（正值=向下），控制器内部取反使"向上为正"。

控制器代码解析

控制器代码均已包含在本教程配套仓库的 autonomy/ 目录中，克隆后即可使用，无需手动复制。

msp_closed_loop_controller.py（闭环定高控制器）

#!/usr/bin/env python3
"""
Closed-loop altitude hold controller for Betaflight SITL.
- Reads drone state via MSP over TCP 5761 (altitude, attitude, IMU, battery)
- Sends RC via UDP 9004 (100Hz)
- PID altitude control with attitude monitoring

This is the SAME architecture used on real hardware (TCP -> UART serial).
"""
import socket, struct, time, signal, sys, argparse, threading
from msp_reader import MSPReader

BF_IP, BF_PORT = '127.0.0.1', 9004
NUM_CH = 16

class ClosedLoopController:
    def __init__(self, target_alt=2.0, hover_base=1450, kp=80, ki=5, kd=40):
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.msp = MSPReader()
        self.target_alt = target_alt
        self.hover_base = hover_base
        self.kp, self.ki, self.kd = kp, ki, kd
        self.integral = 0.0
        self.last_error = 0.0
        self.lock = threading.Lock()
        self.throttle = 1000
        self.aux1 = 1000
        self.running = True
        signal.signal(signal.SIGINT, self.stop)
        signal.signal(signal.SIGTERM, self.stop)

    def stop(self, *args):
        print("\nDisarming...")
        self.running = False
        with self.lock:
            self.throttle = 1000
            self.aux1 = 1000
        time.sleep(0.3)
        self.msp.stop()
        self.sock.close()
        sys.exit(0)

    def send_rc(self, channels):
        ts = time.time()
        pkt = struct.pack('<d' + 'H' * NUM_CH, ts, *channels)
        try:
            self.sock.sendto(pkt, (BF_IP, BF_PORT))
        except:
            pass

    def rc_thread(self):
        """Send RC at 100Hz independently of state reading"""
        while self.running:
            with self.lock:
                t, a = self.throttle, self.aux1
            ch = [1500] * NUM_CH
            ch[2], ch[4] = t, a
            self.send_rc(ch)
            time.sleep(0.01)

    def run(self):
        print("=" * 60)
        print("Betaflight SITL Closed-Loop Controller (MSP + UDP)")
        print(f"Target altitude: {self.target_alt}m")
        print(f"Hover base: {self.hover_base}  PID: kp={self.kp} ki={self.ki} kd={self.kd}")
        print("=" * 60)

        if not self.msp.start():
            print("ERROR: Failed to connect MSP reader. Is SITL running?")
            sys.exit(1)

        threading.Thread(target=self.rc_thread, daemon=True).start()

        # Phase 1: Disarmed init (3s)
        print("\nPhase 1: Init (disarmed, 3s)...")
        with self.lock:
            self.throttle = 1000
            self.aux1 = 1000
        time.sleep(3)

        # Phase 2: Arm (2s) -- throttle MUST be 1000 for arming
        print("Phase 2: Arming (2s)...")
        with self.lock:
            self.throttle = 1000
            self.aux1 = 2000
        time.sleep(2)

        # Phase 3: Altitude hold with PID
        print(f"Phase 3: Altitude hold at {self.target_alt}m...")
        last_time = time.time()

        while self.running:
            state = self.msp.get_state()
            now = time.time()
            dt = now - last_time
            if dt <= 0 or dt > 1.0:
                dt = 0.05

            # Get altitude from MSP (cm -> m).
            # Betaflight reports NED (positive=down), negate so "up" is positive.
            alt = -state.get('alt_cm', 0) / 100.0 if state else 0.0

            # PID control
            error = self.target_alt - alt
            self.integral += error * dt
            self.integral = max(-200, min(200, self.integral))
            deriv = (error - self.last_error) / dt
            pid = self.kp * error + self.ki * self.integral + self.kd * deriv
            thr = int(self.hover_base + pid)
            thr = max(1000, min(1900, thr))

            with self.lock:
                self.throttle = thr
                self.aux1 = 2000

            # Display
            roll = state.get('roll_deg', 0)
            pitch = state.get('pitch_deg', 0)
            volt = state.get('voltage_v', 0)
            rssi = state.get('rssi', 0)
            mode = state.get('flight_mode', 0)
            armed = bool(mode & (1 << 0)) if 'flight_mode' in state else False

            print(f"\r  alt={alt:.2f}m err={error:+.2f} thr={thr} "
                  f"r={roll:.0f}° p={pitch:.0f}° "
                  f"v={volt:.1f}V RSSI={rssi} armed={armed}  ",
                  end='', flush=True)

            self.last_error = error
            last_time = now
            time.sleep(0.05)

def main():
    p = argparse.ArgumentParser()
    p.add_argument('--target-alt', type=float, default=2.0)
    p.add_argument('--hover-base', type=int, default=1450)
    p.add_argument('--kp', type=float, default=80)
    p.add_argument('--ki', type=float, default=5)
    p.add_argument('--kd', type=float, default=40)
    args = p.parse_args()
    ClosedLoopController(args.target_alt, args.hover_base,
                         args.kp, args.ki, args.kd).run()

if __name__ == '__main__':
    main()

核心逻辑：

RC 发送线程（100Hz）：从共享变量读取油门和 AUX 值，按 AETR 通道映射组装 16 通道数组，用 struct.pack 封包通过 UDP 9004 发送
MSP 遥测线程（~20Hz）：循环发送 MSPv1 请求帧，解析响应获取高度、姿态、电压等
主 PID 循环（20Hz）：读取高度，与目标值比较得 error，经 PID 计算叠加到悬停油门基准值

MSPv1 请求帧格式（6 字节）：$ M < size(0) cmd CRC(cmd)。响应帧：$ M > size cmd payload(N字节) CRC。

真机对应关系：UDP 9004 → UART 串口（MSP_SET_RAW_RC），TCP 5761 → UART 串口（MSP 遥测）。msp_reader.py 和 PID 逻辑完全复用，仅将 socket 替换为 serial。

msp_reader.py（MSP 遥测读取器）

MSP 遥测读取器是整个控制链路的关键组件。它通过 TCP 5761 端口连接 SITL，循环查询 5 条 MSP 命令（高度、姿态、IMU、状态、电压），将飞控遥测数据解析为 Python 字典供控制器使用。

#!/usr/bin/env python3
"""
MSP telemetry reader for Betaflight SITL.
Connects to TCP 5761 and queries multiple MSP commands to read full drone state.
"""
import socket, struct, time, threading, select

class MSPReader:
    """Connects to SITL TCP 5761 and reads full drone telemetry via MSP"""

    MSP_CMDS = {
        'altitude': (109, '<ih'),           # alt_cm (int32), vario_cm_s (int16)
        'attitude': (108, '<hhh'),          # roll_ddeg, pitch_ddeg, yaw_deg
        'raw_imu':  (102, '<hhhhhhhhh'),    # acc[3], gyro[3], mag[3] (raw int16)
        'status':   (101, '<HHHIB'),       # cycleTime, i2cErr, sensors, mode, profile
        'analog':   (110, '<BHHh'),         # voltage_dV, mAh, rssi, amperage_cA
    }

    def __init__(self, host='127.0.0.1', port=5761):
        self.state = {}
        self.state_lock = threading.Lock()
        self.running = False
        self.host, self.port = host, port
        self.sock = None

    def connect(self, timeout=10):
        deadline = time.time() + timeout
        while time.time() < deadline:
            try:
                self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                self.sock.settimeout(0.5)
                self.sock.connect((self.host, self.port))
                print(f"[MSP] Connected to {self.host}:{self.port}")
                return True
            except:
                self.sock.close()
                time.sleep(0.5)
        print("[MSP] Connection timeout")
        return False

    def _build_msp_request(self, cmd):
        """Build MSPv1 request frame (no payload). CRC = cmd ^ 0x00."""
        return b'$M<' + bytes([0]) + bytes([cmd]) + bytes([cmd])

    def _parse_msp_response(self, data, cmd_id, fmt):
        """Parse MSP response. Handles both v1 and v2 framing."""
        idx = data.find(b'$M>')
        if idx < 0:
            idx = data.find(b'$X>')
        if idx < 0:
            return None
        if data[idx:idx+3] == b'$M>':
            if idx + 5 > len(data): return None
            size = data[idx + 3]
            cmd = data[idx + 4]
            if cmd != cmd_id or idx + 5 + size > len(data): return None
            payload = data[idx + 5: idx + 5 + size]
            try:
                # slice to exact format size — MSP payload may be larger
                return struct.unpack(fmt, payload[:struct.calcsize(fmt)])
            except: return None
        elif data[idx:idx+3] == b'$X>':
            if idx + 8 > len(data): return None
            cmd = data[idx + 4] | (data[idx + 5] << 8)
            size = data[idx + 6] | (data[idx + 7] << 8)
            if cmd != cmd_id or idx + 8 + size > len(data): return None
            payload = data[idx + 8: idx + 8 + size]
            try:
                return struct.unpack(fmt, payload[:struct.calcsize(fmt)])
            except: return None
        return None

    def _read_all(self):
        data = b''
        try:
            while True:
                ready, _, _ = select.select([self.sock], [], [], 0.01)
                if not ready: break
                chunk = self.sock.recv(4096)
                if not chunk: break
                data += chunk
        except: pass
        return data

    def _send_and_recv(self, cmd_id, fmt):
        try:
            self.sock.sendall(self._build_msp_request(cmd_id))
            for _ in range(5):
                time.sleep(0.02)
                data = self._read_all()
                if data:
                    result = self._parse_msp_response(data, cmd_id, fmt)
                    if result is not None: return result
        except: pass
        return None

    def query_all(self):
        result = {}
        r = self._send_and_recv(109, '<ih')
        if r: result['alt_cm'] = r[0]; result['vario_cm_s'] = r[1]
        r = self._send_and_recv(108, '<hhh')
        if r: result['roll_deg'] = r[0] / 10.0; result['pitch_deg'] = r[1] / 10.0; result['yaw_deg'] = r[2]
        r = self._send_and_recv(102, '<hhhhhhhhh')
        if r: result['acc_x'] = r[0]; result['acc_y'] = r[1]; result['acc_z'] = r[2]; result['gyro_x'] = r[3]; result['gyro_y'] = r[4]; result['gyro_z'] = r[5]
        r = self._send_and_recv(101, '<HHHIB')
        if r: result['cycle_time_us'] = r[0]; result['sensors'] = r[2]; result['flight_mode'] = r[3]; result['pid_profile'] = r[4]
        r = self._send_and_recv(110, '<BHHh')
        if r: result['voltage_v'] = r[0] / 10.0; result['mah'] = r[1]; result['rssi'] = r[2]
        return result

    def reader_thread(self):
        self.running = True
        while self.running:
            result = self.query_all()
            if result:
                with self.state_lock: self.state.update(result)
            time.sleep(0.05)

    def start(self):
        if not self.connect(): return False
        t = threading.Thread(target=self.reader_thread, daemon=True); t.start()
        return True

    def stop(self):
        self.running = False
        if self.sock: self.sock.close()

    def get_state(self):
        with self.state_lock: return dict(self.state)

MSP_STATUS 格式要点：<HHHIB> = cycleTime(2B) + i2cError(2B) + sensors(2B) + flightModeFlags(4B) + pidProfile(1B) = 11 字节。flightModeFlags 的 bit 0 对应 BOXARM（ARM 状态）。注意 struct.unpack 要求 buffer 大小精确匹配格式，必须用 payload[:struct.calcsize(fmt)] 截断，因为 MSP_STATUS 的实际 payload 远超 11 字节。

常见问题排查汇总

| 现象 | 最可能的原因 | 排查方法 | |------|-------------|---------| | Gazebo 启动后 SITL 无 new fdm 消息 | 插件未加载或 UDP 不通 | gz sim -v 4 查看是否出现 "Loaded system [BetaFlightPlugin]" | | SITL 一直显示 RXLOSS（RC 已发但 RXLOSS 不消失） | 旧 eeprom.bin 接收机模式设置错误 | rm ~/bf_sitl/config/eeprom.bin 删除后重启 SITL | | SITL 一直显示 RXLOSS（无 new rc 消息） | RC 控制器未启动 | 确认控制器已在终端 3 运行，SITL 终端应出现 new rc | | SITL 显示 Arming disabled: THROTTLE | 解锁时油门通道值过高 | 检查 Phase 2 阶段 ch[2] 是否为 1000 | | SITL 显示 FAILSAFE RXLOSS | RC 发送间隔超过 100ms | 确保以 ≥50Hz 发送。阻塞操作（如文件读写）必须放入独立线程 | | 螺旋桨转了但不起飞 | 悬停油门过低 | 逐步增加 --hover-base，IRIS 约需 1450-1600 | | 闭环控制器 alt=0.00 不变 | MSP 响应解析失败或高度符号错误 | 单独跑 msp_reader.py 验证 MSP 通信 | | 闭环控制器 alt 数值正确但飞机失控 | PID 参数过于激进 | 从小参数开始：--kp 2 --ki 1 --kd 1 | | 编译 Betaflight SITL 失败 | 缺少构建依赖 | sudo apt install build-essential | | 编译插件时 Could not find gz-sim8 | 缺少 Gazebo 开发包 | sudo apt install -y libgz-sim8-dev libgz-transport13-dev libgz-msgs10-dev libgz-math7-dev libgz-plugin2-dev libsdformat14-dev | | 编译插件时 colcon 报 Cannot find package gz-sim7 | package.xml 未改版本号 | 将 package.xml 中 gz-sim7 改为 gz-sim8，gz-transport12 改为 gz-transport13 | | 编译插件失败，gz/msgs/imu.pb.h not found | gz-msgs10-dev 未安装 | sudo apt install libgz-msgs10-dev | | 编译插件失败，no matching function for Subscribe | std::function + std::bind 未正确实施 | 对照检查二的代码示例逐一核实 | | gz sim --versions 显示多版本 | 混装了多个 Gazebo 版本 | 只保留 gz-harmonic |

总结

本文完整介绍了 Betaflight SITL + Gazebo Harmonic 仿真环境的搭建流程，包括 SITL 编译、Gazebo 插件的四处关键适配修改、CLI 配置解锁、AUX 通道模式设置以及 Python 闭环定高控制器的实现。Betaflight 的 SITL 模式为飞控算法验证提供了无需硬件的完整闭环仿真能力，配合 Gazebo 物理引擎可以验证从传感器输入到电机输出的完整控制链路。如果需要在仿真中集成视觉感知，可以参考无人机仿真环境调用 YOLO 的简单示例，为目标识别任务做准备。

欢迎在 GitHub 仓库上提交改进和反馈！

参考资料

Betaflight SITL 官方文档
OSRF vehicle_gateway 仓库
Betaflight 源码 — SITL 平台实现 — sitl.c、target.h、udplink.c
Gazebo Harmonic 安装文档
ROS2 + Gazebo 桥接包 — ros-humble-ros-gzharmonic API 文档
Betaflight Configurator

ROS2 Hardware: Flashing the Jetson Onboard Computer

Fri, 10 Jul 2026 00:00:00 GMT

Introduction

After covering simulation workflows in the previous five articles, we now begin a new section: real drone flight. Real flight requires significantly more configuration and higher safety standards than simulation, and we'll cover each aspect in turn. This article starts with the essential first step: flashing the onboard computer.

When controlling drones with ROS, the onboard computer handles critical computation tasks — providing sufficient processing power without interfering with the flight controller, enabling real-time path planning, multi-drone cooperative decision-making, visual SLAM, and more. If you haven't set up a simulation environment yet, start with the ROS2+PX4 Simulation Setup Guide. This guide uses the Nvidia Jetson Orin NX and flashes Ubuntu 22.04.

Hardware required:

Onboard computer (Nvidia Jetson Orin NX used here)
Power supply for the onboard computer
Mouse, keyboard, and monitor
At least one Dupont (jumper) wire
A personal computer with Ubuntu dual-boot or virtual machine
USB to Type-C cable

Host Environment Dependencies

Install required packages on the host Ubuntu system:

sudo apt install sshpass abootimg nfs-kernel-server libxml2-utils

Note: Flashing via a virtual machine will download many files. Ensure at least 100GB of free disk space!

Installing SDK Manager

SDK Manager is the standard flashing tool for Nvidia onboard computers. Download it from Nvidia SDK Manager.

If prompted to log in, register a free Nvidia account. Download the .deb package for your Ubuntu version.

Install the package:

cd ~/Download
sudo dpkg -i sdkmanager_2.3.0-12617_amd64.deb

If you encounter dependency errors, fix and update:

sudo apt-get install -f

Reboot and reinstall. Once the installation completes, log in again:

Entering Recovery Mode

For boards with a Recovery button, press and hold as per the documentation. For the Orin NX (which lacks a Recovery button), we short specific pins during power-on.

According to the Nvidia Jetson Orin NX documentation, short the GND and FC REC pins for 10 seconds. These pins are located directly beneath the fan:

GND is the second pin, FC REC is the third:

Short the two pins, connect the USB to your host PC and the Type-C to the Jetson, then power on for 10 seconds. SDK Manager will show the following screen, indicating successful recovery mode entry.

Important: Do NOT click "Continue" yet. Remove the Dupont wire now — it is no longer needed and could damage the board. Keep the power supply connected throughout the flashing process.

Flashing Process

Back in the initial screen, configure the following:

Uncheck Host Machine
Under Target Hardware, select your board model (should auto-detect after USB and Type-C connection)
Select the JetPack version matching your Ubuntu target — JetPack 6.2.1 supports Ubuntu 22.04

Click Continue to proceed to step 2. Leave the default component selections. Check both options at the bottom ("Download now" and "Install later") — SDK Manager will download the required packages first. If prompted that no Nvidia directory exists, click "Create."

When the progress bar completes and "Download Completed successfully" appears, click "Finish" to proceed to installation.

SDK Manager returns to the first step. Repeat the earlier configuration (uncheck Host Machine, select JetPack version) and proceed. In the component list, all items should show "Downloaded." Now uncheck "Download now. Install later" since all components are already downloaded, and proceed directly to installation.

SDK Manager will now begin the lengthy flashing process. The first popup asks for a username and password — similar to a fresh Ubuntu install. Use jetson or nvidia to avoid potential SSH issues later.

Subsequent popups will appear. Since we're using USB (not Ethernet), ensure "USB" is selected. The username and password should match the first entry (usually no changes needed).

Critical: Maintaining USB Connection Stability

Stay at your screen during the flashing process, especially VM users. The system will repeatedly disconnect (Removed) and reconnect (added) the USB device. Dual-boot users just need to monitor automatic reconnection. VM users must manually reconnect the USB to the virtual machine each time — never select "Remember my choice":

Flashing Complete

Connect the monitor, keyboard, and mouse to the Jetson. As flashing progresses, the display driver will install and you'll see the Ubuntu installation process on the monitor. Now just wait — keeping the USB connection stable.

When SDK Manager's progress bar completes and shows "Finished," flashing is done. The monitor will display the Ubuntu login screen — username jetson. Mission accomplished!

Now connect your peripherals and configure the system. Clone the Micro-XRCE-DDS-Agent, ROS2 workspace, px4_msgs, and px4_ros_com source code onto the Jetson as described in the simulation setup articles. Once flashing is complete, the next step is Controlling PX4 via MAVROS2 to achieve Offboard takeoff.

Summary

Like setting up a simulation environment, real-flight experiments have higher safety requirements and correspondingly more involved procedures. This article aims to make it easier for readers to begin real-flight experiments with ROS. In hands-on engineering fields like ROS and UAVs, experiential knowledge is indispensable — it lets us focus our energy on the core work. The author looks forward to continuously supplementing, correcting, and updating this knowledge together with the community.

As always, suggestions and feedback are sincerely welcomed. Thank you for reading and for your continued support!

References

ROS2真机实践：为机载电脑刷机（以Jetson NX为例）

Fri, 10 Jul 2026 00:00:00 GMT

简介

在前五篇文章介绍完仿真的基本流程后，我们开始一个新的部分：真机飞行。真机相对于仿真需要更多的配置，对安全性有更高的要求，我们会依次介绍。今天首先介绍机载电脑的刷机流程。

在利用ROS控制无人机时，机载电脑承担着关键的计算任务，可以在不影响底层飞控的同时提供足够的算力，实现实时路径规划、多机协同决策、视觉SLAM等任务。如果还未搭建仿真环境，建议先参考 ROS2+PX4 仿真环境开发教程完成仿真端的准备工作。本文以Nvidia Jetson Orin NX为例，刷入Ubuntu 22.04系统。

需要准备的硬件：

机载电脑（此处使用Nvidia Jetson Orin NX）
机载电脑电源
鼠标、键盘外设以及显示器
至少一根杜邦线
配备有Ubuntu双系统或者虚拟机的个人电脑
USB转type-C线

主机环境依赖

主机的Ubuntu系统需要安装必要的依赖：

sudo apt install sshpass abootimg nfs-kernel-server libxml2-utils

注意：虚拟机烧录时会下载大量的烧录文件，请使用虚拟机进行烧录的朋友务必留够不少于100G的磁盘空间！

安装SDK Manager

SDK Manager是常见的Nvidia机载电脑的刷机工具，我们先前往 Nvidia SDK Manager 下载。

如果进入后需要登录注册，大家自行注册一个Nvidia账号即可。进入页面后，直接下载对应Ubuntu系统的deb安装包。

下载后的文件统一储存在 ~/Download 目录下，进入并安装：

cd ~/Download
sudo dpkg -i sdkmanager_2.3.0-12617_amd64.deb

如果此时出现报错，多为依赖项缺失或者依赖冲突（dependencies problems），需要修复并更新依赖：

sudo apt-get install -f

完成之后重新启动系统并再次安装。安装完成后出现以下界面，此处又需要登录，直接登录即可：

至此，用于刷机的SDK Manager就安装完成了。

启动烧录模式

对于有Recovery按键的机载电脑，根据具体说明长按或短按按键即可。对于Orin NX这类无Recovery按键的机载电脑，通常采用短接引脚通电的方式，此处笔者给出说明。

根据Nvidia Jetson Orin NX官方文档的说明，我们需要将GND和FC REC两个引脚短接通电10秒。这两个引脚位于机载电脑风扇的正下方：

GND引脚为第二个引脚，FC REC为第三个：

用一根杜邦线将两个引脚短接，随后将USB口接入本机电脑，type-C口接入机载电脑，为机载电脑接入电源10秒后，SDK Manager上会进入以下页面，此时进入烧录模式成功。

注意：请不要急于点"Continue"，后续还有一些说明。现在可以将短接的杜邦线拔掉，后续不再需要，防止烧坏机载电脑。后续烧录过程中，请保持电源的连接。

进行烧录

回到刚刚进入的页面，现在需要做以下几个设置：

取消勾选 Host Machine
在 Target Hardware 中选择自己的机载电脑型号（在USB和type-C连接成功后，应该会自动显示）
根据自己的Jetpack版本号选择SDK Version，笔者在之前的ROS2仿真中使用的是Ubuntu 22.04，此处也对应选择 Jetpack 6.2.1，支持Ubuntu 22.04

此时可以选择Continue进行第二步，这里会检查相应组件的安装情况，大家直接默认不动就好。同时勾选底部以下两个选项，表示先下载组件，再进行安装，此时SDK Manager会开始下载刷机所需的包。如果弹出主目录里没有nvidia的目录，直接点"Create"继续即可。

当底部进度条走完并且出现"Download Completed successfully"的时候，说明下载完成，点击"Finish"进入安装。

随后SDK Manager会再次进入第一步选择各种型号的页面，像最先开始一样取消勾选"Host Machine"并选择对应Jetpack版本号，进入下一步。此时在第3栏中，SDK Manager会检测下载状态，如图所示均为"Downloaded"，表示均已下载完成。现在需要取消勾选"Download now. Install later"，因为已经下载好了，直接安装刷机即可。

随后SDK Manager会进入长时间的刷机过程，需要保持等待。第一个弹窗需要填写用户名和密码，和首次装Ubuntu系统时基本类似。这里推荐使用jetson或nvidia，防止后续使用SSH进行本地电脑与机载电脑连接时出现奇怪的bug。

后续还会弹出几个弹窗，因为我们使用的是USB连接而非以太网连接，只需确保弹窗中的选项是USB，同时用户名和密码为第一次输入的用户名和密码即可（一般而言不需要修改）。

烧录关键：保持USB连接稳定

在烧录过程中，需要一直在屏幕前，尤其是虚拟机的用户。系统会显示USB不断地被弹出（Removed）和重新添加（added）。如果使用双系统进行烧录，只需注意USB被弹出后是否及时自动重新添加；而如果使用虚拟机，还要将USB连接到虚拟机而非主机，此过程不断反复，需要不断操作，不可以选择"记住我的选择"：

烧录完毕

将鼠标、键盘、显示器等外设连接到机载电脑上。随着烧录的进行，显示器驱动会成功安装，可以在显示器上看见Ubuntu系统的安装进程。现在要做的就是等待，并随时保证USB的连接稳定。

等到SDK Manager的安装进度条走完，显示"Finished"，则说明烧录完成。此时连接显示器，可以看见Ubuntu的登录界面，用户名为jetson，大功告成！

现在，大家可以连接鼠标、键盘外设，对机载电脑进行设置操作。同时，按照我们之前给出的ROS2+PX4仿真开发流程，将实机飞行需要用到的Micro-XRCE-DDS-Agent和需要的ROS2工作空间及px4_msgs、px4_ros_com源码克隆到机载电脑上。刷机完成后，下一步就是使用 MAVROS2 让机载电脑控制 PX4 无人机，实现 Offboard 模式起飞。

总结

与配置开发仿真环境一样，真机的实验有更高的安全性要求，因此相应的流程也会更加繁琐。希望通过本篇文章，能够让大家更加轻松地上手使用ROS进行真机实验。在类似ROS、无人机这些工程性质较浓的领域，一些经验性的知识显得必不可少，可以帮助我们将更多精力放在核心的工作中，笔者期望与大家一起不断补充、修改、更新这些知识。

最后，诚挚地希望各位读者能够提出意见或者建议，感谢大家的阅读和不断的支持鼓励！

参考资料

Distributed Control & Second-Order Consensus for UAV Swarms

Fri, 10 Jul 2026 00:00:00 GMT

Introduction

This article presents a distributed control framework for multi-UAV formation systems, built on the ROS2+PX4 simulation environment and Multi-Drone Offboard Control foundations. By integrating ROS2's distributed communication architecture with PX4's MAVLink protocol, the system enables real-time state sharing and cooperative control across multiple drones. The consensus-based control law allows each drone to rely solely on local neighbor information to achieve formation keeping and trajectory tracking, offering a highly reliable inter-drone communication method for large-scale swarm missions.

UAV formation control generally falls into two architectural paradigms: centralized and distributed. Their differences lie primarily in decision hierarchy, communication topology, and system robustness.

Centralized control relies on a single central node (e.g., a ground station or a lead UAV) for global decision-making. All drones execute commands issued by this central node. It offers high control precision and excellent formation consistency, making it suitable for complex missions. However, it suffers from single-point failure risk, high communication load on the central node, and poor scalability -- especially critical when the central node fails or comes under attack, which can severely compromise formation safety.

Distributed control has no central node. Each drone makes autonomous decisions through local communication, interacting only with neighboring drones. This approach provides strong robustness, high scalability, and dynamic adaptability for large-scale formations, at the cost of higher coordination complexity and weaker global optimization capability.

This article is based on a formation of six UAVs. We first establish a first-order swarm cooperative control law, then extend it to a more general second-order model (acceleration input), solve for the feedback gain matrix via the LMI method, and finally validate the approach in Gazebo simulation. The final section provides a rigorous proof of the error dynamics under constant formation offsets.

Mathematical Preliminaries

Extensive literature exists on the mathematical background of distributed cooperative control systems; this section only provides the necessary review. Readers are referred to the following for deeper understanding:

Graph theory fundamentals: Multi-Agent System Control (1) -- Graph Theory (Chinese)
Properties of the Laplacian matrix: CSDN Blog (Chinese)
Consensus control lemmas: CSDN Blog (Chinese), which can be used to prove the convergence of the control law

From a theoretical perspective, it can be shown that the swarm cooperative system constructed in this article is capable of achieving cooperative UAV formation tasks.

First-Order Consensus Control

System Model

Consider a formation of $N+1$ UAVs. The drone labeled $0$ is the leader, and the remaining $N$ drones labeled $i = 1, 2, \dots, N$ are followers.

Each drone uses a first-order kinematic model in which the state consists only of position information. Let $p_i \in \mathbb{R}^3$ denote the position of drone $i$, and let the control input be a velocity command $v_i \in \mathbb{R}^3$:

$$ \dot{p}_i = v_i $$

First-Order Consensus Protocol

We adopt the classical linear consensus protocol, where the control input of drone $i$ is determined by the weighted sum of neighbor state deviations:

$$ v_i = k_p \sum_{j \in \mathcal{N}i} w{ij}(p_j - p_i) $$

where $\mathcal{N}i$ is the neighbor set of drone $i$, $w{ij} \ge 0$ are communication weights, and $k_p > 0$ is the position gain.

For a connected graph, it can be proved that the position states satisfy $p_i(t) - p_j(t) \to 0$ as $t \to \infty$, meaning all followers asymptotically converge to the leader's position. This is the most fundamental application of distributed consensus in swarm formation -- global position synchronization achieved using only local neighbor position deviations.

Second-Order Distributed Control

Theoretical Modeling

Single-UAV Dynamics

We consider a formation of $N+1$ UAVs. Drone $0$ serves as the leader, chosen as a virtual leader (not physically flying). Drones labeled $i = 1, 2, \dots, N$ are followers.

Each drone is modeled with second-order dynamics (containing both position and velocity states). The state of drone $i$ is:

$$ \xi_i = \begin{bmatrix} p_i \ v_i \end{bmatrix} \in \mathbb{R}^6, \quad p_i \in \mathbb{R}^3, \quad v_i \in \mathbb{R}^3 $$

where $p_i$ is the position and $v_i$ is the velocity. The control input is a second-order acceleration command $u_i \in \mathbb{R}^3$.

The linearized second-order model of each drone can be written in matrix form:

$$ \dot{\xi}_i = A \xi_i + B u_i \tag{1} $$

The matrices $A \in \mathbb{R}^{6 \times 6}$ and $B \in \mathbb{R}^{6 \times 3}$ are block matrices, obtained by simple derivation:

$$ \dot{\xi}i = \begin{bmatrix} \dot{p}i \ \dot{v}i \end{bmatrix} = \begin{bmatrix} v_i \ u_i \end{bmatrix} = \begin{bmatrix} 0{3\times3} & I_3 \ 0{3\times3} & 0{3\times3} \end{bmatrix} \begin{bmatrix} p_i \ v_i \end{bmatrix} + \begin{bmatrix} 0_{3\times3} \ I_3 \end{bmatrix} u_i $$

$$ \Rightarrow A = \begin{bmatrix} 0_{3\times3} & I_3 \ 0_{3\times3} & 0_{3\times3} \end{bmatrix}, \quad B = \begin{bmatrix} 0_{3\times3} \ I_3 \end{bmatrix} \tag{2} $$

Linear Consensus Protocol

We adopt the classical linear consensus protocol, where the input of each follower drone ($i = 1, 2, \dots, N$) can be expressed as the weighted sum of neighbor state deviations:

$$ u_i = K \sum_{j=0}^{N} w_{ij}(\xi_j - \xi_i) \tag{3} $$

where $K$ is the state-feedback gain matrix.

Rewrite this in Laplacian matrix form. Define the weight matrix $W = (w_{ij})$, degree matrix $D = \text{diag}(d_0, \dots, d_N)$ where $d_i = \sum_j w_{ij}$, and Laplacian matrix $L = D - W$ whose entries satisfy $l_{ii} = \sum_{j=0}^N w_{ij}$, $l_{ij} = -w_{ij}$ ($i \ne j$), $w_{ii} = 0$.

Expand equation (3):

$$ \begin{aligned} u_i &= K \sum_{j=0}^{N} w_{ij}(\xi_j - \xi_i) \ &= K\left( \sum_{j=0}^{N} w_{ij} \xi_j - \left(\sum_{j=0}^{N} w_{ij}\right)\xi_i \right) \ &= K\left( \sum_{j=0, j \ne i}^{N} w_{ij} \xi_j + w_{ii}\xi_i - \left(\sum_{j=0}^{N} w_{ij}\right)\xi_i \right) \ &= K\left( -\sum_{j=0, j \ne i}^{N} l_{ij} \xi_j + 0 - l_{ii}\xi_i \right) \ &= -K \sum_{j=0}^{N} l_{ij} \xi_j \end{aligned} $$

Therefore:

$$ u_i = -K \sum_{j=0}^{N} l_{ij} \xi_j, \quad i = 1, 2, \dots, N \tag{4} $$

Error System Definition

Define the error of each follower relative to the leader (labeled $0$):

$$ e_i = \xi_i - \xi_0, \quad i = 1, \dots, N \tag{5} $$

From the single-drone dynamics (equation (1)):

$$ \dot{e}_i = \dot{\xi}_i - \dot{\xi}_0 = A\xi_i + Bu_i - \dot{\xi}_0 $$

Substitute the control law (equation (4)):

$$ \dot{e}i = A\xi_i - BK\sum{j=0}^{N} l_{ij} \xi_j - \dot{\xi}_0 $$

For all $j \ge 1$, we have $\xi_j = e_j + \xi_0$. Split into $j=0$ and $j \ge 1$:

$$ \sum_{j=0}^{N} l_{ij} \xi_j = \sum_{j=1}^{N} l_{ij} e_j + \left(l_{i0} + \sum_{j=1}^{N} l_{ij}\right)\xi_0 $$

Using the Laplacian's zero row-sum property $\sum_{j=0}^N l_{ij} = 0$, we therefore have $l_{i0} + \sum_{j=1}^N l_{ij} = 0$, so:

$$ \sum_{j=0}^{N} l_{ij} \xi_j = \sum_{j=1}^{N} l_{ij} e_j $$

Substituting back yields the error dynamics for a single follower $i$:

$$ \dot{e}i = A e_i - BK \sum{j=1}^{N} l_{ij} e_j + (A\xi_0 - \dot{\xi}_0) \tag{6} $$

Stack all $N$ followers:

$$ e = \begin{bmatrix} e_1 \ \vdots \ e_N \end{bmatrix} \in \mathbb{R}^{6N}, \quad d = \mathbf{1}N \otimes \left(A\xi_0 - \dot{\xi}0\right) \in \mathbb{R}^{6N}, \quad \tilde{L} = [-l{ij}]{i,j=1}^N $$

Writing the error dynamics in matrix form:

$$ \dot{e} = \left(I_N \otimes A + \tilde{L} \otimes BK\right) e + d \tag{7} $$

Lyapunov Function and LMI Condition

Choose the Lyapunov function $V(e) = e^\top P e$, where $P = I_N \otimes X^{-1}$ and $X \succ 0$ is a symmetric positive-definite matrix:

$$ V(e) = e^\top \left(I_N \otimes X^{-1}\right) e \tag{8} $$

The closed-loop system (homogeneous part) of the error dynamics is:

$$ \dot{e} = A_{cl} e, \quad A_{cl} = I_N \otimes A + \tilde{L} \otimes BK \tag{9} $$

Compute the Lyapunov derivative:

$$ \dot{V} = e^\top \left( A_{cl}^\top P + P A_{cl} \right) e $$

To guarantee asymptotic stability, we require $\dot{V} \prec 0$, i.e.:

$$ A_{cl}^\top P + P A_{cl} \prec 0 \tag{10} $$

Congruence Transformation Note: Let $S \in \mathbb{R}^{n \times n}$ be a symmetric matrix and $M \in \mathbb{R}^{n \times n}$ be an invertible matrix. Define $\bar{S} = M S M^\top$. Then $S \succ 0 \iff \bar{S} \succ 0$ and $S \prec 0 \iff \bar{S} \prec 0$. For any nonzero vector $z$, $z^\top \bar{S} z = (M^\top z)^\top S (M^\top z)$, and since $M$ is invertible the sign is preserved.

Apply the congruence transformation to eliminate $X^{-1}$:

$$ \bar{S} = (I_N \otimes X) \left( P A_{cl} + A_{cl}^\top P \right) (I_N \otimes X) $$

Then $P A_{cl} + A_{cl}^\top P \prec 0 \iff \bar{S} \prec 0$. Expanding $\bar{S}$:

$$ \bar{S} = I_N \otimes (AX + XA^\top) + \tilde{L} \otimes (BKX) + \tilde{L}^\top \otimes \big(X(BK)^\top\big) \tag{11} $$

Apply the variable substitution $Y = KX$ to eliminate bilinear coupling:

$$ BKX = BY, \quad X(BK)^\top = (BY)^\top $$

Thus the LMI condition is:

$$ I_N \otimes (AX + XA^\top) + \tilde{L} \otimes (BY) + \tilde{L}^\top \otimes (BY)^\top \prec 0, \quad X \succ 0 \tag{12} $$

Control Law with Formation Offsets

Assume the formation has desired relative positions. Define $r_{ij}$ as the desired relative offset between follower $i$ and follower $j$. The control law is modified to:

$$ u_i = K \sum_{j=0}^{N} w_{ij} \big[(\xi_j - \xi_i) + r_{ij}\big] = -K \sum_{j=0}^{N} l_{ij} \xi_j + K \sum_{j=0}^{N} w_{ij} r_{ij} \tag{13} $$

Redefine the error term. Let $r_{ij} = r_i - r_j$ (with $r_0 = 0$), where $r_i$ is the desired formation position offset of follower $i$ relative to the leader. Define the new error as:

$$ e_i = \xi_i - (\xi_0 + r_i), \quad i = 1, \dots, N $$

Differentiate the error, substituting the single-drone dynamics (1) and the control law (13):

$$ \dot{e}i = A\xi_i - BK \sum{j=0}^{N} l_{ij} \xi_j + BK \sum_{j=0}^{N} w_{ij} r_{ij} - \dot{\xi}_0 - \dot{r}_i \tag{14} $$

Simplify Sum 1 (Laplacian term). For $j \ge 1$, substitute $\xi_j = e_j + \xi_0 + r_j$, and $\xi_0 = \xi_0 + r_0$:

$$ \begin{aligned} \sum_{j=0}^{N} l_{ij} \xi_j &= l_{i0} \xi_0 + \sum_{j=1}^{N} l_{ij} (e_j + \xi_0 + r_j) \ &= \sum_{j=1}^{N} l_{ij} e_j + \Big(l_{i0} + \sum_{j=1}^{N} l_{ij}\Big) \xi_0 + \sum_{j=1}^{N} l_{ij} r_j \end{aligned} $$

From the Laplacian zero row-sum property ($\sum_{j=0}^N l_{ij} = 0$), we obtain $l_{i0} + \sum_{j=1}^N l_{ij} = 0$, hence:

$$ \sum_{j=0}^{N} l_{ij} \xi_j = \sum_{j=1}^{N} l_{ij} e_j + \sum_{j=1}^{N} l_{ij} r_j \tag{15} $$

Simplify Sum 2 (formation weighting term). Using $r_{ij} = r_i - r_j$, $w_{ii} = 0$, and $l_{ij} = -w_{ij}$ ($i \ne j$):

$$ \begin{aligned} \sum_{j=0}^{N} w_{ij} r_{ij} &= \sum_{j=0}^{N} w_{ij}(r_i - r_j) = r_i \sum_{j=0}^{N} w_{ij} - \sum_{j=0}^{N} w_{ij} r_j \ &= l_{ii} r_i - \sum_{j=0, j \ne i}^{N} w_{ij} r_j = l_{ii} r_i + \sum_{j=0, j \ne i}^{N} l_{ij} r_j = \sum_{j=0}^{N} l_{ij} r_j \end{aligned} $$

Therefore:

$$ \sum_{j=0}^{N} w_{ij} r_{ij} = \sum_{j=0}^{N} l_{ij} r_j \tag{16} $$

Substitute into the error equation. Substitute $\xi_i = e_i + \xi_0 + r_i$ (for $i \ge 1$), along with (15) and (16), into (14). After expansion, note that the $BK \sum_{j=1}^N l_{ij} r_j$ term and the $BK \sum l_{ij} r_j$ term (the part with $j \ge 1$) cancel each other, and $r_0 = 0$ means $BK l_{i0} r_0 = 0$. The result is:

$$ \dot{e}i = A e_i - BK \sum{j=1}^{N} l_{ij} e_j + (A\xi_0 - \dot{\xi}_0) + (A r_i - \dot{r}_i) \tag{17} $$

Constant offset case -- LMI condition unchanged. If $r_i$ is a constant position offset (the most common case), i.e.:

$$ r_i = \begin{bmatrix} p_{r_i} \ 0 \end{bmatrix}, \quad \dot{r}_i = 0, \quad A = \begin{bmatrix} 0 & I \ 0 & 0 \end{bmatrix} \Rightarrow A r_i = \begin{bmatrix} 0 \ 0 \end{bmatrix} $$

The extra bias term $(A r_i - \dot{r}_i)$ vanishes, and the error dynamics reduce to the classic homogeneous form:

$$ \dot{e}i = A e_i - BK \sum{j=1}^{N} l_{ij} e_j + (A\xi_0 - \dot{\xi}_0) \tag{18} $$

Stacking all followers:

$$ \dot{e} = \left(I_N \otimes A + \tilde{L} \otimes BK\right) e + \mathbf{1}_N \otimes (A\xi_0 - \dot{\xi}_0) \tag{19} $$

This is identical to equation (7). Therefore, when formation-internal relative positions (constant offsets) are present, the Lyapunov analysis and LMI condition (12) remain unchanged.

Simulation Implementation

Solving the LMI Inequality

The above LMI condition is solved in MATLAB using the YALMIP toolbox. The chosen communication topology is: drone $0$ is the virtual leader, drone $1$ subscribes to drones $0$, $2$, and $3$ (relative formation position $(0, 0)$), drone $2$ subscribes to drone $1$ (relative formation position $(-5, -10)$), and drone $3$ subscribes to drone $1$ (relative formation position $(5, -10)$).

% ------------ Parameters ------------
N = 3;               % Number of followers
% System matrices A (6x6) and B (6x3)
A = [zeros(3) eye(3); zeros(3) zeros(3)];
B = [zeros(3); eye(3)];

% Construct L_sub and Ltilde
L_sub = [ 3 -1 -1;
         -1  1  0;
         -1  0  1 ];
Ltilde = -L_sub;

% ------------- YALMIP Variables -------------
n = size(A,1);      % =6
m = size(B,2);      % =3

X = sdpvar(n,n,'symmetric');    % X ≻ 0
Y = sdpvar(m,n,'full');         % Y = K*X (m x n)

% Construct large matrix S = I_N ⊗ (A X + X A') + Ltilde ⊗ (B Y) + Ltilde' ⊗ (B Y)'
S = kron(eye(N), A*X + X*A') + kron(Ltilde, B*Y) + kron(Ltilde', (B*Y)');

% LMI: S < 0, X > 0
eps = 1e-6;
Constraints = [S <= -eps*eye(N*n), X >= eps*eye(n)];
Constraints = [Constraints, X == X'];

% ------------- Solve -------------
options = sdpsettings('verbose', 1, 'solver', 'sedumi');
Objective = [];
sol = optimize(Constraints, Objective, options);

if sol.problem == 0
    Xopt = value(X);
    Yopt = value(Y);
    K = Yopt * (Xopt \ eye(n));   % K = Y * X^{-1}
    disp('Found feasible K:');
    disp(K);
else
    disp('Solver failed; return information:');
    yalmiperror(sol.problem)
end

The solution for the formation positions used in this article (readers should solve according to their own models):

Found feasible K:
    0.6983    0.0000    0.0000    2.1929    0.0000    0.0000
   -0.0000    0.6983    0.0000   -0.0000    2.1929    0.0000
    0.0000   -0.0000    0.6983   -0.0000   -0.0000    2.1929

Hence $K_p = 0.6983$, $K_v = 2.1929$.

From Acceleration to Velocity Commands

PX4 employs a typical cascaded PID control structure: position controller → velocity controller → acceleration controller → attitude controller → angular rate controller → motors.

If one were to directly use second-order acceleration input, gravity compensation and attitude resolution would be required to convert world-frame acceleration commands into body-frame attitude commands:

$$ a_{body}^{des} = R_b^{w^{-1}} \left( a_{world}^{des} - g \right) $$

In Offboard mode, direct acceleration control is uncommon in engineering practice (for instance, $a_z = 0$ does not produce hover; a gravity compensation empirical value must be provided).

Therefore, in this article the virtual acceleration command obtained from the linear consensus protocol is integrated to generate the desired velocity input:

$$ a_i(k) = k_p \sum_{j \in \mathcal{N}i} \big(p_j(k) - p_i(k) + r{ij}(k)\big) + k_v \sum_{j \in \mathcal{N}_i} \big(v_j(k) - v_i(k)\big) \tag{20} $$

$$ v_i^{des}(k+1) = v_i^{des}(k) + a_i(k) \Delta t \tag{21} $$

The result is then smoothed through a first-order filter:

$$ v_i^{cmd}(k+1) = \alpha v_i^{des}(k+1) + (1 - \alpha) v_i^{cmd}(k) \tag{22} $$

where $\alpha \in (0, 1)$ is the filter factor, set to $0.5$ in this article.

Code example for the $x$ and $y$ directions:

float ax = position_gain_ * sum_dx + velocity_gain_ * sum_dvx;
float ay = position_gain_ * sum_dy + velocity_gain_ * sum_dvy;

float vx_integrated = desired_velocity_[0] + ax * dt;
float vy_integrated = desired_velocity_[1] + ay * dt;

desired_velocity_[0] = filter_alpha_ * vx_integrated
                     + (1.0f - filter_alpha_) * desired_velocity_[0];
desired_velocity_[1] = filter_alpha_ * vy_integrated
                     + (1.0f - filter_alpha_) * desired_velocity_[1];

This approach preserves the dynamic characteristics of second-order consensus control while being compatible with PX4's velocity control layer, offering good engineering feasibility and stability.

Filter Discretization Derivation

The continuous-time first-order low-pass filter equation is:

$$ \tau \dot{v}_f(t) + v_f(t) = v(t) $$

where $v_f(t)$ is the filtered velocity signal, $v(t)$ is the input signal, and $\tau$ is the time constant. Rewriting:

$$ \dot{v}_f(t) = \frac{1}{\tau}\big(v(t) - v_f(t)\big) $$

Under discrete conditions with sampling period $dt$:

$$ \dot{v}_f(t) \approx \frac{v_f[k] - v_f[k-1]}{dt} $$

Substituting:

$$ \frac{v_f[k] - v_f[k-1]}{dt} = \frac{1}{\tau}\big(v[k] - v_f[k-1]\big) $$

Multiplying both sides by $dt$ and rearranging:

$$ v_f[k] - v_f[k-1] = \frac{dt}{\tau}\big(v[k] - v_f[k-1]\big) $$

Define $\alpha = \frac{dt}{\tau + dt}$, yielding:

$$ v_f[k] = (1 - \alpha) v_f[k-1] + \alpha v[k] \tag{23} $$

This matches the form of equation (22).

ROS2 Coordinate Transformation Note

In a ROS2 + PX4 launch, each drone instance by default starts with its own takeoff point as the origin $(0, 0)$. Therefore, each time a position is received in a subscription callback, one must manually subtract the takeoff point coordinates to obtain the global coordinates before substituting into the control law. A code example is provided in the next section.

Reference Code

The following is the complete C++ code for UAV $1$ (which subscribes to the virtual leader as well as neighboring UAVs $2$ and $3$), corresponding to the model described above. The code for UAVs $2$ and $3$ is similar, requiring only modifications to the subscription topology and formation offsets.

#include <rclcpp/rclcpp.hpp>
#include <px4_msgs/msg/offboard_control_mode.hpp>
#include <px4_msgs/msg/trajectory_setpoint.hpp>
#include <px4_msgs/msg/vehicle_command.hpp>
#include <px4_msgs/msg/vehicle_odometry.hpp>
#include <vector>
#include <map>
#include <array>
#include <limits>
#include <chrono>

struct NeighborState {
    std::array<float, 3> position = {0.0f, 0.0f, 0.0f};
    std::array<float, 3> velocity = {0.0f, 0.0f, 0.0f};
    bool valid = false;
};

class UAV1Controller : public rclcpp::Node {
public:
    UAV1Controller() : Node("uav1_controller") {
        rclcpp::QoS qos(10);
        qos.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
        qos.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
        qos.history(RMW_QOS_POLICY_HISTORY_KEEP_LAST);

        offboard_control_mode_publisher_ = create_publisher<
            px4_msgs::msg::OffboardControlMode>(
            "/px4_1/fmu/in/offboard_control_mode", qos);
        trajectory_setpoint_publisher_ = create_publisher<
            px4_msgs::msg::TrajectorySetpoint>(
            "/px4_1/fmu/in/trajectory_setpoint", qos);
        vehicle_command_publisher_ = create_publisher<
            px4_msgs::msg::VehicleCommand>(
            "/px4_1/fmu/in/vehicle_command", qos);

        own_odometry_sub_ = create_subscription<
            px4_msgs::msg::VehicleOdometry>(
            "/px4_1/fmu/out/vehicle_odometry", qos,
            [this](const px4_msgs::msg::VehicleOdometry::SharedPtr msg) {
                own_position_ = {msg->position[0], msg->position[1],
                                 msg->position[2]};
                own_velocity_ = {msg->velocity[0], msg->velocity[1],
                                 msg->velocity[2]};
                own_state_valid_ = true;
            });

        uav2_sub_ = create_subscription<px4_msgs::msg::VehicleOdometry>(
            "/px4_2/fmu/out/vehicle_odometry", qos,
            [this](const px4_msgs::msg::VehicleOdometry::SharedPtr msg) {
                neighbor_states_[2] = {
                    {msg->position[0] - 5.0f, msg->position[1] - 10.0f,
                     msg->position[2]},
                    {msg->velocity[0], msg->velocity[1], msg->velocity[2]},
                    true
                };
            });

        uav3_sub_ = create_subscription<px4_msgs::msg::VehicleOdometry>(
            "/px4_3/fmu/out/vehicle_odometry", qos,
            [this](const px4_msgs::msg::VehicleOdometry::SharedPtr msg) {
                neighbor_states_[3] = {
                    {msg->position[0] + 5.0f, msg->position[1] - 10.0f,
                     msg->position[2]},
                    {msg->velocity[0], msg->velocity[1], msg->velocity[2]},
                    true
                };
            });

        position_gain_ = 0.6983f;
        velocity_gain_ = 2.1929f;
        filter_alpha_ = 0.5f;

        start_time_ = this->now();
        control_timer_ = create_wall_timer(std::chrono::milliseconds(20),
                                           [this]() { this->controlLoop(); });
        last_print_time_ = this->now();
        last_control_time_ = this->now();
        desired_velocity_ = {0.0f, 0.0f, 0.0f};
    }

private:
    NeighborState getVirtualLeaderState() {
        auto current_time = this->now();
        double t = (current_time - start_time_).seconds();

        NeighborState leader_state;
        leader_state.valid = true;
        leader_state.position[2] = 0.0f;
        leader_state.velocity[2] = 0.0f;

        if (t <= 10.0) {
            leader_state.velocity[0] = 3.0f;
            leader_state.position[0] = 3.0f * t;
            leader_state.velocity[1] = 0.3f * t;
            leader_state.position[1] = 0.5f * 0.3f * t * t;
        } else if (t <= 20.0) {
            leader_state.velocity[0] = 0.0f;
            leader_state.position[0] = 30.0f;
            leader_state.velocity[1] = 3.0f;
            leader_state.position[1] = 15.0f + 3.0f * (t - 10.0f);
        } else if (t <= 30.0) {
            float d = t - 20.0f;
            leader_state.velocity[0] = 0.0f;
            leader_state.position[0] = 30.0f;
            leader_state.velocity[1] = 3.0f - 0.3f * d;
            leader_state.position[1] = 45.0f + 3.0f * d
                                       - 0.5f * 0.3f * d * d;
        } else {
            leader_state.velocity[0] = 0.0f;
            leader_state.position[0] = 30.0f;
            leader_state.velocity[1] = 0.0f;
            leader_state.position[1] = 60.0f;
        }
        return leader_state;
    }

    void controlLoop() {
        NeighborState leader_state = getVirtualLeaderState();
        neighbor_states_[0] = leader_state;

        if (!own_state_valid_ || !neighbor_states_[2].valid
            || !neighbor_states_[3].valid) return;

        double dt = (this->now() - last_control_time_).seconds();
        last_control_time_ = this->now();

        px4_msgs::msg::OffboardControlMode offboard_msg;
        offboard_msg.position = false;
        offboard_msg.velocity = true;
        offboard_msg.acceleration = false;
        offboard_msg.timestamp = this->now().nanoseconds() / 1000;
        offboard_control_mode_publisher_->publish(offboard_msg);

        static int init_counter = 0;
        if (init_counter < 20) {
            px4_msgs::msg::TrajectorySetpoint setpoint{};
            setpoint.timestamp = get_clock()->now().nanoseconds() / 1000;
            setpoint.position = {std::numeric_limits<float>::quiet_NaN(),
                                std::numeric_limits<float>::quiet_NaN(),
                                std::numeric_limits<float>::quiet_NaN()};
            setpoint.velocity = {0.0f, 0.0f, 0.0f};
            setpoint.acceleration = {std::numeric_limits<float>::quiet_NaN(),
                                    std::numeric_limits<float>::quiet_NaN(),
                                    std::numeric_limits<float>::quiet_NaN()};
            trajectory_setpoint_publisher_->publish(setpoint);
            init_counter++;
            if (init_counter == 20) {
                px4_msgs::msg::VehicleCommand cmd;
                cmd.command = px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE;
                cmd.param1 = 1.0f; cmd.param2 = 6.0f;
                cmd.target_system = 2; cmd.target_component = 1;
                cmd.source_system = 1; cmd.source_component = 1;
                cmd.from_external = true;
                cmd.timestamp = this->now().nanoseconds() / 1000;
                vehicle_command_publisher_->publish(cmd);
            }
            return;
        }

        float sum_dx = (neighbor_states_[0].position[0] - own_position_[0])
                     + (neighbor_states_[2].position[0] - own_position_[0]
                        + 5.0f)
                     + (neighbor_states_[3].position[0] - own_position_[0]
                        - 5.0f);

        float sum_dy = (neighbor_states_[0].position[1] - own_position_[1])
                     + (neighbor_states_[2].position[1] - own_position_[1]
                        + 10.0f)
                     + (neighbor_states_[3].position[1] - own_position_[1]
                        + 10.0f);

        float sum_dvx = (neighbor_states_[0].velocity[0] - own_velocity_[0])
                      + (neighbor_states_[2].velocity[0] - own_velocity_[0])
                      + (neighbor_states_[3].velocity[0] - own_velocity_[0]);

        float sum_dvy = (neighbor_states_[0].velocity[1] - own_velocity_[1])
                      + (neighbor_states_[2].velocity[1] - own_velocity_[1])
                      + (neighbor_states_[3].velocity[1] - own_velocity_[1]);

        float ax = position_gain_ * sum_dx + velocity_gain_ * sum_dvx;
        float ay = position_gain_ * sum_dy + velocity_gain_ * sum_dvy;

        float vx_integrated = desired_velocity_[0] + ax * dt;
        float vy_integrated = desired_velocity_[1] + ay * dt;

        desired_velocity_[0] = filter_alpha_ * vx_integrated
                             + (1.0f - filter_alpha_) * desired_velocity_[0];
        desired_velocity_[1] = filter_alpha_ * vy_integrated
                             + (1.0f - filter_alpha_) * desired_velocity_[1];
        desired_velocity_[2] = 0.0f;

        px4_msgs::msg::TrajectorySetpoint setpoint;
        setpoint.timestamp = this->now().nanoseconds() / 1000;
        setpoint.velocity = {desired_velocity_[0], desired_velocity_[1], 0.0f};
        setpoint.position = {NAN, NAN, NAN};
        setpoint.acceleration = {NAN, NAN, NAN};
        trajectory_setpoint_publisher_->publish(setpoint);
    }

    // Publishers
    rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr
        offboard_control_mode_publisher_;
    rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::SharedPtr
        trajectory_setpoint_publisher_;
    rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr
        vehicle_command_publisher_;

    // Subscribers
    rclcpp::Subscription<px4_msgs::msg::VehicleOdometry>::SharedPtr
        own_odometry_sub_;
    rclcpp::Subscription<px4_msgs::msg::VehicleOdometry>::SharedPtr uav2_sub_;
    rclcpp::Subscription<px4_msgs::msg::VehicleOdometry>::SharedPtr uav3_sub_;

    rclcpp::TimerBase::SharedPtr control_timer_;

    std::array<float, 3> own_position_ = {0.0f, 0.0f, 0.0f};
    std::array<float, 3> own_velocity_ = {0.0f, 0.0f, 0.0f};
    std::array<float, 3> desired_velocity_ = {0.0f, 0.0f, 0.0f};
    bool own_state_valid_ = false;

    std::map<int, NeighborState> neighbor_states_;
    float position_gain_, velocity_gain_, filter_alpha_;
    rclcpp::Time last_print_time_, last_control_time_, start_time_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<UAV1Controller>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

Simulation Results

At startup, all three drones are manually commanded to take off via commander takeoff and hover at $2.5\text{m}$ altitude. The consensus controller then takes over in the $xy$-plane, driving the formation to follow the virtual leader's trajectory.

The overall formation tracking performance is satisfactory. There is some overshoot but overall stability is maintained, confirming the effectiveness of the second-order control law.

Notably, the unique advantages of distributed communication enable further robustness testing: after the formation begins moving, killing UAV $1$'s simulation (simulating a shoot-down scenario) leaves the remaining drones unaffected, continuing along their planned paths. This is precisely the core reliability benefit of distributed communication -- using a connected communication topology for data exchange, the impact of a single-point failure on the overall formation can be effectively mitigated in practical applications.

Summary

This article began with a first-order swarm system and progressively introduced second-order dynamics to build a distributed communication-based UAV formation control system. Starting from first-order position consensus, we derived the error dynamics and Lyapunov stability conditions for the second-order model, converted the stability conditions into a solvable LMI via congruence transformation (obtaining the feedback gain matrix through YALMIP in MATLAB), adapted acceleration-form commands to PX4-compatible velocity commands via integration and filtering, and provided a rigorous derivation for the case with constant formation offsets, verifying that the LMI condition remains invariant.

Compared to centralized control, the distributed approach offers superior robustness and scalability -- advantages that are decisive in complex, dynamically changing real-world scenarios. In emerging fields such as low-altitude economy and swarm combat systems, this integrated communication-and-control architecture will play an increasingly important role.

This article presents only an initial control scheme, and many areas for improvement were identified during simulation -- such as how to further suppress overshoot, how to adapt to non-connected (switching) communication topologies, and how to deploy the computation on real onboard computers. For deploying these algorithms on real hardware, see ROS2 Hardware: Flashing the Jetson and Controlling PX4 with MAVROS2 for the complete simulation-to-real workflow.

Finally, thank you for your patient reading and constructive feedback!

References

无人机分布式通信与二阶一致性协议

Fri, 10 Jul 2026 00:00:00 GMT

简介

本文基于 ROS2+PX4 无人机编队仿真环境和多机 Offboard 控制的基础，介绍分布式通信在无人机集群协同控制系统中的应用。通过Gazebo仿真环境和ROS2的分布式通信框架，结合PX4飞控的MAVLink协议，实现多无人机间的实时状态共享与协同控制。系统采用一致性算法，使无人机仅依赖局部邻居节点的信息即可达成编队队形保持、沿轨迹飞行等目标，为大规模无人机集群的协同任务提供一种高可靠性的编队间通信方式。

常见的无人机编队通信方式有集中式控制和分布式控制两种核心架构，主要区别在于决策层级、通信结构和系统鲁棒性。

集中式控制依赖单一中央节点（如地面站或主无人机）进行全局决策，所有无人机根据中央节点指令执行统一动作。其优势在于控制精度高、队形一致性佳，适合复杂任务。然而，这种通信方式存在单点故障风险，且单一节点通信压力大，扩展性较差。尤其是当中央节点出现故障或受到攻击时，编队控制的安全性会受到极大影响。

分布式控制则无中心节点，各无人机通过局部通信自主决策，仅需与邻近无人机交互信息。这种方式具有强鲁棒性、高扩展性和动态适应性，适合大规模编队，但协调复杂度较高，全局优化能力较弱。

本文基于六架无人机组成的编队，首先建立一阶集群协同控制律，然后拓展至更一般化的二阶模型（加速度输入），并通过LMI方法求解反馈增益矩阵，最终在Gazebo仿真中进行验证。文章末尾给出含常定编队偏置的严格误差动力学证明。

一阶集群协同控制

系统建模

考虑由 $N+1$ 架无人机组成的编队。编号为 $0$ 的无人机为领航者（leader），其余 $N$ 架编号为 $i=1,2,\dots,N$ 的无人机为跟随者（followers）。

每架无人机采用一阶运动学模型，其状态仅包含位置信息。设第 $i$ 架无人机的位置为 $p_i \in \mathbb{R}^3$，控制输入为速度指令 $v_i \in \mathbb{R}^3$，则：

$$ \dot{p}_i = v_i $$

一阶一致性协议

选取经典线性一致性协议，第 $i$ 架无人机的控制输入由其邻居状态偏差的加权和决定：

$$ v_i = k_p \sum_{j \in \mathcal{N}i} w{ij}(p_j - p_i) $$

其中 $\mathcal{N}i$ 为第 $i$ 架无人机的邻居集合，$w{ij} \ge 0$ 为通信权重，$k_p > 0$ 为位置增益。

可以证明，对于连通图，各无人机的位置状态满足 $p_i(t) - p_j(t) \to 0$（$t \to \infty$），即所有跟随者渐近收敛到领航者的位置。这正是分布式一致性算法在集群编队中最基础的应用形式——仅依赖邻居位置偏差，即可实现全局位置同步。

二阶分布式控制集群系统

理论建模

单无人机系统建模

我们考虑由 $N+1$ 架无人机组成的编队，$0$ 号无人机作为领航者（leader），选取为虚拟领航者（不实际参与飞行）。编号 $i = 1, 2, \dots, N$ 的无人机为跟随者（followers）。

每架无人机采用二阶动力学模型（包含位置与速度状态）。记第 $i$ 架无人机的状态为：

$$ \xi_i = \begin{bmatrix} p_i \ v_i \end{bmatrix} \in \mathbb{R}^6, \quad p_i \in \mathbb{R}^3, \quad v_i \in \mathbb{R}^3 $$

其中 $p_i$ 为位置，$v_i$ 为速度。控制输入为二阶加速度指令 $u_i \in \mathbb{R}^3$。

每架无人机的二阶线性化模型可以写为矩阵形式：

$$ \dot{\xi}_i = A \xi_i + B u_i \tag{1} $$

矩阵 $A \in \mathbb{R}^{6 \times 6}$ 与 $B \in \mathbb{R}^{6 \times 3}$ 为块矩阵，简单推导即可得：

$$ \Rightarrow A = \begin{bmatrix} 0_{3\times3} & I_3 \ 0_{3\times3} & 0_{3\times3} \end{bmatrix}, \quad B = \begin{bmatrix} 0_{3\times3} \ I_3 \end{bmatrix} \tag{2} $$

线性一致性协议

选取经典线性一致性协议，每架跟随者无人机（$i=1,2,\dots,N$）的输入可以表示为邻居状态偏差的加权和：

$$ u_i = K \sum_{j=0}^{N} w_{ij}(\xi_j - \xi_i) \tag{3} $$

其中 $K$ 为状态反馈增益矩阵。

将其写为拉普拉斯矩阵形式。定义权重矩阵 $W = (w_{ij})$，度矩阵 $D = \text{diag}(d_0, \dots, d_N)$ 其中 $d_i = \sum_j w_{ij}$，拉普拉斯矩阵 $L = D - W$，元素满足 $l_{ii} = \sum_{j=0}^N w_{ij}$，$l_{ij} = -w_{ij}$（$i \ne j$），$w_{ii}=0$。

将式（3）展开：

因此：

$$ u_i = -K \sum_{j=0}^{N} l_{ij} \xi_j, \quad i = 1, 2, \dots, N \tag{4} $$

误差系统定义

定义每架跟随者相对于领航者（编号 $0$）的误差：

$$ e_i = \xi_i - \xi_0, \quad i = 1, \dots, N \tag{5} $$

由单机动力学（式（1））可得：

$$ \dot{e}_i = \dot{\xi}_i - \dot{\xi}_0 = A\xi_i + Bu_i - \dot{\xi}_0 $$

代入控制律（式（4））：

$$ \dot{e}i = A\xi_i - BK\sum{j=0}^{N} l_{ij} \xi_j - \dot{\xi}_0 $$

对所有 $j \ge 1$ 有 $\xi_j = e_j + \xi_0$，拆分为 $j=0$ 和 $j \ge 1$ 两项：

$$ \sum_{j=0}^{N} l_{ij} \xi_j = \sum_{j=1}^{N} l_{ij} e_j + \left(l_{i0} + \sum_{j=1}^{N} l_{ij}\right)\xi_0 $$

利用拉普拉斯矩阵的行和为零 $\sum_{j=0}^N l_{ij} = 0$，因此 $l_{i0} + \sum_{j=1}^N l_{ij} = 0$，故：

$$ \sum_{j=0}^{N} l_{ij} \xi_j = \sum_{j=1}^{N} l_{ij} e_j $$

代回得单个跟随者 $i$ 的误差动力学方程：

$$ \dot{e}i = A e_i - BK \sum{j=1}^{N} l_{ij} e_j + (A\xi_0 - \dot{\xi}_0) \tag{6} $$

堆叠所有 $N$ 个跟随者：

将误差动力学方程写成矩阵形式：

$$ \dot{e} = \left(I_N \otimes A + \tilde{L} \otimes BK\right) e + d \tag{7} $$

Lyapunov 函数与 LMI 条件

选取 Lyapunov 函数 $V(e) = e^\top P e$，其中 $P = I_N \otimes X^{-1}$，$X \succ 0$ 为对称正定阵：

$$ V(e) = e^\top \left(I_N \otimes X^{-1}\right) e \tag{8} $$

误差动力学方程的闭环系统（齐次部分）为：

$$ \dot{e} = A_{cl} e, \quad A_{cl} = I_N \otimes A + \tilde{L} \otimes BK \tag{9} $$

计算 Lyapunov 导数：

$$ \dot{V} = e^\top \left( A_{cl}^\top P + P A_{cl} \right) e $$

为保证渐近稳定性，需要 $\dot{V} \prec 0$ ，即：

$$ A_{cl}^\top P + P A_{cl} \prec 0 \tag{10} $$

同余变换说明： 设 $S \in \mathbb{R}^{n \times n}$ 为对称矩阵，$M \in \mathbb{R}^{n \times n}$ 为可逆矩阵，定义 $\bar{S} = M S M^\top$，则满足 $S \succ 0 \iff \bar{S} \succ 0$ 和 $S \prec 0 \iff \bar{S} \prec 0$。对任意非零向量 $z$，$z^\top \bar{S} z = (M^\top z)^\top S (M^\top z)$，因 $M$ 可逆二者符号完全相同。

运用同余变换去掉 $X^{-1}$：

$$ \bar{S} = (I_N \otimes X) \left( P A_{cl} + A_{cl}^\top P \right) (I_N \otimes X) $$

则 $P A_{cl} + A_{cl}^\top P \prec 0 \iff \bar{S} \prec 0$。展开 $\bar{S}$：

$$ \bar{S} = I_N \otimes (AX + XA^\top) + \tilde{L} \otimes (BKX) + \tilde{L}^\top \otimes \big(X(BK)^\top\big) \tag{11} $$

使用变量替换 $Y = KX$ 消除双线性耦合：

$$ BKX = BY, \quad X(BK)^\top = (BY)^\top $$

于是 LMI 条件为：

$$ I_N \otimes (AX + XA^\top) + \tilde{L} \otimes (BY) + \tilde{L}^\top \otimes (BY)^\top \prec 0, \quad X \succ 0 \tag{12} $$

包含编队位置的控制律

假设编队内存在期望相对位置，定义 $r_{ij}$ 为跟随者 $i$ 与 $j$ 之间的期望相对偏差。控制律修改为：

$$ u_i = K \sum_{j=0}^{N} w_{ij} \big[(\xi_j - \xi_i) + r_{ij}\big] = -K \sum_{j=0}^{N} l_{ij} \xi_j + K \sum_{j=0}^{N} w_{ij} r_{ij} \tag{13} $$

重新定义误差项。 设 $r_{ij} = r_i - r_j$（$r_0 = 0$），$r_i$ 为跟随者 $i$ 相对于领航者的期望编队位置偏置。定义新的误差为：

$$ e_i = \xi_i - (\xi_0 + r_i), \quad i = 1, \dots, N $$

对误差求导，代入单机动力学（1）和控制律（13）：

$$ \dot{e}i = A\xi_i - BK \sum{j=0}^{N} l_{ij} \xi_j + BK \sum_{j=0}^{N} w_{ij} r_{ij} - \dot{\xi}_0 - \dot{r}_i \tag{14} $$

化简和式 1（拉普拉斯项）。 对 $j \ge 1$，代入 $\xi_j = e_j + \xi_0 + r_j$，$\xi_0 = \xi_0 + r_0$：

由拉普拉斯矩阵行和为零（$\sum_{j=0}^N l_{ij} = 0$）得 $l_{i0} + \sum_{j=1}^N l_{ij} = 0$，从而：

$$ \sum_{j=0}^{N} l_{ij} \xi_j = \sum_{j=1}^{N} l_{ij} e_j + \sum_{j=1}^{N} l_{ij} r_j \tag{15} $$

化简和式 2（编队加权项）。 利用 $r_{ij} = r_i - r_j$ 及 $w_{ii} = 0$、$l_{ij} = -w_{ij}$（$i \ne j$）：

因此：

$$ \sum_{j=0}^{N} w_{ij} r_{ij} = \sum_{j=0}^{N} l_{ij} r_j \tag{16} $$

代入误差方程。 将 $\xi_i = e_i + \xi_0 + r_i$（$i \ge 1$）、（15）、（16）代入（14），展开后注意 $BK \sum_{j=1}^N l_{ij} r_j$ 项与 $BK \sum l_{ij} r_j$ 中含 $j \ge 1$ 的部分相互抵消，且 $r_0 = 0$ 使 $BK l_{i0} r_0 = 0$，得：

$$ \dot{e}i = A e_i - BK \sum{j=1}^{N} l_{ij} e_j + (A\xi_0 - \dot{\xi}_0) + (A r_i - \dot{r}_i) \tag{17} $$

常定偏置情形——LMI 条件不变。 若 $r_i$ 为常定位置偏置（最常见情况），即：

$$ r_i = \begin{bmatrix} p_{r_i} \ 0 \end{bmatrix}, \quad \dot{r}_i = 0, \quad A = \begin{bmatrix} 0 & I \ 0 & 0 \end{bmatrix} \Rightarrow A r_i = \begin{bmatrix} 0 \ 0 \end{bmatrix} $$

额外的偏置项 $(A r_i - \dot{r}_i)$ 消失，误差动力学化归为经典的齐次形式：

$$ \dot{e}i = A e_i - BK \sum{j=1}^{N} l_{ij} e_j + (A\xi_0 - \dot{\xi}_0) \tag{18} $$

堆叠所有跟随者：

$$ \dot{e} = \left(I_N \otimes A + \tilde{L} \otimes BK\right) e + \mathbf{1}_N \otimes (A\xi_0 - \dot{\xi}_0) \tag{19} $$

与式（7）完全一致。因此 存在编队内部相对位置（常定偏置）时，Lyapunov 分析与 LMI 条件（12）保持不变。

仿真实现

求解 LMI 不等式

将上述 LMI 条件在 MATLAB 的 YALMIP 工具中求解。选取的拓扑结构为：$0$ 号无人机为虚拟领航者，$1$ 号无人机订阅 $0$、$2$、$3$ 号无人机（相对编队位置 $(0, 0)$），$2$ 号无人机订阅 $1$ 号（相对编队位置 $(-5, -10)$），$3$ 号无人机订阅 $1$ 号（相对编队位置 $(5, -10)$）。

% ------------ 设置参数 ------------
N = 3;               % follower 数量
% 系统矩阵 A (6x6) 和 B (6x3)
A = [zeros(3) eye(3); zeros(3) zeros(3)];
B = [zeros(3); eye(3)];

% 构造 L_sub 和 Ltilde
L_sub = [ 3 -1 -1;
         -1  1  0;
         -1  0  1 ];
Ltilde = -L_sub;

% ------------- YALMIP 变量 -------------
n = size(A,1);      % =6
m = size(B,2);      % =3

X = sdpvar(n,n,'symmetric');    % X ≻ 0
Y = sdpvar(m,n,'full');         % Y = K*X (m x n)

% 构造大矩阵 S = I_N ⊗ (A X + X A') + Ltilde ⊗ (B Y) + Ltilde' ⊗ (B Y)'
S = kron(eye(N), A*X + X*A') + kron(Ltilde, B*Y) + kron(Ltilde', (B*Y)');

% LMI: S < 0, X > 0
eps = 1e-6;
Constraints = [S <= -eps*eye(N*n), X >= eps*eye(n)];
Constraints = [Constraints, X == X'];

% ------------- 求解 -------------
options = sdpsettings('verbose', 1, 'solver', 'sedumi');
Objective = [];
sol = optimize(Constraints, Objective, options);

if sol.problem == 0
    Xopt = value(X);
    Yopt = value(Y);
    K = Yopt * (Xopt \ eye(n));   % K = Y * X^{-1}
    disp('Found feasible K:');
    disp(K);
else
    disp('求解失败；返回信息：');
    yalmiperror(sol.problem)
end

本文的编队位置求解结果（读者需根据自己建模求解）：

Found feasible K:
    0.6983    0.0000    0.0000    2.1929    0.0000    0.0000
   -0.0000    0.6983    0.0000   -0.0000    2.1929    0.0000
    0.0000   -0.0000    0.6983   -0.0000   -0.0000    2.1929

因此 $K_p = 0.6983$，$K_v = 2.1929$。

加速度积分

PX4 采用典型的级联 PID 控制结构：位置控制器 → 速度控制器 → 加速度控制器 → 姿态控制器 → 角速率控制器 → 电机。

若直接使用二阶加速度输入，需通过重力补偿和姿态解算将世界坐标系下的加速度指令转换为机体姿态指令：

$$ a_{body}^{des} = R_b^{w^{-1}} \left( a_{world}^{des} - g \right) $$

在 Offboard 模式下直接采用加速度控制在工程实践中并不多（例如 $a_z = 0$ 并不能直接悬停，需给予重力补偿的经验值）。

因此，本文将线性一致性协议得到的虚拟加速度指令通过积分变换生成期望的速度输入：

$$ a_i(k) = k_p \sum_{j \in \mathcal{N}i} \big(p_j(k) - p_i(k) + r{ij}(k)\big) + k_v \sum_{j \in \mathcal{N}_i} \big(v_j(k) - v_i(k)\big) \tag{20} $$

$$ v_i^{des}(k+1) = v_i^{des}(k) + a_i(k) \Delta t \tag{21} $$

再通过一阶滤波器进行平滑处理：

$$ v_i^{cmd}(k+1) = \alpha v_i^{des}(k+1) + (1 - \alpha) v_i^{cmd}(k) \tag{22} $$

其中 $\alpha \in (0, 1)$ 为滤波因子，本文取 $0.5$。

在 $x$、$y$ 方向的代码示例：

float ax = position_gain_ * sum_dx + velocity_gain_ * sum_dvx;
float ay = position_gain_ * sum_dy + velocity_gain_ * sum_dvy;

float vx_integrated = desired_velocity_[0] + ax * dt;
float vy_integrated = desired_velocity_[1] + ay * dt;

desired_velocity_[0] = filter_alpha_ * vx_integrated
                     + (1.0f - filter_alpha_) * desired_velocity_[0];
desired_velocity_[1] = filter_alpha_ * vy_integrated
                     + (1.0f - filter_alpha_) * desired_velocity_[1];

该方法保留了二阶一致性控制的动力学特性，同时兼容 PX4 的速度控制层，具有良好的工程可实现性与稳定性。

离散化滤波器推导

连续时间的一阶低通滤波器方程为：

$$ \tau \dot{v}_f(t) + v_f(t) = v(t) $$

其中 $v_f(t)$ 为滤波后的速度信号，$v(t)$ 为输入信号，$\tau$ 为时间常数。改写为：

$$ \dot{v}_f(t) = \frac{1}{\tau}\big(v(t) - v_f(t)\big) $$

在离散条件下，采样周期为 $dt$：

$$ \dot{v}_f(t) \approx \frac{v_f[k] - v_f[k-1]}{dt} $$

代入得：

$$ \frac{v_f[k] - v_f[k-1]}{dt} = \frac{1}{\tau}\big(v[k] - v_f[k-1]\big) $$

两边乘以 $dt$ 并整理：

$$ v_f[k] - v_f[k-1] = \frac{dt}{\tau}\big(v[k] - v_f[k-1]\big) $$

定义 $\alpha = \frac{dt}{\tau + dt}$，得：

$$ v_f[k] = (1 - \alpha) v_f[k-1] + \alpha v[k] \tag{23} $$

与式（22）形式一致。

ROS2 中的坐标变换注意

在 ROS2 + PX4 启动中，每架无人机实例启动时默认以自身启动点坐标为原点 $(0, 0)$。因此，在每次回调订阅位置时，需人为将坐标减去启动点的坐标，获得全局坐标后再代入控制律。代码示例见下节。

参考代码

以下给出上文建模中 $1$ 号无人机（同时订阅虚拟领航者及 $2$、$3$ 号邻居无人机）的完整 C++ 代码。$2$、$3$ 号无人机的代码类似，仅需修改订阅拓扑和编队偏置。

#include <rclcpp/rclcpp.hpp>
#include <px4_msgs/msg/offboard_control_mode.hpp>
#include <px4_msgs/msg/trajectory_setpoint.hpp>
#include <px4_msgs/msg/vehicle_command.hpp>
#include <px4_msgs/msg/vehicle_odometry.hpp>
#include <vector>
#include <map>
#include <array>
#include <limits>
#include <chrono>

struct NeighborState {
    std::array<float, 3> position = {0.0f, 0.0f, 0.0f};
    std::array<float, 3> velocity = {0.0f, 0.0f, 0.0f};
    bool valid = false;
};

class UAV1Controller : public rclcpp::Node {
public:
    UAV1Controller() : Node("uav1_controller") {
        rclcpp::QoS qos(10);
        qos.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
        qos.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
        qos.history(RMW_QOS_POLICY_HISTORY_KEEP_LAST);

        offboard_control_mode_publisher_ = create_publisher<
            px4_msgs::msg::OffboardControlMode>(
            "/px4_1/fmu/in/offboard_control_mode", qos);
        trajectory_setpoint_publisher_ = create_publisher<
            px4_msgs::msg::TrajectorySetpoint>(
            "/px4_1/fmu/in/trajectory_setpoint", qos);
        vehicle_command_publisher_ = create_publisher<
            px4_msgs::msg::VehicleCommand>(
            "/px4_1/fmu/in/vehicle_command", qos);

        own_odometry_sub_ = create_subscription<
            px4_msgs::msg::VehicleOdometry>(
            "/px4_1/fmu/out/vehicle_odometry", qos,
            [this](const px4_msgs::msg::VehicleOdometry::SharedPtr msg) {
                own_position_ = {msg->position[0], msg->position[1],
                                 msg->position[2]};
                own_velocity_ = {msg->velocity[0], msg->velocity[1],
                                 msg->velocity[2]};
                own_state_valid_ = true;
            });

        uav2_sub_ = create_subscription<px4_msgs::msg::VehicleOdometry>(
            "/px4_2/fmu/out/vehicle_odometry", qos,
            [this](const px4_msgs::msg::VehicleOdometry::SharedPtr msg) {
                neighbor_states_[2] = {
                    {msg->position[0] - 5.0f, msg->position[1] - 10.0f,
                     msg->position[2]},
                    {msg->velocity[0], msg->velocity[1], msg->velocity[2]},
                    true
                };
            });

        uav3_sub_ = create_subscription<px4_msgs::msg::VehicleOdometry>(
            "/px4_3/fmu/out/vehicle_odometry", qos,
            [this](const px4_msgs::msg::VehicleOdometry::SharedPtr msg) {
                neighbor_states_[3] = {
                    {msg->position[0] + 5.0f, msg->position[1] - 10.0f,
                     msg->position[2]},
                    {msg->velocity[0], msg->velocity[1], msg->velocity[2]},
                    true
                };
            });

        position_gain_ = 0.6983f;
        velocity_gain_ = 2.1929f;
        filter_alpha_ = 0.5f;

        start_time_ = this->now();
        control_timer_ = create_wall_timer(std::chrono::milliseconds(20),
                                           [this]() { this->controlLoop(); });
        last_print_time_ = this->now();
        last_control_time_ = this->now();
        desired_velocity_ = {0.0f, 0.0f, 0.0f};
    }

private:
    NeighborState getVirtualLeaderState() {
        auto current_time = this->now();
        double t = (current_time - start_time_).seconds();

        NeighborState leader_state;
        leader_state.valid = true;
        leader_state.position[2] = 0.0f;
        leader_state.velocity[2] = 0.0f;

        if (t <= 10.0) {
            leader_state.velocity[0] = 3.0f;
            leader_state.position[0] = 3.0f * t;
            leader_state.velocity[1] = 0.3f * t;
            leader_state.position[1] = 0.5f * 0.3f * t * t;
        } else if (t <= 20.0) {
            leader_state.velocity[0] = 0.0f;
            leader_state.position[0] = 30.0f;
            leader_state.velocity[1] = 3.0f;
            leader_state.position[1] = 15.0f + 3.0f * (t - 10.0f);
        } else if (t <= 30.0) {
            float d = t - 20.0f;
            leader_state.velocity[0] = 0.0f;
            leader_state.position[0] = 30.0f;
            leader_state.velocity[1] = 3.0f - 0.3f * d;
            leader_state.position[1] = 45.0f + 3.0f * d
                                       - 0.5f * 0.3f * d * d;
        } else {
            leader_state.velocity[0] = 0.0f;
            leader_state.position[0] = 30.0f;
            leader_state.velocity[1] = 0.0f;
            leader_state.position[1] = 60.0f;
        }
        return leader_state;
    }

    void controlLoop() {
        NeighborState leader_state = getVirtualLeaderState();
        neighbor_states_[0] = leader_state;

        if (!own_state_valid_ || !neighbor_states_[2].valid
            || !neighbor_states_[3].valid) return;

        double dt = (this->now() - last_control_time_).seconds();
        last_control_time_ = this->now();

        px4_msgs::msg::OffboardControlMode offboard_msg;
        offboard_msg.position = false;
        offboard_msg.velocity = true;
        offboard_msg.acceleration = false;
        offboard_msg.timestamp = this->now().nanoseconds() / 1000;
        offboard_control_mode_publisher_->publish(offboard_msg);

        static int init_counter = 0;
        if (init_counter < 20) {
            px4_msgs::msg::TrajectorySetpoint setpoint{};
            setpoint.timestamp = get_clock()->now().nanoseconds() / 1000;
            setpoint.position = {std::numeric_limits<float>::quiet_NaN(),
                                std::numeric_limits<float>::quiet_NaN(),
                                std::numeric_limits<float>::quiet_NaN()};
            setpoint.velocity = {0.0f, 0.0f, 0.0f};
            setpoint.acceleration = {std::numeric_limits<float>::quiet_NaN(),
                                    std::numeric_limits<float>::quiet_NaN(),
                                    std::numeric_limits<float>::quiet_NaN()};
            trajectory_setpoint_publisher_->publish(setpoint);
            init_counter++;
            if (init_counter == 20) {
                px4_msgs::msg::VehicleCommand cmd;
                cmd.command = px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE;
                cmd.param1 = 1.0f; cmd.param2 = 6.0f;
                cmd.target_system = 2; cmd.target_component = 1;
                cmd.source_system = 1; cmd.source_component = 1;
                cmd.from_external = true;
                cmd.timestamp = this->now().nanoseconds() / 1000;
                vehicle_command_publisher_->publish(cmd);
            }
            return;
        }

        float sum_dx = (neighbor_states_[0].position[0] - own_position_[0])
                     + (neighbor_states_[2].position[0] - own_position_[0]
                        + 5.0f)
                     + (neighbor_states_[3].position[0] - own_position_[0]
                        - 5.0f);

        float sum_dy = (neighbor_states_[0].position[1] - own_position_[1])
                     + (neighbor_states_[2].position[1] - own_position_[1]
                        + 10.0f)
                     + (neighbor_states_[3].position[1] - own_position_[1]
                        + 10.0f);

        float sum_dvx = (neighbor_states_[0].velocity[0] - own_velocity_[0])
                      + (neighbor_states_[2].velocity[0] - own_velocity_[0])
                      + (neighbor_states_[3].velocity[0] - own_velocity_[0]);

        float sum_dvy = (neighbor_states_[0].velocity[1] - own_velocity_[1])
                      + (neighbor_states_[2].velocity[1] - own_velocity_[1])
                      + (neighbor_states_[3].velocity[1] - own_velocity_[1]);

        float ax = position_gain_ * sum_dx + velocity_gain_ * sum_dvx;
        float ay = position_gain_ * sum_dy + velocity_gain_ * sum_dvy;

        float vx_integrated = desired_velocity_[0] + ax * dt;
        float vy_integrated = desired_velocity_[1] + ay * dt;

        desired_velocity_[0] = filter_alpha_ * vx_integrated
                             + (1.0f - filter_alpha_) * desired_velocity_[0];
        desired_velocity_[1] = filter_alpha_ * vy_integrated
                             + (1.0f - filter_alpha_) * desired_velocity_[1];
        desired_velocity_[2] = 0.0f;

        px4_msgs::msg::TrajectorySetpoint setpoint;
        setpoint.timestamp = this->now().nanoseconds() / 1000;
        setpoint.velocity = {desired_velocity_[0], desired_velocity_[1], 0.0f};
        setpoint.position = {NAN, NAN, NAN};
        setpoint.acceleration = {NAN, NAN, NAN};
        trajectory_setpoint_publisher_->publish(setpoint);
    }

    // Publishers
    rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr
        offboard_control_mode_publisher_;
    rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::SharedPtr
        trajectory_setpoint_publisher_;
    rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr
        vehicle_command_publisher_;

    // Subscribers
    rclcpp::Subscription<px4_msgs::msg::VehicleOdometry>::SharedPtr
        own_odometry_sub_;
    rclcpp::Subscription<px4_msgs::msg::VehicleOdometry>::SharedPtr uav2_sub_;
    rclcpp::Subscription<px4_msgs::msg::VehicleOdometry>::SharedPtr uav3_sub_;

    rclcpp::TimerBase::SharedPtr control_timer_;

    std::array<float, 3> own_position_ = {0.0f, 0.0f, 0.0f};
    std::array<float, 3> own_velocity_ = {0.0f, 0.0f, 0.0f};
    std::array<float, 3> desired_velocity_ = {0.0f, 0.0f, 0.0f};
    bool own_state_valid_ = false;

    std::map<int, NeighborState> neighbor_states_;
    float position_gain_, velocity_gain_, filter_alpha_;
    rclcpp::Time last_print_time_, last_control_time_, start_time_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<UAV1Controller>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

仿真效果

开始时，在三架无人机的终端中使用 commander takeoff 手动启动并在 $2.5\text{m}$ 高度悬停，随后在 $xy$ 方向进行控制，使其按编队跟随虚拟领航者运动。

整体编队运动效果良好，存在一定超调但保持了稳定性，说明二阶控制律有效。

值得关注的是，利用分布式通信的独特优势，我们还可以做进一步的鲁棒性测试：在整体开始运动后，关掉第 $1$ 号无人机的仿真（模拟被击落场景），剩余的无人机仍然按固定路线飞行，不受影响。这正是分布式通信的核心可靠性所在——利用连通的通信拓扑网络实现数据交换，在实际应用中可以有效减少单点故障对编队整体的影响。

总结

本文从一阶集群系统出发，逐步引入二阶动力学模型，搭建了一个基于分布式通信的无人机编队控制系统。从一阶位置一致性推导至二阶模型的误差动力学方程和 Lyapunov 稳定性条件，通过同余变换将稳定性条件转化为可求解的 LMI，在 MATLAB 中利用 YALMIP 工具箱求得反馈增益矩阵；同时将加速度形式的控制指令通过积分和滤波转化为速度指令，兼容 PX4 的级联 PID 控制架构，并对含常定编队偏置的情形进行了严格推导，验证了 LMI 条件不变。

分布式控制相比于集中式控制具有更高的鲁棒性和可扩展性，在复杂、突变的实际应用场景中具有显著优势。在低空经济、蜂群作战等场景中，这种通信控制一体化的架构将发挥越来越重要的作用。

本文只是一个初步的控制方案，在仿真中也发现了许多可以改进的地方——例如如何进一步抑制超调、如何适应非连通的通信拓扑切换、如何在真实机载电脑上进行计算等。关于如何将仿真算法部署到真实无人机上，可以参考 ROS2 真机实践：为机载电脑刷机和使用机载电脑控制 PX4 无人机（MAVROS2），了解从仿真到实机的完整流程。

最后，再次感谢大家的耐心阅读和批评指正！

参考资料

ROS2+PX4 Multi-Drone Offboard Control Tutorial

Fri, 10 Jul 2026 00:00:00 GMT

Introduction

Following up on the multi-drone simulation setup covered in the previous article, this article explains how to control multiple drones using Offboard mode in ROS2. Multi-drone simulation involves several subtle naming conventions and code details that are easy to overlook — this article covers them all. If you encounter issues during simulation, use this as a troubleshooting reference.

Communication Overview

Multi-drone communication is more convenient in ROS2. Let's reference the PX4 official documentation:

XRCE-DDS allows multiple clients to connect to the same agent via UDP. This is particularly useful in simulation since only one agent needs to be started.

Following the previous article, each drone instance already has an independent px4_instance number assigned, corresponding to the -i 0/1/2... flag in our launch command. There's no need to manually assign instance numbers. Let's now proceed to Offboard control.

Starting the Simulation

Following the workflow from the previous article, we start two x_500 drone instances. Launch QGC ground station and Gazebo first, then open two terminals to start instances at positions (0,0) and (0,2) with px4_instance numbers 0 and 1:

# Terminal 1
cd ~/PX4-Autopilot
PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART=4001 PX4_GZ_MODEL_POSE="0,0" PX4_SIM_MODEL=gz_x500 ./build/px4_sitl_default/bin/px4 -i 0

# Terminal 2
cd ~/PX4-Autopilot
PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART=4001 PX4_GZ_MODEL_POSE="0,2" PX4_SIM_MODEL=gz_x500 ./build/px4_sitl_default/bin/px4 -i 1

Now check the ROS2 topics:

ros2 topic list

This shows the published topics from both PX4 instances:

We can see that topics from both PX4 instances are successfully published. Note the naming convention — you'll need this when writing control code:

Naming Rule: When the instance number is 0, topic names start with /fmu/. When the instance number is ≥ 1, topic names start with /px4_i/fmu/, where i is the instance number from the launch command. This is critical.

Starting Communication

Now add communication for both drones. As explained above, use:

MicroXRCEAgent udp4 -p 8888

A single agent instance handles both drones. From the terminal output we can see two client IDs: 0x00000001 and 0x00000002. These correspond to the MAVLINK IDs, as explained in the official documentation:

From the first table: when the agent starts, it automatically assigns UXRCE_DDS_KEY values to each instance. This value equals the MAVLINK ID, and both equal px4_instance + 1. So our drone 0 maps to 1, and drone 1 maps to 2.

From the second table: the target_system number also equals the MAVLINK ID, which is px4_instance + 1.

With these naming conventions clarified, we can now move on to the code.

Dual-Drone Offboard Code Walkthrough

We'll write a leader-follower formation flight controller using Offboard mode.

#include <px4_msgs/msg/offboard_control_mode.hpp>
#include <px4_msgs/msg/trajectory_setpoint.hpp>
#include <px4_msgs/msg/vehicle_command.hpp>
#include <rclcpp/rclcpp.hpp>
#include <stdint.h>

#include <chrono>
#include <cmath>
#include <iostream>

using namespace std::chrono;
using namespace std::chrono_literals;
using namespace px4_msgs::msg;

Include headers and declare namespaces. px4_msgs is the most heavily used PX4 message package — we primarily use three types: OffboardControlMode, TrajectorySetpoint, and VehicleCommand.

class MultiOffboardControl : public rclcpp::Node
{
public:
    MultiOffboardControl() : Node("multi_offboard_control")
    {
        offboard_control_mode_publisher_uav0_ = this->create_publisher<OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
        trajectory_setpoint_publisher_uav0_ = this->create_publisher<TrajectorySetpoint>("/fmu/in/trajectory_setpoint", 10);
        vehicle_command_publisher_uav0_ = this->create_publisher<VehicleCommand>("/fmu/in/vehicle_command", 10);

        offboard_control_mode_publisher_uav1_ = this->create_publisher<OffboardControlMode>("/px4_1/fmu/in/offboard_control_mode", 10);
        trajectory_setpoint_publisher_uav1_ = this->create_publisher<TrajectorySetpoint>("/px4_1/fmu/in/trajectory_setpoint", 10);
        vehicle_command_publisher_uav1_ = this->create_publisher<VehicleCommand>("/px4_1/fmu/in/vehicle_command", 10);

        offboard_setpoint_counter_ = 0;
        time_start_ = this->get_clock()->now();

        auto timer_callback = [this]() -> void {
            if (offboard_setpoint_counter_ == 10 || offboard_setpoint_counter_ % 50 == 0) {
                this->publish_vehicle_command_uav0(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
                this->arm_uav0();
                this->publish_vehicle_command_uav1(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
                this->arm_uav1();
            }

            publish_offboard_control_mode_uav0();
            publish_trajectory_setpoint_uav0();
            publish_offboard_control_mode_uav1();
            publish_trajectory_setpoint_uav1();

            if (offboard_setpoint_counter_ < 11) {
                offboard_setpoint_counter_++;
            }
        };
        timer_ = this->create_wall_timer(100ms, timer_callback);
    }

Define the MultiOffboardControl class and constructor, naming the node "multi_offboard_control". The constructor creates publishers for both drone instances' ROS2 topics with a queue size of 10. This is where the topic naming rule matters: drone 0 uses /fmu/ prefix, drone 1 uses /px4_1/fmu/ prefix. Pay close attention here.

A 100ms timer is set up: on the 10th cycle it arms both drones, and thereafter it continuously publishes Offboard control commands.

Note: The timer frequency must meet the Offboard minimum of 2Hz. Our 100ms (10Hz) satisfies this requirement. Lower frequencies combined with position-only control may result in larger trajectory deviations.

    void arm_uav0() { publish_vehicle_command_uav0(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0); }
    void disarm_uav0() { publish_vehicle_command_uav0(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0); }
    void arm_uav1() { publish_vehicle_command_uav1(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0); }
    void disarm_uav1() { publish_vehicle_command_uav1(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0); }

private:
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<OffboardControlMode>::SharedPtr offboard_control_mode_publisher_uav0_;
    rclcpp::Publisher<TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher_uav0_;
    rclcpp::Publisher<VehicleCommand>::SharedPtr vehicle_command_publisher_uav0_;
    rclcpp::Publisher<OffboardControlMode>::SharedPtr offboard_control_mode_publisher_uav1_;
    rclcpp::Publisher<TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher_uav1_;
    rclcpp::Publisher<VehicleCommand>::SharedPtr vehicle_command_publisher_uav1_;
    uint64_t offboard_setpoint_counter_;
    rclcpp::Time time_start_;
    float leader_position_[3] = {0.0, 0.0, -5.0};

arm_uav0() and disarm_uav0() arm and disarm drone 0 respectively — the same commands we saw in the PX4 terminal in the previous article.

For trajectory generation, we use the simplest leader-follower algorithm: the leader's initial position is (0, 0, -5). Drone 0 (leader) flies in a circular pattern at altitude 5m, radius 5m, period 20s. Drone 1 (follower) tracks the leader's position with a fixed offset — 2m behind and 2m to the right.

Each drone instance has three corresponding functions: publish_offboard_control_mode_uav0/1(), publish_trajectory_setpoint_uav0/1(), and publish_vehicle_command_uav0/1(), corresponding to the three main message types. Let's examine each:

    void publish_offboard_control_mode_uav0()
    {
        OffboardControlMode msg{};
        msg.position = true;
        msg.velocity = false;
        msg.acceleration = false;
        msg.attitude = false;
        msg.body_rate = false;
        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
        offboard_control_mode_publisher_uav0_->publish(msg);
    }

publish_offboard_control_mode_uav0() sends the control mode to drone 0. Setting msg.position = true enables position control mode in Offboard. Other modes (velocity, acceleration, attitude, body rate) are set to false. For more advanced flight control algorithm research, modify these flags to use multiple control modes.

    void publish_trajectory_setpoint_uav0()
    {
        auto now = this->get_clock()->now();
        double t = (now - time_start_).seconds();
        const double radius = 5.0;
        const double period = 20.0;
        double omega = 2.0 * M_PI / period;

        TrajectorySetpoint msg{};
        msg.position = {
            static_cast<float>(radius * std::cos(omega * t)),
            static_cast<float>(radius * std::sin(omega * t)),
            -5.0f
        };
        msg.yaw = -omega * t;
        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
        trajectory_setpoint_publisher_uav0_->publish(msg);

        leader_position_[0] = msg.position[0];
        leader_position_[1] = msg.position[1];
        leader_position_[2] = msg.position[2];
        RCLCPP_INFO(this->get_logger(), "uav0 position: x=%f, y=%f, z=%f",
                    leader_position_[0], leader_position_[1], leader_position_[2]);
    }

publish_trajectory_setpoint_uav0() sends trajectory setpoints to drone 0 (the leader), which flies in a circle at constant angular velocity.

A note on msg.yaw: this represents the quadrotor's yaw angle using Euler angle attitude representation. Quadrotors use the NED right-hand coordinate system with counterclockwise as the positive rotation direction. The three Euler angles — roll, pitch, and yaw — represent the drone's orientation. In both drones' trajectory code, yaw is set to align the drone with the flight path tangent/normal. Below are Euler angle reference diagrams:

    void publish_vehicle_command_uav0(uint16_t command, float param1 = 0.0, float param2 = 0.0)
    {
        VehicleCommand msg{};
        msg.param1 = param1;
        msg.param2 = param2;
        msg.command = command;
        msg.target_system = 1;
        msg.target_component = 1;
        msg.source_system = 1;
        msg.source_component = 1;
        msg.from_external = true;
        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
        RCLCPP_INFO(this->get_logger(), "uav0 command: %u, param1=%f, param2=%f, target_system=%u",
                    command, param1, param2, msg.target_system);
        vehicle_command_publisher_uav0_->publish(msg);
    }

publish_vehicle_command_uav0() sends vehicle commands to drone 0. Two parameters deserve attention: msg.target_system follows the px4_instance + 1 rule (drone 0 → 1, drone 1 → 2, etc.). msg.from_external must be true since we're using external Offboard control mode.

    void publish_offboard_control_mode_uav1()
    {
        OffboardControlMode msg{};
        msg.position = true;
        msg.velocity = false;
        msg.acceleration = false;
        msg.attitude = false;
        msg.body_rate = false;
        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
        offboard_control_mode_publisher_uav1_->publish(msg);
    }

    void publish_trajectory_setpoint_uav1()
    {
        const float offset_x = 2.0;
        const float offset_y = 2.0;
        const float offset_z = 0.0;

        TrajectorySetpoint msg{};
        msg.position = {
            leader_position_[0] + offset_x,
            leader_position_[1] + offset_y,
            leader_position_[2] + offset_z
        };
        msg.yaw = -std::atan2(msg.position[1], msg.position[0]);
        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
        RCLCPP_INFO(this->get_logger(), "uav1 setpoint: x=%f, y=%f, z=%f",
                    msg.position[0], msg.position[1], msg.position[2]);
        trajectory_setpoint_publisher_uav1_->publish(msg);
    }

    void publish_vehicle_command_uav1(uint16_t command, float param1 = 0.0, float param2 = 0.0)
    {
        VehicleCommand msg{};
        msg.param1 = param1;
        msg.param2 = param2;
        msg.command = command;
        msg.target_system = 2;
        msg.target_component = 1;
        msg.source_system = 1;
        msg.source_component = 1;
        msg.from_external = true;
        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
        RCLCPP_INFO(this->get_logger(), "uav1 command: %u, param1=%f, param2=%f, target_system=%u",
                    command, param1, param2, msg.target_system);
        vehicle_command_publisher_uav1_->publish(msg);
    }
};

Drone 1's (follower) three functions mirror the leader's, with the topic prefix changed to /px4_1/fmu/, target_system set to 2, and trajectory set to the leader's current position plus a fixed offset of (2.0, 2.0, 0.0) for rear-right formation flight.

int main(int argc, char *argv[])
{
    std::cout << "Starting multi offboard control node..." << std::endl;
    setvbuf(stdout, NULL, _IONBF, BUFSIZ);
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MultiOffboardControl>());
    rclcpp::shutdown();
    return 0;
}

The main() function: rclcpp::init initializes the ROS2 environment, rclcpp::spin runs the multi_offboard_control node, and rclcpp::shutdown cleans up on exit.

After compiling and running as described in the previous article, both drones follow the programmed trajectories and maintain formation, confirming the dual-drone formation simulation is working correctly. Below is the simulation video:

Summary

Drone formation simulation is more convenient in ROS2, primarily due to integrated communication — no need to manually add communication for each drone instance. However, subtle naming convention mistakes often cause failures: ROS2 topic naming rules, MAVLINK ID rules, and target_system rules. This article has covered these conventions with reference to PX4 official documentation.

For clarity, this article used a simple two-drone leader-follower formation. Real-world applications demand far more sophisticated approaches. In a later article, we introduce Distributed Control & Second-Order Consensus, using LMI-based gain synthesis to achieve more robust multi-drone formation control. We look forward to seeing what you build.

Thank you for reading. Feedback, corrections, and suggestions are always welcome.

References

BUAA Reliable Flight Control Group
PX4 Official Documentation: ROS2 Multi-Vehicle Simulation

ROS2+PX4 多机OFFBOARD模式教程

Fri, 10 Jul 2026 00:00:00 GMT

引言

书接上文，我们基于已经介绍过的在ROS2+PX4环境下启动多机仿真的流程，介绍一下如何使用Offboard模式在ROS2中控制多机仿真。在多机仿真中，有一些细小的命名规则和代码细节需要注意，本文会一一讲解。如果读者在仿真过程中发现没有能够成功实现多无人机仿真，可以对照本文进行问题排查。

通信说明

在ROS2中进行多机通信会更加便捷，在此处我们参考一下PX4官方文档对此部分给出的说明：

XRCE-DDS允许多个客户端通过 UDP 连接到同一个代理。这在模拟中特别有用，因为只需要启动一个代理。

因此，在我们前一篇文章添加通信时，其实已经为不同的无人机实例分配好了一个独立的px4_instance编号，对应着我们启动命令的 -i 0/1/2...。因此我们不再需要手动的去为各个实例分配编号。现在，我们开始进行Offboard控制。

启动仿真

我们仿照前一篇文章的流程，启动两个x_500无人机实例。按照步骤启动QGC地面站、Gazebo仿真环境。打开两个终端启动实例，分别放置在(0,0)和(0,2)处，px4_instance编号分别为0,1：

# 第一个终端
cd ~/PX4-Autopilot
PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART=4001 PX4_GZ_MODEL_POSE="0,0" PX4_SIM_MODEL=gz_x500 ./build/px4_sitl_default/bin/px4 -i 0

# 第二个终端
cd ~/PX4-Autopilot
PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART=4001 PX4_GZ_MODEL_POSE="0,2" PX4_SIM_MODEL=gz_x500 ./build/px4_sitl_default/bin/px4 -i 1

此时我们在终端输入：

ros2 topic list

可以查看px4实例的话题名称：

我们可以发现，px4的两个实例的话题成功发布，现在我们需要记录一下这个命名规则，后续编写代码时需要用到：

命名规则：当实例编号为0时，话题的名称以 /fmu/ 开始；而当实例编号大于等于1时，话题的名称以 /px4_i/fmu/ 开始，其中 i 为我们启动仿真时的实例编号。这个规则需要注意。

启动通信

现在我们为两个无人机添加通信，按照上一篇文章和本文开头的说明，使用：

MicroXRCEAgent udp4 -p 8888

启动一个通信客户端即可完成通信，此时我们根据终端的输出可以发现通信成功，同时有两个客户端的ID，一个为 0x00000001，一个为 0x00000002，这与两个实例的MAVLINK ID相关，此处可以在官方文档上找到答案：

根据第一张图我们得知，在启动通信客户端时，自动为两个实例分配了UXRCE_DDS_KEY值，且与MAVLINK ID相同，值均为 px4_instance + 1，因此我们的0号无人机对应的是1，而1号无人机对应的是2。

根据第二张图我们得知，target_system 编号与MAVLINK ID相同，值也为 px4_instance + 1。

至此，关于多机通信的一些命名规则，以及一些容易被忽视的细节问题就解决好了。

双机Offboard代码解释

我们编写一个双无人机编队的飞行代码，使用Offboard模式进行外部控制。

#include <px4_msgs/msg/offboard_control_mode.hpp>
#include <px4_msgs/msg/trajectory_setpoint.hpp>
#include <px4_msgs/msg/vehicle_command.hpp>
#include <rclcpp/rclcpp.hpp>
#include <stdint.h>

#include <chrono>
#include <cmath>
#include <iostream>

using namespace std::chrono;
using namespace std::chrono_literals;
using namespace px4_msgs::msg;

添加头文件和命名空间，其中 px4_msgs 是我们使用最多的px4的消息类型，我们主要使用其中的三个：OffboardControlMode、TrajectorySetpoint 和 VehicleCommand。

class MultiOffboardControl : public rclcpp::Node
{
public:
    MultiOffboardControl() : Node("multi_offboard_control")
    {
        offboard_control_mode_publisher_uav0_ = this->create_publisher<OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
        trajectory_setpoint_publisher_uav0_ = this->create_publisher<TrajectorySetpoint>("/fmu/in/trajectory_setpoint", 10);
        vehicle_command_publisher_uav0_ = this->create_publisher<VehicleCommand>("/fmu/in/vehicle_command", 10);

        offboard_control_mode_publisher_uav1_ = this->create_publisher<OffboardControlMode>("/px4_1/fmu/in/offboard_control_mode", 10);
        trajectory_setpoint_publisher_uav1_ = this->create_publisher<TrajectorySetpoint>("/px4_1/fmu/in/trajectory_setpoint", 10);
        vehicle_command_publisher_uav1_ = this->create_publisher<VehicleCommand>("/px4_1/fmu/in/vehicle_command", 10);

        offboard_setpoint_counter_ = 0;
        time_start_ = this->get_clock()->now();

        auto timer_callback = [this]() -> void {
            if (offboard_setpoint_counter_ == 10 || offboard_setpoint_counter_ % 50 == 0) {
                this->publish_vehicle_command_uav0(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
                this->arm_uav0();
                this->publish_vehicle_command_uav1(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
                this->arm_uav1();
            }

            publish_offboard_control_mode_uav0();
            publish_trajectory_setpoint_uav0();
            publish_offboard_control_mode_uav1();
            publish_trajectory_setpoint_uav1();

            if (offboard_setpoint_counter_ < 11) {
                offboard_setpoint_counter_++;
            }
        };
        timer_ = this->create_wall_timer(100ms, timer_callback);
    }

随后定义 MultiOffboardControl 类，将节点命名为 "multi_offboard_control" 并进行初始化。在初始化时创建了发布者，发布两个无人机实例对应的ROS2话题，消息队列长为10。这个地方就要与上文我们介绍过的话题命名规则相对应，第0号无人机的话题以 /fmu/ 开头，后续以 /px4_i/fmu/ 开头，其中 i 为每个无人机实例的编号，此处需要小心。

随后是一个定时器的逻辑，我们使用一个100ms的定时器，在第十次循环发送解锁无人机的指令，后续持续发布Offboard控制指令，向无人机发送控制模式和轨迹信息。

注意：此处的定时器频率需要设定一个合适的值，由于Offboard是外部控制模式，其本身有很大的安全性挑战，所以其要求一个至少为2Hz的消息频率；此处我们是100ms（10Hz），满足要求。同时，如果我们使用的消息频率较低，且只使用位置控制（position_control），有可能会造成轨迹上较大的偏差。

    void arm_uav0() { publish_vehicle_command_uav0(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0); }
    void disarm_uav0() { publish_vehicle_command_uav0(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0); }
    void arm_uav1() { publish_vehicle_command_uav1(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0); }
    void disarm_uav1() { publish_vehicle_command_uav1(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0); }

private:
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<OffboardControlMode>::SharedPtr offboard_control_mode_publisher_uav0_;
    rclcpp::Publisher<TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher_uav0_;
    rclcpp::Publisher<VehicleCommand>::SharedPtr vehicle_command_publisher_uav0_;
    rclcpp::Publisher<OffboardControlMode>::SharedPtr offboard_control_mode_publisher_uav1_;
    rclcpp::Publisher<TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher_uav1_;
    rclcpp::Publisher<VehicleCommand>::SharedPtr vehicle_command_publisher_uav1_;
    uint64_t offboard_setpoint_counter_;
    rclcpp::Time time_start_;
    float leader_position_[3] = {0.0, 0.0, -5.0};

arm_uav0() 和 disarm_uav0() 两个函数是用于第0号无人机解锁和上锁，我们在上一篇文章的px4终端曾经看到过这些命令。

随后我们为两个无人机实例分别编写运动轨迹的逻辑，此处我们用一个最简单的leader-follower算法，即初始时 leader_position 为 (0, 0, -5)，后续我们选取第0号无人机为leader，按照半径为5、周期为20秒，在5米空中进行盘旋运动，同时将它的实时位置作为1号无人机（follower）的目标位置进行发送，让其在第0号无人机后方2米、右侧2米进行跟随伴飞。

我们可以看到，两个无人机实例分别对应三个函数，分别是 publish_offboard_control_mode_uav0/1()、publish_trajectory_setpoint_uav0/1() 以及 publish_vehicle_command_uav0/1()，与我们之前讲的三种主要的msgs消息类型相对应。他们分别用来发布控制模式、运动轨迹及速度和加速度、车辆命令（解锁、起飞、切换模式等信息），我们一一来看：

    void publish_offboard_control_mode_uav0()
    {
        OffboardControlMode msg{};
        msg.position = true;
        msg.velocity = false;
        msg.acceleration = false;
        msg.attitude = false;
        msg.body_rate = false;
        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
        offboard_control_mode_publisher_uav0_->publish(msg);
    }

publish_offboard_control_mode_uav0() 函数是向第0号无人机发送控制模式的函数，函数的内部我们将 msg.position 的值设为 true，用以启动Offboard中的位置控制模式，即向无人机发布轨迹信息，其他值设为 false，代表我们暂时不使用速度控制、加速度控制、姿态控制等。如果需要对无人机进行更细致的飞行控制算法的研究，可以在此进行修改，使用多种控制模式。

    void publish_trajectory_setpoint_uav0()
    {
        auto now = this->get_clock()->now();
        double t = (now - time_start_).seconds();
        const double radius = 5.0;
        const double period = 20.0;
        double omega = 2.0 * M_PI / period;

        TrajectorySetpoint msg{};
        msg.position = {
            static_cast<float>(radius * std::cos(omega * t)),
            static_cast<float>(radius * std::sin(omega * t)),
            -5.0f
        };
        msg.yaw = -omega * t;
        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
        trajectory_setpoint_publisher_uav0_->publish(msg);

        leader_position_[0] = msg.position[0];
        leader_position_[1] = msg.position[1];
        leader_position_[2] = msg.position[2];
        RCLCPP_INFO(this->get_logger(), "uav0 position: x=%f, y=%f, z=%f",
                    leader_position_[0], leader_position_[1], leader_position_[2]);
    }

publish_trajectory_setpoint_uav0() 函数是向第0号无人机发送轨迹信息的函数，第0号无人机作为我们选定的领航者，按照固定的角速度进行盘旋飞行。这个函数的轨迹逻辑较为简单，我们不再赘述，可以在上述的示例中找到详细的代码。

此处有一个小的说明，在该函数中有一个 msg.yaw，这表示四旋翼无人机的偏航角，这是使用欧拉角进行姿态表示。在四旋翼中，我们使用NED右手坐标系，取逆时针为旋转正方向，用英文roll、pitch、yaw分别表示无人机的滚转角、俯仰角和偏航角。在两架无人机的轨迹代码中，我们添加了偏航角的表达，这是为了让飞机沿着飞行轨迹的法向/切向进行运动，特此说明。在此处给出一个欧拉角的表示方法示意图：

    void publish_vehicle_command_uav0(uint16_t command, float param1 = 0.0, float param2 = 0.0)
    {
        VehicleCommand msg{};
        msg.param1 = param1;
        msg.param2 = param2;
        msg.command = command;
        msg.target_system = 1;
        msg.target_component = 1;
        msg.source_system = 1;
        msg.source_component = 1;
        msg.from_external = true;
        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
        RCLCPP_INFO(this->get_logger(), "uav0 command: %u, param1=%f, param2=%f, target_system=%u",
                    command, param1, param2, msg.target_system);
        vehicle_command_publisher_uav0_->publish(msg);
    }

publish_vehicle_command_uav0() 函数是向第0号无人机发送车辆命令的函数。在这个函数中，我们需要关注的有两个比较重要的参数：msg.target_system 和 msg.from_external；前一个参数是我们在文章开头时就已经提到过的，他的命名规则为 px4_instance + 1，与MAVLINK ID、UXRCE_DDS_KEY的值均相同。因此，第0号无人机的 target_system 值为1，第1号无人机的 target_system 值为2，以此类推，在后续添加多个无人机实例时，在 vehicle_command 函数里修改对应的值即可。第二个参数 from_external 是外部控制指令的参数，我们设为 true，因为我们此时使用的是外部控制模式。

    void publish_offboard_control_mode_uav1()
    {
        OffboardControlMode msg{};
        msg.position = true;
        msg.velocity = false;
        msg.acceleration = false;
        msg.attitude = false;
        msg.body_rate = false;
        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
        offboard_control_mode_publisher_uav1_->publish(msg);
    }

    void publish_trajectory_setpoint_uav1()
    {
        const float offset_x = 2.0;
        const float offset_y = 2.0;
        const float offset_z = 0.0;

        TrajectorySetpoint msg{};
        msg.position = {
            leader_position_[0] + offset_x,
            leader_position_[1] + offset_y,
            leader_position_[2] + offset_z
        };
        msg.yaw = -std::atan2(msg.position[1], msg.position[0]);
        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
        RCLCPP_INFO(this->get_logger(), "uav1 setpoint: x=%f, y=%f, z=%f",
                    msg.position[0], msg.position[1], msg.position[2]);
        trajectory_setpoint_publisher_uav1_->publish(msg);
    }

    void publish_vehicle_command_uav1(uint16_t command, float param1 = 0.0, float param2 = 0.0)
    {
        VehicleCommand msg{};
        msg.param1 = param1;
        msg.param2 = param2;
        msg.command = command;
        msg.target_system = 2;
        msg.target_component = 1;
        msg.source_system = 1;
        msg.source_component = 1;
        msg.from_external = true;
        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
        RCLCPP_INFO(this->get_logger(), "uav1 command: %u, param1=%f, param2=%f, target_system=%u",
                    command, param1, param2, msg.target_system);
        vehicle_command_publisher_uav1_->publish(msg);
    }
};

第1号无人机（follower）的三个函数与leader同理，只是话题前缀改为 /px4_1/fmu/，target_system 改为2，轨迹设定为leader当前位置加上固定偏移量 (2.0, 2.0, 0.0)，实现后方右侧伴飞。

int main(int argc, char *argv[])
{
    std::cout << "Starting multi offboard control node..." << std::endl;
    setvbuf(stdout, NULL, _IONBF, BUFSIZ);
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MultiOffboardControl>());
    rclcpp::shutdown();
    return 0;
}

最后运行 main() 函数，rclcpp::init 用于初始化ROS2环境，rclcpp::spin 用于运行 multi_offboard_control 节点，最后在程序结束时使用 rclcpp::shutdown 清理环境。整个代码流程结束。

我们按照上一篇文章所述将代码编译成可执行文件并运行，可以发现两架无人机成功按照我们的代码逻辑进行飞行，并保持一定的编队轨迹，证明我们的双无人机编队仿真代码成功运行。以下是仿真视频：

总结

在ROS2中，无人机编队的仿真会更加的容易，主要体现在其对于通信的集成，不需要我们再单独地为每架无人机实例添加通信。但是，在编写实际工程代码时，一些细微的命名规则的混淆往往会造成失败，主要体现在ROS2话题的命名规则、MAVLINK ID的命名规则、以及 target_system 的命名规则。本文参照PX4的官方文档，对这些规则进行了介绍，读者朋友们可以参考和排查。

同时，本文为了简明扼要地介绍多无人机Offboard控制流程，选择了一个简单的leader-follower两机的编队，在实际工程和科学研究中，这是远远不能够满足现实产品需求的。在后续文章中，我们将引入分布式通信与二阶一致性协议，利用 LMI 方法求解反馈增益矩阵，实现更高鲁棒性的多机编队协同控制。期待读者朋友们的优秀成果。

最后，再次感谢读者朋友们的支持和耐心的阅读，欢迎大家对本文进行批评指正、补充及建议。

参考资料

北京航空航天大学可靠飞行控制研究组（BUAA Reliable Flight Control Group）
PX4官方文档：ROS2 Multi-Vehicle Simulation

YOLOv8 Object Detection in PX4 Drone Simulation

Fri, 10 Jul 2026 00:00:00 GMT

Introduction

In the third part of our drone formation simulation series, we focus on integrating object detection into the ROS2+PX4 simulation environment using YOLOv8 for efficient real-time detection. YOLO (You Only Look Once) is a high-performance real-time object detection algorithm widely used in computer vision tasks such as drone target recognition, autonomous driving, and surveillance.

We use PX4 SITL (Software-In-The-Loop) with Gazebo Ignition and the gz_x500_depth quadrotor model. The system subscribes to the RGB camera (OakD-Lite) feed, applies YOLOv8 for target detection, and visualizes results in real-time on Ubuntu 22.04. This provides a foundation for more advanced cooperative drone tasks.

Creating a Python Virtual Environment

This setup uses Python 3.10.12 — adjust commands for your Python version as needed.

# Create virtual environment
python3 -m venv ~/px4-venv
# Activate
source ~/px4-venv/bin/activate
# Link ROS2 environment
source /opt/ros/humble/setup.bash
export PYTHONPATH=/opt/ros/humble/lib/python3.10/site-packages:$PYTHONPATH

Installing MAVSDK

If you encounter NumPy version conflicts, uninstall the current version with pip uninstall numpy -y first, then install NumPy 1.x as shown below.

pip install mavsdk
pip install aioconsole
pip install pygame
sudo apt install ros-humble-ros-gzgarden
pip install "numpy<2"
pip install opencv-python

Installing YOLO

pip install ultralytics

Launching the Quadrotor with Camera and Adding Camera Topics

We modify the launch command slightly from the previous articles:

PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART=4001 PX4_GZ_MODEL_POSE="0,0" PX4_SIM_MODEL=gz_x500_depth ./build/px4_sitl_default/bin/px4 -i 0

For the x500 model, 4001/4002 correspond to two different quadrotor configurations ("X" frame and cross frame). Here we use the gz_x500_depth model with a depth camera, which differs from the previous setup.

Now let's examine the model.sdf file for x500_depth and its camera module OakD_Lite:

The OakD-Lite camera module has two camera sensors: the IMX214 (RGB camera) and the StereoOV7251 (depth camera). The RGB camera outputs RGB_INT8 format, while the depth camera outputs R_FLOAT32 format. RGB cameras are commonly used for image recognition, while depth cameras measure distance and are useful for obstacle avoidance. Here, we'll use the RGB camera first.

Key parameters from the SDF file:

<width>1920</width>
<height>1080</height>
<format>RGB_INT8</format>

These three parameters represent image dimensions and format — critical for the code that follows. RGB_INT8 is a three-channel format.

<always_on>1</always_on>
<update_rate>30</update_rate>
<visualize>true</visualize>
<topic>image_raw</topic>

<always_on> keeps the camera continuously active. The camera has a ROS2 topic named image_raw (added manually here), which we'll use for message transport.

Running YOLOv8 Object Detection

mkdir -p ~/YOLO
cd ~/YOLO
touch uav_camera_det.py

Create a /YOLO directory and write a Python script for YOLOv8 object detection:

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
from ultralytics import YOLO

model = YOLO('yolov8m.pt')

Import the required libraries and image conversion tools. rclpy is the ROS2 Python client library, CvBridge converts between ROS and OpenCV image formats, and yolov8m.pt is the medium-sized model trained on the COCO dataset (80 object classes).

COCO dataset categories:

0: person         1: bicycle       2: car           3: motorcycle
4: airplane       5: bus           6: train         7: truck
8: boat           9: traffic light 10: fire hydrant 11: stop sign
...
76: scissors      77: teddy bear   78: hair drier   79: toothbrush

To reduce computation and improve detection speed, we can limit the target classes — more on that below.

class ImageSubscriber(Node):
    def __init__(self):
        super().__init__('image_subscriber')
        self.subscription = self.create_subscription(
            Image,
            '/image_raw',
            self.listener_callback,
            10)
        self.br = CvBridge()
        self.frame_count = 0
        self.process_interval = 1

        cv2.namedWindow('YOLOv8 Detection', cv2.WINDOW_NORMAL)
        cv2.resizeWindow('YOLOv8 Detection', 640, 480)

Similar to the C++ Offboard code in the previous article, we define a new class image_subscriber as a ROS2 subscriber instance. The topic name must match the one we added to the RGB camera's SDF file: /image_raw. CvBridge converts ROS image format (RGB8) to OpenCV format (BGR8). The display window is set to 640×480.

def listener_callback(self, data):
    self.frame_count += 1
    if self.frame_count % self.process_interval != 0:
        self.get_logger().info("Skipping frame")
        return
    try:
        self.get_logger().info(f"Received frame, encoding: {data.encoding}")
        image = self.br.imgmsg_to_cv2(data, desired_encoding="bgr8")
        self.get_logger().info(f"Image shape: {image.shape}")
        image = cv2.resize(image, (640, 480))
        cv2.imwrite("input_image.jpg", image)
        results = model.predict(image, classes=[0, 2])
        annotated_image = results[0].plot()
        cv2.imwrite("yolo_output.jpg", annotated_image)
        cv2.imshow('YOLOv8 Detection', annotated_image)

        if cv2.waitKey(1) & 0xFF == ord('q'):
            raise KeyboardInterrupt
        self.get_logger().info(f"Detected {len(results[0].boxes)} objects")
        for box in results[0].boxes:
            self.get_logger().info(f"Detected: class={box.cls}, conf={box.conf}, bbox={box.xyxy}")
    except Exception as e:
        self.get_logger().error(f"Image processing error: {str(e)}")

The callback function: cv2.resize(image, (640, 480)) preprocesses the 1920×1080 image from the SDF config down to 640×480. results = model.predict(image, classes=[0, 2]) runs YOLOv8 with detection limited to classes 0 and 2 (person, car) for faster performance. To detect all 80 classes, use results = model.predict(image) without the classes parameter — though this is computationally expensive.

def destroy_node(self):
    cv2.destroyAllWindows()
    super().destroy_node()

Override the node cleanup method to close OpenCV windows and clean up ROS2 resources.

def main(args=None):
    rclpy.init(args=args)
    image_subscriber = ImageSubscriber()
    try:
        rclpy.spin(image_subscriber)
    except KeyboardInterrupt:
        pass
    finally:
        image_subscriber.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

The main() function initializes the ROS2 environment, spins the subscriber node, and cleans up on exit.

Running the Simulation

# Terminal 1
python3 simulation-gazebo

# Terminal 2
cd ~/PX4-Autopilot
PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART=4001 PX4_GZ_MODEL_POSE="0,0" PX4_SIM_MODEL=gz_x500_depth ./build/px4_sitl_default/bin/px4 -i 0

# Terminal 3
MicroXRCEAgent udp4 -p 8888

Start Gazebo, the depth-camera quadrotor, and communication as before. Then run the ROS2-Gazebo bridge:

ros2 run ros_gz_image image_bridge /image_raw

This bridges Gazebo's camera topics to ROS2 sensor_msgs.msg.Image messages, publishing to the /image_raw topic we configured in the SDF file.

Now run YOLOv8 detection and Offboard control:

# Terminal 1 — Activate virtual environment and run YOLO
source ~/px4-venv/bin/activate
cd ~/YOLO
python3 uav_camera_det.py

# Terminal 2 — Run Offboard control
ros2 run multioffboardcontrol uav0_1

Results

The depth-camera quadrotor starts successfully and YOLOv8 detection is active. Since no vehicles or people were placed in the simulation world, no targets are detected — users can add objects to verify detection.

Note that since the camera is rigidly attached to the drone's body frame and the Offboard control uses circular motion, the camera rotates with the drone, causing unstable imagery. Using level flight with adjustable camera gimbal would improve detection stability.

Summary

In this article, we integrated YOLOv8 for object detection in the ROS2+PX4 simulation environment. Key points include understanding the difference between RGB and depth cameras, ensuring the SDF file has the required topic configuration (add it manually if not), and considerations for practical improvements such as compensating for body-frame camera rotation and improving detection accuracy.

With both RGB and depth cameras available, the quadrotor can perform object detection, autonomous obstacle avoidance, and more. To integrate visual perception into multi-drone formations, see the earlier ROS2+PX4 Multi-Drone Offboard Control guide — we look forward to seeing your applications.

Thank you for reading. Feedback, corrections, and suggestions are always welcome.

References

无人机仿真环境调用YOLO的简单示例

Fri, 10 Jul 2026 00:00:00 GMT

简介

在无人机编队仿真的第三部分，我们聚焦于将目标识别技术集成到 ROS2+PX4 仿真环境中，利用YOLOv8实现高效的实时目标检测。YOLO（You Only Look Once）是一种高效、实时的目标检测算法，广泛应用于计算机视觉任务，如无人机目标识别、自动驾驶和监控。

我们基于PX4 SITL（软件在环仿真）和Gazebo Ignition，结合gz_x500_depth四旋翼模型，展示如何通过ROS2订阅无人机搭载的RGB相机（OakD-Lite）图像，应用YOLOv8模型检测特定目标，并在Ubuntu 22.04环境中实时可视化检测结果。这可以为后续编队协同任务提供更多技术支持。

创建Python虚拟环境

我们使用的Python版本号为3.10.12，读者可以根据自己的Python版本更改命令。

# 创建虚拟环境
python3 -m venv ~/px4-venv
# 激活
source ~/px4-venv/bin/activate
# 链接ROS2环境
source /opt/ros/humble/setup.bash
export PYTHONPATH=/opt/ros/humble/lib/python3.10/site-packages:$PYTHONPATH

安装MAVSDK

如果后续出现Numpy的版本号冲突的问题，建议先使用 pip uninstall numpy -y 卸载当前版本，再按照以下命令安装Numpy 1.x版本。

pip install mavsdk
pip install aioconsole
pip install pygame
sudo apt install ros-humble-ros-gzgarden
pip install "numpy<2"
pip install opencv-python

安装YOLO

pip install ultralytics

启动带相机的四旋翼模型并添加相机话题

我们稍微修改一下前两文的启动仿真命令：

PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART=4001 PX4_GZ_MODEL_POSE="0,0" PX4_SIM_MODEL=gz_x500_depth ./build/px4_sitl_default/bin/px4 -i 0

我们再来看一下这个启动命令，对于x500模型，4001/4002分别对应两种不同的四旋翼配置（"X"型和十字型机架），在此处，我们使用带深度相机的 gz_x500_depth 的x_500模型，这与之前的有所不同。

我们前往models所在的文件夹，找到 x500_depth 的 model.sdf 文件以及其相机模块 OakD_Lite 的sdf文件：

我们发现，相机模块OakD_Lite一共搭载了两个相机传感器，分别为IMX214（RGB相机）和StereoOV7251（深度相机）。其中RGB相机的输出图像格式为RGB_INT8，深度相机的输出图像格式为R_FLOAT32。RGB相机是常用的进行图像识别的传感器，而深度相机用于探测距离，在避障任务中有很好的应用。在此处，我们先使用RGB相机。

现在我们需要关注sdf文件的这几行：

<width>1920</width>
<height>1080</height>
<format>RGB_INT8</format>

这三个是比较重要的参数，代表图像的尺寸和格式，这在后续的代码中需要注意，尤其是图像的格式需要匹配，RGB_INT8是一个三通道的参数。

<always_on>1</always_on>
<update_rate>30</update_rate>
<visualize>true</visualize>
<topic>image_raw</topic>

<always_on> 代表相机持续保持开启状态，我们在最后为这个相机添加一个ROS2话题（此处为笔者自行添加），名称为 image_raw，后续我们会利用这个话题进行消息的传输。

调用YOLOv8进行目标识别

mkdir -p ~/YOLO
cd ~/YOLO
touch uav_camera_det.py

创建一个/YOLO目录，现在编写一个调用YOLOv8进行目标检测的Python代码。

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
from ultralytics import YOLO

model = YOLO('yolov8m.pt')

调用一些需要的库和图像转换工具，类似于上一篇文章，rclpy是ROS2的Python客户端，CvBridge是ROS与openCV的图像转换工具，yolov8m.pt 是中等规模模型（Medium），基于COCO数据集训练，支持80类目标。以下是COCO数据集的80个完整类别：

0: person         1: bicycle       2: car           3: motorcycle
4: airplane       5: bus           6: train         7: truck
8: boat           9: traffic light 10: fire hydrant 11: stop sign
12: parking meter 13: bench        14: bird         15: cat
16: dog           17: horse        18: sheep        19: cow
20: elephant      21: bear         22: zebra        23: giraffe
24: backpack      25: umbrella     26: handbag      27: tie
28: suitcase      29: frisbee      30: skis         31: snowboard
32: sports ball   33: kite         34: baseball bat 35: baseball glove
36: skateboard    37: surfboard    38: tennis racket 39: bottle
40: wine glass    41: cup          42: fork         43: knife
44: spoon         45: bowl         46: banana       47: apple
48: sandwich      49: orange       50: broccoli     51: carrot
52: hot dog       53: pizza        54: donut        55: cake
56: chair         57: couch        58: potted plant 59: bed
60: dining table  61: toilet       62: tv           63: laptop
64: mouse         65: remote       66: keyboard     67: cell phone
68: microwave     69: oven         70: toaster      71: sink
72: refrigerator  73: book         74: clock        75: vase
76: scissors      77: teddy bear   78: hair drier   79: toothbrush

为了减少计算消耗，提高检测速度，我们往往将检测目标进行缩小，我们稍后讲。

class ImageSubscriber(Node):
    def __init__(self):
        super().__init__('image_subscriber')
        self.subscription = self.create_subscription(
            Image,
            '/image_raw',
            self.listener_callback,
            10)
        self.br = CvBridge()
        self.frame_count = 0
        self.process_interval = 1

        cv2.namedWindow('YOLOv8 Detection', cv2.WINDOW_NORMAL)
        cv2.resizeWindow('YOLOv8 Detection', 640, 480)

类似于上一篇文章使用C++编写的Offboard指令，这里我们定义一个新的类，发布一个名为 image_subscriber 的ROS2订阅者实例，进行初始化。我们在这里需要注意，在创建订阅者实例时，话题名称要与我们之前修改的RGB相机的sdf文件的话题名称相同，这里为我们之前发布的 /image_raw，需要匹配。调用CvBridge函数，这是因为我们需要将ROS的图像格式（RGB8）转换为OpenCV的图像格式（BGR8），最后将窗口可视化，以640×480的尺寸进行可视化。

def listener_callback(self, data):
    self.frame_count += 1
    if self.frame_count % self.process_interval != 0:
        self.get_logger().info("跳过帧")
        return
    try:
        self.get_logger().info(f"收到视频帧，编码: {data.encoding}")
        image = self.br.imgmsg_to_cv2(data, desired_encoding="bgr8")
        self.get_logger().info(f"图像尺寸: {image.shape}")
        image = cv2.resize(image, (640, 480))
        cv2.imwrite("input_image.jpg", image)
        results = model.predict(image, classes=[0, 2])
        annotated_image = results[0].plot()
        cv2.imwrite("yolo_output.jpg", annotated_image)
        cv2.imshow('YOLOv8 Detection', annotated_image)

        if cv2.waitKey(1) & 0xFF == ord('q'):
            raise KeyboardInterrupt
        self.get_logger().info(f"检测到 {len(results[0].boxes)} 个目标")
        for box in results[0].boxes:
            self.get_logger().info(f"检测到: 类别={box.cls}, 置信度={box.conf}, 坐标={box.xyxy}")
    except Exception as e:
        self.get_logger().error(f"处理图像错误: {str(e)}")

定义回调函数，image = cv2.resize(image, (640, 480)) 是用于图像的预处理的函数，将我们的sdf里定义的1920×1080的图像压缩为640×480。随后使用 results = model.predict(image, classes=[0, 2]) 运行YOLOv8，限制检测类别为0,2（对应人、车），这样可以提高检测的速度。如果要检测其他物体，可以按照上述的80个类别编号进行添加。如果80个类别都要检测，可以使用 results = model.predict(image) 实现全部检测，最后进行可视化，但这样肯定会极大地浪费计算效率。

def destroy_node(self):
    cv2.destroyAllWindows()
    super().destroy_node()

重写一个销毁ROS2节点的方法，关闭OpenCV的窗口，清理ROS2节点和环境。

def main(args=None):
    rclpy.init(args=args)
    image_subscriber = ImageSubscriber()
    try:
        rclpy.spin(image_subscriber)
    except KeyboardInterrupt:
        pass
    finally:
        image_subscriber.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

最后执行 main() 函数，实现对话题的订阅以及调用YOLOv8实现无人机的目标识别及检测。至此，我们进行ROS2+PX4+YOLOv8的目标识别任务的准备就做好了。

实现仿真

# 终端1
python3 simulation-gazebo

# 终端2
cd ~/PX4-Autopilot
PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART=4001 PX4_GZ_MODEL_POSE="0,0" PX4_SIM_MODEL=gz_x500_depth ./build/px4_sitl_default/bin/px4 -i 0

# 终端3
MicroXRCEAgent udp4 -p 8888

按照之前的步骤启动Gazebo仿真、带深度相机的四旋翼模型以及打开通信，现在我们需要运行这个命令：

ros2 run ros_gz_image image_bridge /image_raw

这是一个将ROS2与Gazebo进行桥接的命令，可以将Gazebo里的话题消息转换为ROS2的 sensor_msgs.msg.Image 消息，并发布到ROS2话题。此时我们订阅的就是我们之前在sdf文件里添加的 /image_raw 话题。

运行这个命令后，我们执行一下我们的YOLOv8检测的代码，并且运行一下我们的Offboard控制，让飞机飞起来，在可视化窗口查看一下摄像头和检测信息。

# 终端1 — 激活虚拟环境并运行YOLO检测
source ~/px4-venv/bin/activate
cd ~/YOLO
python3 uav_camera_det.py

# 终端2 — 运行Offboard控制
ros2 run multioffboardcontrol uav0_1

结果展示

我们发现，带深度相机的四旋翼飞机成功启动仿真，并且YOLOv8成功启动了检测。由于笔者的世界搭建里还没有加入车和人，所以没有目标探测到，读者朋友们可以进行添加并验证。

同时，由于相机是固连在机身坐标系上，而作者此时的Offboard模式使用的是盘旋指令，因此相机会跟着飞机姿态进行旋转，图像不稳定。后续可以采取平飞+调整相机姿态的方式实现更稳定的目标识别。

总结

在这篇文章里，我们通过调用YOLOv8实现了在ROS2+PX4中进行目标检测的任务。需要着重注意的是相机种类（RGB相机和深度相机）的区别，同时注意sdf文件里是否有相应的话题配置，如果没有，需要手动添加。在仿真过程中我们发现还有一些细节可以调整，例如如何适应相机随着机身坐标系的偏转，如何提高识别成功率等，读者朋友们可以在实际应用场景中进行改进。

在后续的任务中，搭配RGB相机和深度相机的四旋翼飞机可以实现场景下的目标识别、自主避障（深度相机）等任务。如果需要在多机编队中集成视觉感知，可以参考前文的 ROS2+PX4 多机 Offboard 控制，将目标检测与编队飞行结合。期待大家的实际应用。

最后，再次感谢读者朋友们的支持和耐心的阅读，欢迎大家对本文进行批评指正、补充及建议。

参考资料

ROS2+PX4 Drone Formation Simulation Setup Guide

Thu, 09 Jul 2026 00:00:00 GMT

Introduction

PX4, as one of the most popular open-source flight controllers worldwide, provides convenience for developers in drone design and control. ROS, as a robot operating system, simplifies the drone formation simulation workflow through its distributed communication architecture, while the Gazebo physics simulation engine provides a realistic simulation environment.

ROS has now been updated to ROS2, which offers greater applicability and maintainability compared to ROS1. There are currently few ROS2+PX4 simulation tutorials available, and continuous updates of both platforms bring many environment configuration challenges. This article provides a personal development workflow.

Installing Ubuntu

This environment is built on Ubuntu 22.04. You can install it as a virtual machine or set up a dual-boot system alongside Windows. Here is a video tutorial for installing Ubuntu 22.04 as a dual-boot system on Windows 11:

Step-by-step dual-boot installation: Windows 11 + Ubuntu 22.04

Now we begin the environment setup process. For reference, here is another developer's workflow that is similar to ours and quite concise. If you only need single-drone simulation, this tutorial is also a good reference:

Ubuntu PX4 Drone Simulation Environment Setup (5) -- Simulation Environment Setup (with Ubuntu 22.04, ROS2 Humble)

Note: Before starting the following steps, ensure you have reliable internet access (with proper network tools if needed), otherwise cloning repositories from GitHub may fail midway.

Downloading & Compiling PX4 Source

First, clone the PX4 source code from GitHub and update all submodules:

git clone https://github.com/PX4/PX4-Autopilot.git --recursive

Since the subsequent environment setup will install many packages, and it may be difficult to fully clean them up if something goes wrong, it is recommended to back up the ~/PX4-Autopilot directory at this point, so you don't need to re-clone the entire repository if the environment needs a fresh start:

zip -r PX4-Autopilot.zip PX4-Autopilot/

Next, install the required dependencies:

bash ./PX4-Autopilot/Tools/setup/ubuntu.sh

At this point, the PX4 source code is downloaded and all dependencies are installed. We can now try compiling and launching the simulation:

cd ~/PX4-Autopilot/
make px4_sitl         # Build the SITL firmware first (required!)
make px4_sitl jmavsim # Once built, launch jmavsim

Common error: If you run make px4_sitl jmavsim directly and encounter ninja: error: unknown target 'jmavsim', it means the PX4 CMake/Ninja build system hasn't generated the jmavsim target yet. You must run make px4_sitl first to complete the initial build, then make px4_sitl jmavsim to launch the simulator. After this first-time setup, make px4_sitl jmavsim will work directly.

You will see the PX4 source code compile and the jmavsim physics simulation engine start up. This works because the ubuntu.sh dependency script already installed jmavsim and Gazebo-classic for us. If you see green ready for takeoff text in the terminal and the jmavsim window appears, compilation is successful. If this step fails, check your network tools.

Now press Enter in the terminal, and at the pxh> prompt, enter PX4 commands:

commander takeoff/land/shutdown

These commands control the drone's takeoff, landing, and shutdown of the simulation. You should see the drone taking off and landing in the jmavsim UI. At this point, the PX4 source download and compilation are complete.

Installing ROS2 and Related Dependencies

We use ROS2 Humble for development. The installation and environment configuration for ROS2 and its dependencies can be somewhat tedious, so I strongly recommend using the "FishROS" one-click installation tool. Many thanks to FishROS for providing this convenience for the ROS community. Simply run the following command in your Ubuntu terminal and follow the on-screen instructions:

wget http://fishros.com/install -O fishros && . fishros

The one-click installation tool also includes options for Docker and other tools -- feel free to install them as needed. Again, many thanks. Additionally, developers using the ROS2 Foxy distribution can also use this tool for installation and environment configuration.

Installing Gazebo Sim 8.9.0

According to the official PX4 documentation, the former Gazebo Ignition has been renamed to Gazebo, and the old Gazebo is now called Gazebo-classic. Ubuntu 22.04 supports the Gazebo (Ignition) series. When we installed PX4 dependencies earlier, ubuntu.sh automatically installed jmavsim and Gazebo-classic, but for multi-drone simulation we need Gazebo Harmonic (v8.x, i.e., Gazebo Sim 8.9.0).

Important: The Gazebo version and the ROS2 bridge package must match. Mixing multiple versions is the primary cause of environment crashes. Gazebo Garden (v7), Harmonic (v8), and Ionic (v9) cannot coexist.

First, add the OSRF repository and install Gazebo Harmonic:

cd ~/PX4-Autopilot
sudo apt install wget
sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
sudo apt update
sudo apt-get install gz-harmonic

Then install the corresponding Gazebo-ROS2 communication bridge (Harmonic maps to gzharmonic):

sudo apt install ros-humble-ros-gzharmonic

After installation, verify version uniqueness (only one version number should appear):

gz sim --versions   # Should only show 8.9.0

Common mistake: Do not install gz-garden or ros-humble-ros-gzgarden. That is Gazebo Garden (v7), which is incompatible with PX4's current gz-sim8 compilation target.

Installing MicroXRCE-DDS Agent

According to the official documentation, in ROS2, PX4 uses the uXRCE-DDS middleware to allow publishing and subscribing to uORB messages on the companion computer, replacing MAVROS. We follow the official example to download, compile, and install.

Download the source:

git clone -b v2.4.2 https://github.com/eProsima/Micro-XRCE-DDS-Agent.git

Compile:

cd Micro-XRCE-DDS-Agent
mkdir build
cd build
cmake ..
make

There will likely be a build error here. Go to build/fastdds/tmp/fastdds-gitclone.cmake, change 2.12.x to v2.12.1, then re-run make.

The compilation process takes a while, so be patient. After compilation finishes, install:

sudo make install
sudo ldconfig /usr/local/lib/

Installing QGC Ground Station

When starting the PX4+Gazebo simulation, losing connection to the ground station will prevent PX4 from controlling the drone. Therefore, you need to install QGC Ground Station. Here is a CSDN guide that walks through the steps:

(Latest) Ubuntu PX4 Drone Simulation Environment Setup (3) -- Installing QGC Ground Station on Ubuntu

Single Drone Offboard Test and Development Guide

All development environment components are now set up. Let's verify PX4 Offboard mode.

Offboard mode is a special flight mode in PX4 that allows external systems (such as companion computers, ROS2 nodes, etc.) to send control commands directly via MAVLink or uXRCE-DDS, enabling real-time control of the drone's attitude, position, or velocity for autonomous flight or advanced missions.

First, create a ROS2 workspace:

mkdir -p ~/ros2_ws/src

Then download the source code (make sure your network is working):

cd ~/ros2_ws/src
git clone https://github.com/PX4/px4_msgs.git -b release/1.14
git clone https://github.com/PX4/px4_ros_com.git -b release/v1.14

Compile:

cd ~/ros2_ws
colcon build

Update environment and configure environment variables:

echo "source ~/ros2_ws/install/setup.bash" >> ~/.bashrc
echo "export GZ_SIM_RESOURCE_PATH=\$HOME/.simulation-gazebo/models:\$GZ_SIM_RESOURCE_PATH" >> ~/.bashrc
source ~/.bashrc

Launching the Offboard Simulation

Now we start the Offboard simulation. First, open the already installed QGC Ground Station.

Terminal 1 -- Start communication:

MicroXRCEAgent udp4 -p 8888

Terminal 2 -- Start simulation:

cd ~/PX4-Autopilot
make px4_sitl gz_x500

At this point, the ground station will connect to PX4, Gazebo will launch, and the x500 drone will appear. When the PX4 terminal shows green ready for takeoff, the simulation has started successfully. We will explain this make-based simulation startup in more detail in the multi-drone simulation section.

Terminal 3 -- Run Offboard control source:

ros2 run px4_ros_com offboard_control

You should see the drone ascend 5 meters in Gazebo, confirming Offboard mode control is successful.

Offboard Mode Development Workflow

In the ~/ros2_ws workspace we just created, navigate to the px4_ros_com directory and open CMakeLists.txt. Find these three lines:

add_executable(offboard_control src/examples/offboard/offboard_control.cpp)
ament_target_dependencies(offboard_control rclcpp px4_msgs)
install(TARGETS offboard_control DESTINATION lib/${PROJECT_NAME})

This compiles offboard_control.cpp from src/examples/offboard into an executable named offboard_control, with its dependency on px4_msgs. You can open the src/examples/offboard directory to find the offboard_control.cpp source code.

Therefore, to develop single-drone Offboard mode simulations, follow this workflow:

Inside the already created and configured ~/ros2_ws ROS2 workspace, create a new package (at the same level as the px4_ros_com directory).
In the new package, write your Offboard mode control code (C++/Python) under the /src directory.
In the new package's CMakeLists.txt, find these lines:

add_executable(offboard_control src/examples/offboard/offboard_control.cpp)
ament_target_dependencies(offboard_control rclcpp px4_msgs)
install(TARGETS offboard_control DESTINATION lib/${PROJECT_NAME})

Change the path to the location of your C++/Python code, and give it an executable name. Before running the simulation, you must recompile the ROS2 workspace!

cd ~/ros2_ws
colcon build

Finally, following the earlier method, open QGC Ground Station, start communication and simulation, then open a new terminal and run:

ros2 run <package_name> <executable_name>

If you see the drone flying according to your code logic in Gazebo, the simulation is successful.

Supplementary Notes

Supplement 1: When using the make px4_sitl gz_x500 command to start the simulation, the automatically launched gz sim simulator world is the default world. Its sdf file is no longer in ~/.gz/worlds, but in ./PX4-Autopilot/Tools/simulation/gz/worlds. Similarly, all models are also located under Tools/simulation. Therefore, when building your simulation environment, you should go to these directories to directly modify the sdf files.

Supplement 2: When writing C++/Python code for Offboard mode control, pay attention to coordinate system transformations. In PX4, the coordinate system is NED (North-East-Down), meaning the x-axis points north, the y-axis points east, and the z-axis points down (all altitudes are negative). In Gazebo Ignition (now Gazebo), the coordinate system is ENU (East-North-Up), meaning the x-axis points east, the y-axis points north, and the z-axis points up (altitudes are positive). Make sure to account for coordinate conversions when writing your control logic.

At this point, the basic workflow for ROS2+PX4 single-drone Offboard mode simulation is fully covered. If you have successfully run through the above workflow, you can now proceed with further single-drone simulation development.

Multi-Drone (Formation) Development Workflow

In the previous section, we completed the single-drone simulation environment setup using ROS2+PX4. Now we proceed to the multi-drone environment setup in preparation for formation control logic simulation.

Reviewing Single-Drone Startup

Let's first review the primary command for starting a single-drone simulation:

cd ~/PX4-Autopilot
make px4_sitl gz_x500

This make command actually performs three tasks:

Compiles the PX4 source code
Starts PX4
Launches the Gazebo simulation

However, for multi-drone simulation, we cannot simply run make repeatedly -- that would cause each x500 drone to launch its own separate Gazebo instance, which is not what we want. Therefore, for multi-drone simulation in the Gazebo environment, our goals are:

Start Gazebo simulation separately
Launch PX4 instances individually, adding drones to the Gazebo environment
Establish communication

Starting Gazebo Separately

First, we need to download the Gazebo launch script (simulation-gazebo):

wget https://raw.githubusercontent.com/PX4/PX4-gazebo-models/main/simulation-gazebo

Then, in the directory where the script is located, run:

python3 simulation-gazebo

Troubleshooting: If the Gazebo window fails to open (crash or black screen), it's usually due to incompatible GPU drivers or a VM without GPU passthrough. Prefix the command with LIBGL_ALWAYS_SOFTWARE=1 to force CPU software rendering:
LIBGL_ALWAYS_SOFTWARE=1 python3 simulation-gazebo
This environment variable tells Mesa/OpenGL to use the LLVMpipe software rasterizer instead of hardware GPU acceleration. The tradeoff is lower frame rate, but simulation functionality is unaffected.

The first time you run the script, it will download some components into .simulation-gazebo (use Ctrl+H in your home directory to reveal hidden folders). The /worlds/default.sdf file inside is the simulation world sdf file -- you will modify this file later.

Supplement 3: Note the distinction from Supplement 1 in the single-drone simulation section. This is a significant difference between single-drone and multi-drone setup. When starting a single-drone simulation, we use the make command, and the world sdf file is in ./PX4-Autopilot/Tools/simulation/gz/worlds. However, if you examine the simulation-gazebo.py script we downloaded for multi-drone simulation, you will find that the world sdf file is under .simulation-gazebo. Make sure to distinguish between these two.

Adding Multiple Drones

After starting the Gazebo environment separately, we add drones to it. This time we do not use the make command, but the following command instead:

cd ~/PX4-Autopilot
PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART=4001 PX4_GZ_MODEL_POSE="0,2" PX4_SIM_MODEL=gz_x500 ./build/px4_sitl_default/bin/px4 -i 0

Parameter explanation:

| Parameter | Description | |-----------|-------------| | PX4_GZ_STANDALONE=1 | Use standalone mode (start only PX4 without launching its own simulation, connect to the separately started simulation) | | PX4_SYS_AUTOSTART=4001 | Required airframe configuration field | | PX4_GZ_MODEL_POSE="0,2" | Initial drone position -- different drones should have different positions | | PX4_SIM_MODEL=gz_x500 | Drone model is x500 | | -i 0 | Drone instance ID (ID 0) |

After running this command, you should see a drone appear in the previously launched Gazebo window.

Now add a second drone (ID -i 1, position "0,1"):

PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART=4001 PX4_GZ_MODEL_POSE="0,1" PX4_SIM_MODEL=gz_x500 ./build/px4_sitl_default/bin/px4 -i 1

To add more drones, follow the same pattern, or write a unified script to add them all at once.

Now you should see two drones side by side in Gazebo. In each drone's PX4 terminal, you can enter:

commander takeoff/land

to observe each drone's takeoff and landing. Finally, add the communication:

MicroXRCEAgent udp4 -p 8888

At this point, the entire environment setup for ROS2+PX4 single-drone and multi-drone simulation in Gazebo is complete. In the next article, we build on this environment to implement ROS2+PX4 Multi-Drone Offboard Control, achieving dual-drone formation flight using a leader-follower algorithm. Beyond that, you can also pursue target recognition, path planning, and many other projects. The first step is always the hardest -- I hope this article can contribute even a small amount to research in this direction.

If you still encounter simulation issues, please refer to the official PX4 documentation and related GitHub projects. Also, if you're interested in Betaflight, check out the Betaflight + Gazebo SITL Tutorial for an alternative SITL simulation approach. Environment configuration and workflow familiarization is a simple but tedious process. As a beginner, I have provided my personal development workflow, and there are certainly better approaches out there. Corrections and suggestions for better methods are always welcome -- they will help those who come after us.

Gazebo Environment Troubleshooting Guide

Based on bugs reported over the past year, here is a summary of troubleshooting and repair methods for dependency conflicts caused by multiple coexisting Gazebo versions.

1. Check Which Gazebo Versions Are Installed

# List all Gazebo-related packages
dpkg -l | grep -E "^ii" | awk '{print $2}' | grep -E "^gz-|^libgz-|^sdformat|^libsdformat|python3-gz" | sort

# Check available Gazebo Sim versions (multiple versions = conflict)
gz sim --versions

# Check the current default version
gz sim --version

Healthy state: gz sim --versions should output only one version number (e.g., 8.9.0), and all libgz-*-X should have the same major version (e.g., all 7 or all 8).

2. Typical Symptoms of Version Conflicts

# Symptom 1: Multiple versions coexist
gz sim --versions
# Output:
# 8.9.0
# 7.9.0        ← Garden and Harmonic coexist -- conflict!

# Symptom 2: CMake finds the wrong version
grep "gz-sim_DIR" ~/PX4-Autopilot/build/px4_sitl_default/CMakeCache.txt
# Should match gz sim --version: Harmonic → gz-sim8, Garden → gz-sim7

# Symptom 3: ROS2 bridge package version mismatch
dpkg -l | grep ros-humble-ros-gz
# Should only have gzharmonic or only gzgarden, not both simultaneously

3. Fix: Unify to a Single Gazebo Version

Using Harmonic (v8) as an example (recommended for ROS2 Humble, as it has a corresponding bridge package):

# 1. Remove all Garden (v7) packages
sudo apt-get purge -y gz-garden gz-sim7-cli gz-launch6-cli gz-transport12-cli \
  libgz-sim7 libgz-sim7-dev libgz-sim7-plugins \
  libgz-launch6 libgz-launch6-dev \
  libgz-transport12 libgz-transport12-dev \
  libgz-fuel-tools8 libgz-fuel-tools8-dev \
  libgz-gui7 libgz-gui7-dev \
  libgz-msgs9 libgz-msgs9-dev \
  libgz-physics6 libgz-physics6-dev \
  libgz-rendering7 libgz-rendering7-dev \
  libgz-sensors7 libgz-sensors7-dev \
  libsdformat13 libsdformat13-dev sdformat13-sdf \
  python3-gz-sim7

# 2. Remove Garden ROS2 bridge
sudo apt-get purge -y ros-humble-ros-gzgarden*

# 3. Install Harmonic ROS2 bridge
sudo apt-get install -y ros-humble-ros-gzharmonic

# 4. Verify
gz sim --versions  # Should only show 8.9.0
dpkg -l | grep gz-garden    # Should output nothing
dpkg -l | grep gz-harmonic  # Should show as installed

4. Common `.bashrc` Issues

# Problem 1: Duplicate source entries (repeated appending inflates PATH)
grep "source.*setup.bash" ~/.bashrc | sort | uniq -c
# If any line appears more than once, manually clean up ~/.bashrc

# Problem 2: Incorrect ROS2 loading order
# Correct order: source /opt/ros/humble/setup.bash first, then source ~/ros2_ws/install/setup.bash
# The workspace overlay must come after the base environment

# Problem 3: Model path variable
# Gazebo Garden/Harmonic use GZ_SIM_RESOURCE_PATH, not GAZEBO_MODEL_PATH
echo $GZ_SIM_RESOURCE_PATH  # Should point to your model directory

5. Check if PX Build Target Matches Gazebo Version

# PX4 CMake cache records the gz-sim version
grep "gz-sim_DIR" ~/PX4-Autopilot/build/px4_sitl_default/CMakeCache.txt
# gz-sim8 → Harmonic
# gz-sim7 → Garden

# If the version does not match the installed Gazebo, recompile PX4:
cd ~/PX4-Autopilot
make clean
make px4_sitl gz_x500

6. Check ROS2 Bridge Package Dependencies

# Confirm the bridge package's SDFormat dependency matches the Gazebo version
apt-cache depends ros-humble-ros-gzharmonic | grep sdformat
# Harmonic → libsdformat14-dev

# Confirm no residual Garden bridge packages
dpkg -l ros-humble-ros-gzgarden* 2>/dev/null
# Should show "no packages found" or status "un" (uninstalled)

References

ROS2+PX4 仿真环境开发教程

Thu, 09 Jul 2026 00:00:00 GMT

简介

PX4 作为目前全世界广泛流行的开源飞控，在无人机设计和控制方面为开发人员带来便利。ROS 作为机器人操作系统，其提供的分布式通信架构简化了无人机编队仿真流程，通过 Gazebo 物理仿真引擎，可以提供真实的仿真环境。

ROS 目前已经更新到了 ROS2，相较于 ROS1，其应用性和可维护性会更加强大。目前已有的 ROS2+PX4 的仿真案例并不多，且伴随二者的不断更新，有很多环境配置的问题。笔者在这里提供一种个人的开发流程。

Ubuntu 系统的安装

本环境基于 Ubuntu 22.04 系统进行搭建。大家可以安装虚拟机或者在 Windows 系统中安装双系统。在这里，笔者给出一个在 Windows11 下安装 Ubuntu 22.04 双系统的视频教程链接：

手把手教你安装双系统 Windows11+Ubuntu 22.04

现在开始进行环境开发流程，这里给出一个博主的环境开发流程，与我们的环境搭建较为相似，简洁明了。如果只有单机仿真需求，也可以参考这篇资料进行环境搭建：

Ubuntu 搭建 PX4 无人机仿真环境(5) -- 仿真环境搭建(以 Ubuntu 22.04, ROS2 Humble 为例)

注意：在开始下列步骤之前，建议首先保证能够科学上网，且网速流畅，否则后续从 Github 克隆仓库时会中途闪退。

PX4 源码下载及编译

首先从 Github 上克隆 PX4 源码，并且更新子模块：

git clone https://github.com/PX4/PX4-Autopilot.git --recursive

由于后续环境配置会装很多东西，有可能难以完全清干净，建议在这一步完成后运行以下命令，备份 ~/PX4-Autopilot，以免后续环境配错需要重新克隆代码：

zip -r PX4-Autopilot.zip PX4-Autopilot/

接着安装相关依赖：

bash ./PX4-Autopilot/Tools/setup/ubuntu.sh

此时 PX4 的源码已经下载好并且安装好相关依赖了，我们可以试着运行以下命令进行编译及启动仿真：

cd ~/PX4-Autopilot/
make px4_sitl         # 先编译 SITL 固件（必须！）
make px4_sitl jmavsim # 编译完成后启动 jmavsim 仿真

常见错误：如果直接运行 make px4_sitl jmavsim 出现 ninja: error: unknown target 'jmavsim'，说明 PX4 的 CMake/Ninja 构建系统尚未生成 jmavsim 目标。必须先执行 make px4_sitl 完成首次编译，再运行 make px4_sitl jmavsim 启动仿真，之后便可直接使用后者。

我们会发现，开始编译 PX4 的源码，并且开始启动 jmavsim 物理仿真引擎，这是因为在我们安装依赖时就已经为我们装好了 jmavsim 和 Gazebo-classic。如果发现终端中出现了绿色的 ready for takeoff，且 jmavsim 出现，则说明编译成功。如果这一步失败，请检查自己的科学上网工具。

现在我们在终端中回车，在 pxh> 后面输入 PX4 的指令：

commander takeoff/land/shutdown

则可以控制无人机的起飞、降落以及关闭仿真。此时可以在 jmavsim 的 UI 中看到无人机的起降。到此步，PX4 的源代码下载和编译全部完成。

ROS2 及其相关依赖的安装

我们使用 ROS2 Humble 进行开发，ROS2 及其依赖的安装和环境配置有些繁琐，在此我强烈建议各位使用"鱼香 ROS"的一键安装功能，在这里感谢其为 ROS 开发提供的便利。直接在 Ubuntu 的系统终端输入以下命令并按终端返回的指令安装即可：

wget http://fishros.com/install -O fishros && . fishros

其一键安装工具中还有 Docker 等工具的一键安装选项，大家可以按需使用，再次感谢。另外，使用 ROS2 Foxy 版本进行开发的朋友也可以使用该工具进行安装和环境配置。

Gazebo Sim 8.9.0 的安装

根据 PX4 的官方文档，之前的 Gazebo Ignition 更名为 Gazebo，以前的 Gazebo 现在叫 Gazebo-classic。Ubuntu 22.04 支持 Gazebo (Ignition) 系列。在之前安装 PX4 依赖时，ubuntu.sh 会自动安装 jmavsim 和 Gazebo-classic，而我们多机仿真需要使用的是 Gazebo Harmonic（v8.x，即 Gazebo Sim 8.9.0）。

重要：Gazebo 的版本与 ROS2 桥接包必须一致，混装多个版本是环境崩溃的首要原因。Gazebo Garden (v7)、Harmonic (v8)、Ionic (v9) 不能共存。

首先添加 OSRF 源并安装 Gazebo Harmonic：

cd ~/PX4-Autopilot
sudo apt install wget
sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
sudo apt update
sudo apt-get install gz-harmonic

然后安装对应版本的 Gazebo-ROS2 通信桥（Harmonic 对应 gzharmonic）：

sudo apt install ros-humble-ros-gzharmonic

安装完成后验证版本唯一性（只应出现一个版本号）：

gz sim --versions   # 应只显示 8.9.0

常见错误：不要安装 gz-garden 或 ros-humble-ros-gzgarden，那是 Gazebo Garden (v7)，与 PX4 当前的 gz-sim8 编译目标不兼容。

MicroXRCE-DDS Agent 的安装

根据官方文档说明，在 ROS2 中 PX4 使用 uXRCE-DDS 中间件来允许在配套计算机上发布和订阅 uORB 消息，而不再使用 MAVROS，我们按照官方案例进行下载、编译及安装。

下载源码：

git clone -b v2.4.2 https://github.com/eProsima/Micro-XRCE-DDS-Agent.git

编译：

cd Micro-XRCE-DDS-Agent
mkdir build
cd build
cmake ..
make

这里应该会有个报错，大家去 build/fastdds/tmp/fastdds-gitclone.cmake 里把 2.12.x 改为 v2.12.1 后重新 make 一下就可以了。

编译的过程会较长，需要进行一定等待，编译结束后进行安装：

sudo make install
sudo ldconfig /usr/local/lib/

QGC 地面站的安装

在后续启动 PX4+Gazebo 仿真时，如果丢失地面站的连接，会导致无法启动 PX4 飞控对无人机进行控制，因此需要在系统中安装 QGC 地面站。安装的方法我给出一个 CSDN 博主的链接，按照上面的步骤操作即可：

(最新) Ubuntu 搭建 PX4 无人机仿真环境(3) -- Ubuntu 安装 QGC 地面站

单机 Offboard 测试流程及开发简介

现在所有的开发环境已经配置好了，我们现在进行 PX4 的 Offboard 模式的验证。

Offboard 模式是 PX4 中的一种特殊飞行模式，允许外部系统（如机载计算机、ROS2 节点等）通过 MAVLink 或 uXRCE-DDS 直接发送控制指令，实时控制无人机的姿态、位置或速度，从而实现自主飞行或高级任务。

首先创建 ROS2 的工作空间：

mkdir -p ~/ros2_ws/src

随后下载源码（注意科学上网）：

cd ~/ros2_ws/src
git clone https://github.com/PX4/px4_msgs.git -b release/1.14
git clone https://github.com/PX4/px4_ros_com.git -b release/v1.14

进行编译：

cd ~/ros2_ws
colcon build

更新环境并配置环境变量：

echo "source ~/ros2_ws/install/setup.bash" >> ~/.bashrc
echo "export GZ_SIM_RESOURCE_PATH=\$HOME/.simulation-gazebo/models:\$GZ_SIM_RESOURCE_PATH" >> ~/.bashrc
source ~/.bashrc

启动 Offboard 仿真

现在我们启动 Offboard 仿真。首先打开已经装好的 QGC 地面站。

终端 1 —— 打开通信：

MicroXRCEAgent udp4 -p 8888

终端 2 —— 启动仿真：

cd ~/PX4-Autopilot
make px4_sitl gz_x500

此时地面站会与 PX4 进行连接，Gazebo 启动，出现了 x500 的无人机。当 PX4 终端出现绿色的 ready for takeoff 时，说明仿真启动成功。有关这个使用 make 命令启动仿真的环节，我们在后续进行多无人机仿真时会详细解释。

终端 3 —— 运行 Offboard 模式源码：

ros2 run px4_ros_com offboard_control

我们可以发现 Gazebo 中无人机上升 5 米，Offboard 模式控制成功。

Offboard 模式开发流程

我们在刚刚创建并下载好源码的 ~/ros2_ws 里找到 px4_ros_com 目录下的 CMakeLists.txt 文件，找到这三行：

add_executable(offboard_control src/examples/offboard/offboard_control.cpp)
ament_target_dependencies(offboard_control rclcpp px4_msgs)
install(TARGETS offboard_control DESTINATION lib/${PROJECT_NAME})

这是将 src/examples/offboard 中的 offboard_control.cpp 文件编译成可执行文件并命名为 offboard_control，同时依赖来源于 px4_msgs，我们可以打开 src/examples/offboard 目录便可以找到 offboard_control.cpp 源代码。

因此如果进行单机使用 Offboard 模式进行仿真，便可以按照以下流程开发：

在已经创建并且配置好环境的 ~/ros2_ws 这个 ROS2 工作空间下再创建一个包（和 px4_ros_com 目录并行）
在新创建的包中，在 /src 目录下编写基于 Offboard 模式进行控制的 C++/Python 代码
在新创建的包中的 CMakeLists.txt 中找到以下几行：

add_executable(offboard_control src/examples/offboard/offboard_control.cpp)
ament_target_dependencies(offboard_control rclcpp px4_msgs)
install(TARGETS offboard_control DESTINATION lib/${PROJECT_NAME})

将路径改为 C++/Python 代码的地址，并给他命名一个可执行文件的名字。在进行仿真前，一定要重新编译 ROS2 工作空间！

cd ~/ros2_ws
colcon build

最后，仿照之前的方法打开 QGC 地面站、通信以及启动仿真，随后再打开一个新的终端：

ros2 run <包名称> <可执行文件名称>

如果在 Gazebo 中看见无人机按照代码逻辑进行飞行，则仿真成功。

补充说明

补充 1：使用 make px4_sitl gz_x500 命令启动仿真时，自动启动的 gz sim 仿真器的世界为 default，其 sdf 文件不再在 ~/.gz/worlds 里，而是在 ./PX4-Autopilot/Tools/simulation/gz/worlds 中。同理，所有的模型也均在 Tools/simulation 里，因此，在搭建仿真环境时，应该去到这些文件夹中直接修改 sdf 文件。

补充 2：在编写 C++/Python 代码进行 Offboard 模式控制时，需要注意坐标系的变换。在 PX4 中，坐标系为 NED 坐标系，即 North-East-Down，因此 x 坐标指向北，y 坐标指向东，z 坐标指向下（所有的高度为负值）；而在 Gazebo Ignition 中，坐标系为 ENU 坐标系，此时 x 坐标指向东，y 坐标指向北，z 坐标指向上（高度为正值）。整理控制逻辑时需要注意坐标的换算。

至此，ROS2+PX4 在 Offboard 模式下进行单机仿真的基本流程就全部介绍完了。大家如果成功地跑通以上流程，接下来就可以基于此进行更多的单机仿真开发了。

多机（编队）的开发流程

在前面的部分，我们完成了基于 ROS2+PX4 的单机仿真环境开发。现在我们开始进行多机的环境开发，为后续编队的控制逻辑仿真进行预先准备。

回顾单机启动

我们先回顾一下启动单机仿真最主要的指令：

cd ~/PX4-Autopilot
make px4_sitl gz_x500

这个 make 指令其实一共完成了三件事：

编译 PX4 源代码
启动 PX4
启动 Gazebo 仿真

但在多机仿真中，我们不需要反复地使用 make 指令，这样会导致每一架 x500 无人机单独启动了一个 Gazebo 仿真，而这不是我们想要的。因此在 Gazebo 环境中启动多机仿真，我们的目标为以下几个：

单独启动 Gazebo 仿真
分别启动 PX4，添加无人机进入 Gazebo 环境
建立通信

单独启动 Gazebo

首先我们需要下载一个 Gazebo 的启动脚本（simulation-gazebo）：

wget https://raw.githubusercontent.com/PX4/PX4-gazebo-models/main/simulation-gazebo

随后在脚本所在的目录终端运行以下指令：

python3 simulation-gazebo

常见问题：如果 Gazebo 窗口无法打开（闪退或黑屏），通常是因为显卡驱动不兼容或虚拟机缺少 GPU 直通。可以在命令前加上 LIBGL_ALWAYS_SOFTWARE=1，强制使用 CPU 软件渲染：
LIBGL_ALWAYS_SOFTWARE=1 python3 simulation-gazebo
这个环境变量告诉 Mesa/OpenGL 使用 LLVMpipe 软件光栅化器代替硬件 GPU 加速。缺点是帧率会有所下降，但不影响仿真功能。

首次运行脚本后，会下载一些组件放在 .simulation-gazebo 下（在主目录里用 Ctrl+H 打开隐藏文件夹），其中的 /worlds/default.sdf 即为仿真的世界 sdf 文件，后续在这个文件里进行修改。

补充 3：此处需与单机仿真部分的补充 1 进行区别。这是单机和多机一个较大的不同。启动单机仿真时我们使用的是 make 指令，世界的 sdf 文件在 ./PX4-Autopilot/Tools/simulation/gz/worlds 中，但如果我们仔细看多机仿真时我们下载好的 simulation-gazebo.py 代码，会发现世界的 sdf 文件在 .simulation-gazebo 下，需要进行区别。

添加多架无人机

在单独启动了 Gazebo 环境后，我们往其中添加无人机，此时我们不再使用 make 命令，而是用以下指令：

cd ~/PX4-Autopilot
PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART=4001 PX4_GZ_MODEL_POSE="0,2" PX4_SIM_MODEL=gz_x500 ./build/px4_sitl_default/bin/px4 -i 0

参数说明：

| 参数 | 含义 | |------|------| | PX4_GZ_STANDALONE=1 | 使用 standalone 模式（不启动仿真，只启动 PX4，等待单独启动的仿真） | | PX4_SYS_AUTOSTART=4001 | 必须字段 | | PX4_GZ_MODEL_POSE="0,2" | 无人机初始位置，不同飞机位置应不同 | | PX4_SIM_MODEL=gz_x500 | 飞机模型为 x500 | | -i 0 | 无人机编号（第 0 号） |

运行该指令后，我们应该可以在之前单独打开的 Gazebo 里看见出现了一架飞机。

现在我们再添加第二架飞机（编号为 -i 1，位置为 "0,1"）：

PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART=4001 PX4_GZ_MODEL_POSE="0,1" PX4_SIM_MODEL=gz_x500 ./build/px4_sitl_default/bin/px4 -i 1

如果要添加更多的无人机，按此方法添加即可，也可以编写统一的脚本进行添加。

现在我们会发现在 Gazebo 里出现了两架并排的无人机，可以分别在他们启动 PX4 后的终端输入：

commander takeoff/land

可以分别观察他们的起降情况。最后添加通信：

MicroXRCEAgent udp4 -p 8888

至此，基于 ROS2+PX4 在 Gazebo 里进行无人机单机、多机仿真的环境开发就全部结束了。在下一篇文章中，我们将基于此环境实现 ROS2+PX4 多机 Offboard 编队控制，使用 leader-follower 算法进行双无人机编队飞行。除此之外，还可以基于此完成目标识别、路径规划等更多项目。万事开头难，希望本文能够给此方向的研究贡献一点微薄之力。

如果仿真仍有问题，大家可以参考 PX4 官方文档以及 Github 上相关项目资料。另外，如果你对 Betaflight 飞控感兴趣，也可以参考 Betaflight + Gazebo 软件在环仿真教程，了解另一种 SITL 仿真方案。环境的配置以及开发流程的熟悉是简单但是繁琐的过程，笔者作为初学者，提供一种自己的开发流程，同时也肯定会有其他更好的方式。如有错漏，欢迎指出，为后来者提供一个教训；如有更佳方式，也希望大家分享。

Gazebo 环境排错指南

根据一年多来大家的 bug，这里汇总 Gazebo 多版本共存导致依赖冲突的排查和修复方法。

1. 检查当前安装了哪些 Gazebo 版本

# 查看所有 Gazebo 相关包
dpkg -l | grep -E "^ii" | awk '{print $2}' | grep -E "^gz-|^libgz-|^sdformat|^libsdformat|python3-gz" | sort

# 查看可用的 Gazebo Sim 版本（多版本意味着冲突）
gz sim --versions

# 查看当前默认版本
gz sim --version

正常状态：gz sim --versions 只输出一个版本号（如 8.9.0），所有 libgz-*-X 的 X 都指向同一代（如全是 7 或全是 8）。

2. 版本冲突的典型表现

# 症状 1：多个版本共存
gz sim --versions
# 输出：
# 8.9.0
# 7.9.0        ← 说明 Garden 和 Harmonic 共存，冲突！

# 症状 2：CMake 找到错误版本
grep "gz-sim_DIR" ~/PX4-Autopilot/build/px4_sitl_default/CMakeCache.txt
# 应与 gz sim --version 对应：Harmonic → gz-sim8，Garden → gz-sim7

# 症状 3：ROS2 桥接包版本不匹配
dpkg -l | grep ros-humble-ros-gz
# 应只有 gzharmonic 或只有 gzgarden，不能同时存在

3. 修复：统一到单一 Gazebo 版本

以 Harmonic (v8) 为例（ROS2 Humble 推荐，因为有对应桥接包）：

# 1. 移除 Garden (v7) 所有包
sudo apt-get purge -y gz-garden gz-sim7-cli gz-launch6-cli gz-transport12-cli \
  libgz-sim7 libgz-sim7-dev libgz-sim7-plugins \
  libgz-launch6 libgz-launch6-dev \
  libgz-transport12 libgz-transport12-dev \
  libgz-fuel-tools8 libgz-fuel-tools8-dev \
  libgz-gui7 libgz-gui7-dev \
  libgz-msgs9 libgz-msgs9-dev \
  libgz-physics6 libgz-physics6-dev \
  libgz-rendering7 libgz-rendering7-dev \
  libgz-sensors7 libgz-sensors7-dev \
  libsdformat13 libsdformat13-dev sdformat13-sdf \
  python3-gz-sim7

# 2. 移除 Garden 的 ROS2 桥接
sudo apt-get purge -y ros-humble-ros-gzgarden*

# 3. 安装 Harmonic 的 ROS2 桥接
sudo apt-get install -y ros-humble-ros-gzharmonic

# 4. 验证
gz sim --versions  # 应只显示 8.9.0
dpkg -l | grep gz-garden    # 应无输出
dpkg -l | grep gz-harmonic  # 应显示已安装

4. `.bashrc` 常见问题

# 问题 1：source 重复（多次追加导致 PATH 越来越长）
grep "source.*setup.bash" ~/.bashrc | sort | uniq -c
# 如果某行出现超过 1 次，需要手动清理 ~/.bashrc

# 问题 2：ROS2 加载顺序错误
# 正确顺序：先 source /opt/ros/humble/setup.bash，再 source ~/ros2_ws/install/setup.bash
# 工作空间必须放在基础环境之后

# 问题 3：模型路径变量
# Gazebo Garden/Harmonic 使用 GZ_SIM_RESOURCE_PATH，不是 GAZEBO_MODEL_PATH
echo $GZ_SIM_RESOURCE_PATH  # 应指向模型目录

5. 检查 PX4 编译目标与 Gazebo 版本是否一致

# PX4 CMake 缓存记录的 gz-sim 版本
grep "gz-sim_DIR" ~/PX4-Autopilot/build/px4_sitl_default/CMakeCache.txt
# gz-sim8 → Harmonic
# gz-sim7 → Garden

# 如果版本与安装的 Gazebo 不一致，需要重新编译 PX4：
cd ~/PX4-Autopilot
make clean
make px4_sitl gz_x500

6. 检查 ROS2 桥接包依赖

# 确认桥接包的 SDFormat 依赖与 Gazebo 版本匹配
apt-cache depends ros-humble-ros-gzharmonic | grep sdformat
# Harmonic → libsdformat14-dev

# 确认无残留的 Garden 桥接
dpkg -l ros-humble-ros-gzgarden* 2>/dev/null
# 应显示 "没有找到" 或状态为 "un" (已卸载)