4、单机底层控制：PX4/ArduPilot姿态控制接口、mavros节点、offboard模式切换

好，我们进入第四章。这一章很关键，说白了就是打通「大脑」和「手脚」的最后一公里。你写好了编队算法，总得让飞机动起来吧？怎么动？就是通过底层控制接口。

我个人习惯把这一章叫做「驯服飞控」。因为不管你用PX4还是ArduPilot，它们都有自己的脾气。你得学会跟它们对话，用它们听得懂的语言——也就是mavros。

4.1 飞控姿态控制接口：PX4 vs ArduPilot

先说说这两个主流飞控。它们都支持MAVLink协议，但姿态控制的接口细节不太一样。

PX4 这边，它默认的offboard控制模式接受的是姿态/角速率/推力的设定值。你发一个roll、pitch、yaw的角度，再加一个thrust（0~1），它内部自己算混控。

ArduPilot 呢，它更灵活一些。你可以选「guided」模式，直接发位置、速度或者姿态。我个人觉得ArduPilot的接口更「直白」一点，但PX4的响应更「硬核」。

我在项目中遇到过一个问题：用PX4的时候，如果thrust给得太小，飞控会直接切回自稳模式。嗯，这里要注意，PX4对thrust有个最低阈值，低于0.1它就觉得你在「乱发指令」。

4.2 mavros节点：ROS与飞控的桥梁

mavros，说白了就是一个ROS节点。它把MAVLink协议翻译成ROS话题和服务。你不需要懂MAVLink的二进制包结构，只需要发ROS消息就行。

启动mavros的命令很简单：

roslaunch mavros px4.launch fcu_url:=/dev/ttyUSB0:921600

如果是ArduPilot，把px4.launch换成ardupilot.launch就行。

启动之后，你会看到一堆话题。我挑几个最重要的说：

话题名	类型	说明
/mavros/setpoint_raw/attitude	mavros_msgs/AttitudeTarget	发姿态和推力
/mavros/setpoint_raw/local	mavros_msgs/PositionTarget	发位置或速度
/mavros/state	mavros_msgs/State	获取当前飞行模式
/mavros/local_position/pose	geometry_msgs/PoseStamped	获取本地位置

4.3 offboard模式切换

这是最容易出问题的一步。为什么？因为飞控有安全机制。你想想看，如果地面站突然发了个乱码，飞机直接翻跟头怎么办？

所以PX4和ArduPilot都要求：必须先持续发送setpoint，再切换模式。这个「持续」一般要求2秒以上，频率至少10Hz。

我曾经踩过一个坑：代码里先切换模式，再发setpoint。结果飞机直接切回自稳，怎么都进不了offboard。后来才发现，顺序反了。

正确的流程是这样的：

1. 启动mavros，确认连接
2. 开始以10Hz以上频率发送setpoint（哪怕是个悬停指令）
3. 发送模式切换指令（服务调用）
4. 确认模式已切换成功
5. 解锁（arm）
6. 开始执行你的编队任务

代码示例（Python）：

#!/usr/bin/env python3
import rospy
from mavros_msgs.msg import State, AttitudeTarget
from mavros_msgs.srv import SetMode, CommandBool
from geometry_msgs.msg import PoseStamped

# 先发一个悬停setpoint
def send_attitude_setpoint():
    pub = rospy.Publisher('/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=10)
    rate = rospy.Rate(20)  # 20Hz
    msg = AttitudeTarget()
    msg.type_mask = 0b000000  # 使用姿态和推力
    msg.orientation.x = 0.0
    msg.orientation.y = 0.0
    msg.orientation.z = 0.0
    msg.orientation.w = 1.0
    msg.thrust = 0.5

    # 先发2秒，再切换模式
    for i in range(40):  # 2秒 * 20Hz
        pub.publish(msg)
        rate.sleep()

# 切换offboard模式
def set_offboard_mode():
    rospy.wait_for_service('/mavros/set_mode')
    try:
        set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
        result = set_mode(custom_mode='OFFBOARD')
        rospy.loginfo("Offboard mode set: %s", result.mode_sent)
    except rospy.ServiceException as e:
        rospy.logerr("Failed to set mode: %s", e)

# 解锁
def arm_vehicle():
    rospy.wait_for_service('/mavros/cmd/arming')
    try:
        arm = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
        result = arm(value=True)
        rospy.loginfo("Arm result: %s", result.success)
    except rospy.ServiceException as e:
        rospy.logerr("Failed to arm: %s", e)

if __name__ == '__main__':
    rospy.init_node('offboard_switch_demo')
    send_attitude_setpoint()
    set_offboard_mode()
    arm_vehicle()
    rospy.spin()

4.4 实际项目中的避坑指南

嗯，最后聊点实战经验。你可能会遇到这些问题：

• 模式切不过去： 检查一下是不是GPS信号不好？PX4的offboard模式需要3D fix。我建议你在室外等GPS灯变绿再试。
• 飞机乱飘： 大概率是setpoint的频率不够。低于10Hz的话，飞控会认为连接丢失，自动切回自稳。
• ArduPilot的guided模式： 它跟PX4的offboard不太一样。ArduPilot要求先发SET_MODE服务，再发SET_POSITION_TARGET。而且它有个「心跳」机制，setpoint断了超过3秒就退出guided。

我记得有一次做编队演示，三架飞机同时起飞。结果有一架怎么都进不了offboard。排查了半天，发现是USB转串口的线接触不良，mavros断连了。从那以后，我每次都会在代码里加一个连接状态检查：

def wait_for_connection():
    state = rospy.wait_for_message('/mavros/state', State)
    while not state.connected:
        rospy.logwarn("Waiting for FCU connection...")
        rospy.sleep(0.5)
        state = rospy.wait_for_message('/mavros/state', State)
    rospy.loginfo("FCU connected!")

好了，这一章的内容就这些。你只要把mavros跑通，能切offboard，能发姿态指令，单机底层控制这块就算拿下了。下一章我们会讲多机通信，那才是编队的核心。

公众号:蓝海资料掘金营，微信deep3321