PX4 Autopilot Guide 2026

from pymavlink import mavutil
import time

# Connect to SITL (UDP) or real FC (serial)
master = mavutil.mavlink_connection('udpin:0.0.0.0:14540')

# Wait for heartbeat
master.wait_heartbeat()
print(f"Connected: system {master.target_system}")

# Arm the vehicle
master.arducopter_arm()
master.motors_armed_wait()
print("Armed!")

# Command takeoff to 5m
master.mav.command_long_send(
    master.target_system,
    master.target_component,
    mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
    0, 0, 0, 0, 0, 0, 0, 5.0  # altitude = 5m
)
time.sleep(5)

# Fly to waypoint (lat, lon, alt)
master.mav.set_position_target_global_int_send(
    0, master.target_system, master.target_component,
    mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
    0b0000111111111000,  # position only
    int(47.397742 * 1e7), int(8.545594 * 1e7), 10,
    0, 0, 0, 0, 0, 0, 0, 0
)

# Install MAVROS for ROS Humble (for ROS 1 projects only)
sudo apt install ros-humble-mavros ros-humble-mavros-extras -y
sudo /opt/ros/humble/lib/mavros/install_geographiclib_datasets.sh

# Launch MAVROS connected to SITL
ros2 launch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

# Check connection
ros2 topic echo /mavros/state

# Arm and takeoff via MAVROS service
ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: true}"
ros2 service call /mavros/cmd/takeoff mavros_msgs/srv/CommandTOL "{altitude: 5.0}"

# Install Micro XRCE-DDS Agent
pip install -U micro-xrce-dds-agent

# Or build from source (for ROS 2 Jazzy)
git clone https://github.com/eProsima/Micro-XRCE-DDS-Agent
cd Micro-XRCE-DDS-Agent
mkdir build && cd build
cmake .. -DUXRCE_BUILD_SHARED_LIBS=ON
make -j$(nproc)
sudo make install

# Start the agent (UDP for SITL, serial /dev/ttyS0 for hardware)
MicroXRCEAgent udp4 -p 8888

# In another terminal, start PX4 SITL
make px4_sitl gz_x500

# PX4 XRCE client auto-connects to agent.
# In ROS 2:
ros2 topic list | grep /fmu

import rclpy
from rclpy.node import Node
from px4_msgs.msg import (
    OffboardControlMode, TrajectorySetpoint,
    VehicleCommand, VehicleStatus
)
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy

class OffboardControl(Node):
    def __init__(self):
        super().__init__('offboard_control')

        qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            history=HistoryPolicy.KEEP_LAST,
            depth=1
        )

        self.offboard_pub = self.create_publisher(OffboardControlMode, '/fmu/in/offboard_control_mode', qos)
        self.traj_pub = self.create_publisher(TrajectorySetpoint, '/fmu/in/trajectory_setpoint', qos)
        self.cmd_pub = self.create_publisher(VehicleCommand, '/fmu/in/vehicle_command', qos)

        self.timer = self.create_timer(0.1, self.timer_callback)  # 10Hz
        self.counter = 0

    def arm(self):
        cmd = VehicleCommand()
        cmd.command = VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM
        cmd.param1 = 1.0  # arm
        cmd.from_external = True
        self.cmd_pub.publish(cmd)

    def engage_offboard(self):
        cmd = VehicleCommand()
        cmd.command = VehicleCommand.VEHICLE_CMD_DO_SET_MODE
        cmd.param1 = 1.0  # custom mode
        cmd.param2 = 6.0  # offboard
        self.cmd_pub.publish(cmd)

    def timer_callback(self):
        # Must publish offboard mode setpoints at >2Hz before switching to offboard
        mode = OffboardControlMode()
        mode.position = True
        self.offboard_pub.publish(mode)

        setpoint = TrajectorySetpoint()
        setpoint.position = [0.0, 0.0, -5.0]  # NED frame: -Z = up 5m
        setpoint.yaw = 0.0
        self.traj_pub.publish(setpoint)

        if self.counter == 10:  # After 1s of setpoints, arm and switch
            self.arm()
            self.engage_offboard()
        self.counter += 1

def main():
    rclpy.init()
    node = OffboardControl()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == "__main__":
    main()