px4仿真和示例运行

PX4 测试开发说明（SITL 与实机）

1. 快速使用说明

1.1 环境依赖

python3 -m pip install --upgrade pip
python3 -m pip install mavsdk

1.2 启动仿真（SITL）

在 PX4-Autopilot 目录执行：

make px4_sitl gz_x500

默认情况下，MAVSDK 常用连接地址是：udp://:14540

1.3 运行当前脚本

在本目录执行：

python3 px4_takeoff_forward_circle_land.py \
  --address udp://:14540 \
  --takeoff-alt 5 \
  --forward-speed 2.0 \
  --forward-time 4 \
  --circle-speed 2.0 \
  --circle-yaw-rate 30 \
  --circle-time 10

1.4 脚本实际流程

1. 连接飞控
2. 等待状态满足（默认要求全局定位与 Home 点）
3. Arm 解锁
4. Takeoff 起飞
5. 进入 Offboard
6. 前进
7. 绕圈
8. 降落
9. Disarm 上锁

---

2. 常用函数手册（重点）

下面按"你写测试代码最常用"的顺序来讲。

2.1 连接与状态类

connect()

用途：连接 PX4 飞控。

典型调用：

• await drone.connect(system_address="udp://:14540")

常见地址：

• 仿真：udp://:14540
• 实机 UDP：udp://0.0.0.0:14540
• 实机串口：serial:///dev/ttyUSB0:57600

注释建议：

• 写清"连接链路来源"，比如是 SITL 端口还是数传串口。

core.connection_state()

用途：监听连接状态流，确认是否真的连上。

典型逻辑：

• async for state in drone.core.connection_state():
• if state.is_connected: break

注释建议：

• 写清"这里不是一次性查询，是持续状态流"。

telemetry.health()

用途：读取健康状态，如 GPS / Home 点是否可用。

常用字段：

• is_global_position_ok
• is_home_position_ok

注释建议：

• 写清为什么要等这个状态（例如 Offboard 任务依赖定位）。

telemetry.in_air()

用途：判断飞机是否在空中。

典型场景：

• 起飞后等待 True
• 降落后等待 False

注释建议：

• 写明"用于动作阶段切换，不是精确高度判断"。

---

2.2 Action 高层动作函数

action.arm()

用途：解锁电机。

前置条件：

• 飞控状态允许解锁（传感器健康、模式与安全开关满足）

失败常见原因：

• 未通过 Preflight 检查
• 遥控器/安全开关条件不满足

注释建议：

• 写明"失败时不要继续下发运动指令"。

action.disarm()

用途：上锁电机。

注意：

• 有些机型在降落后会自动上锁，手动 disarm 可能返回异常。

注释建议：

• 写明"异常可能可忽略（已自动上锁）"。

action.set_takeoff_altitude(alt_m)

用途：设置起飞相对高度。

参数：

• alt_m：米

建议：

• 仿真：3~5m 常见
• 首次实机：2~3m 更稳妥

注释建议：

• 写明"相对 Home 点高度，不是绝对海拔"。

action.takeoff()

用途：触发起飞动作。

搭配：

• 之后要等待 in_air == True

注释建议：

• 写明"takeoff 发送后不代表已稳定离地"。

action.land()

用途：触发自动降落。

搭配：

• 之后要等待 in_air == False

注释建议：

• 写明"land 下发后仍需等待落地确认"。

---

2.3 Offboard 实时控制函数

offboard.set_velocity_body(VelocityBodyYawspeed(vx, vy, vz, yaw_rate))

用途：机体系速度控制（最常用）。

参数含义（FRD）：

• vx > 0：向前
• vy > 0：向右
• vz > 0：向下
• yaw_rate > 0：顺时针偏航（从上往下看）

典型用法：

• 前进：(2.0, 0, 0, 0)
• 绕圈：(2.0, 0, 0, 30)
• 悬停：(0, 0, 0, 0)

注释建议：

• 一定写清坐标系和符号方向，避免后续"左右/上下反了"。

offboard.start()

用途：进入 Offboard 模式。

关键前置条件：

• 必须先发送至少一次 setpoint（如 set_velocity_body(0,0,0,0)）

常见错误：

• 未先发 setpoint 导致启动失败

注释建议：

• 写明"先发 setpoint 再 start，是 PX4 Offboard 的硬要求"。

offboard.stop()

用途：退出 Offboard 控制，交回给飞控模式控制。

建议：

• 任务结束先置零速度，再 stop()，更平滑。

注释建议：

• 写明"退出后不要假设 Offboard 设定还在持续生效"。

---

2.4 封装动作函数（测试代码里最推荐）

下面这些是你在脚本里封装好的"常用函数类型"，建议每个测试项目都保留。

wait_until_ready()

用途：把"可飞行条件"集中管理。

建议注释写法：

• 解释是否强依赖 GPS（室内可关闭）。

wait_for_in_air(expected, timeout_s)

用途：阶段同步，防止动作穿插。

建议注释写法：

• 写清 expected=True/False 含义。
• 写清超时后应该如何处理（通常触发紧急降落或中断任务）。

set_body_velocity(vx, vy, vz, yaw_rate_dps)

用途：统一下发机体系速度，作为所有运动动作的底层接口。

建议注释写法：

• 写清单位：m/s, deg/s。

hold(duration_s)

用途：通过 0 速度实现短时悬停。

建议注释写法：

• 写清"是速度闭锁悬停，不是位置锁定到某一点"。

move_forward(speed_mps, duration_s)

用途：前进测试动作。

建议注释写法：

• 写清这是"按时间积分距离"的开环动作，受风和姿态影响。

circle(speed_mps, yaw_rate_dps, duration_s)

用途：绕圈测试动作。

关键关系：

• 近似半径：r ≈ v / w(rad/s)

建议注释写法：

• 写明这是"圆弧近似"，不是精准半径闭环。

emergency_land()

用途：异常时统一回收。

建议逻辑：

1. 先发 0 速度
2. 触发 land()
3. 打印异常来源

建议注释写法：

• 写清"这是最后保护，不替代 RC 接管和硬件急停"。

---

3. 详细注释应该写什么（模板）

很多"注释不够详细"的根本原因是只写"做了什么"，没写"为什么这么做"。

推荐每个关键函数都写 5 类信息：

1. 功能目的
2. 输入参数与单位
3. 前置条件
4. 失败行为/异常策略
5. 坐标系与方向约定（控制函数必须写）

可以直接套这个模板：

async def move_forward(speed_mps: float, duration_s: float) -> None:
    """
    目的：以前向速度飞行一段时间，用于直线段测试。
    参数：
      speed_mps: 前向速度，单位 m/s，>0 表示机头方向。
      duration_s: 持续时间，单位 s。
    前置条件：
      - 已解锁且已离地
      - 已成功进入 Offboard
    失败策略：
      - 下发失败抛异常，上层统一进入 emergency_land()
    注意：
      - 该动作按时间开环控制，实际距离受风和机体姿态影响。
    """

---

4. 从仿真切到实机：转换清单（可直接照做）

4.1 必改项

1. 修改连接地址 --address

• 仿真：udp://:14540
• 实机 UDP：udp://0.0.0.0:14540
• 实机串口：serial:///dev/ttyUSB0:57600

2. 降低初始参数

• --takeoff-alt 2.5
• --forward-speed 1.0
• --forward-time 2
• --circle-speed 1.0
• --circle-yaw-rate 12
• --circle-time 4

3. 根据环境设置定位约束

• 室外 GPS：保持默认（要求全局定位）
• 室内无 GPS：加 --no-require-global-position

4.2 安全检查项（实机必须）

1. 人与场地

• 空旷区域、低空试飞、周围清场
• 旁站人员可随时口令终止

2. 遥控接管

• RC 开机并可切回人工模式
• 急停逻辑与 failsafe 先验证

3. 飞控状态

• EKF 正常
• 罗盘方向正确
• 电池电压、低压保护、地理围栏确认

4. 动作策略

• 先验证单动作：起飞/悬停/降落
• 再加前进
• 最后加绕圈

4.3 推荐实机首飞命令

python3 px4_takeoff_forward_circle_land.py \
  --address udp://0.0.0.0:14540 \
  --takeoff-alt 2.5 \
  --forward-speed 1.0 \
  --forward-time 2 \
  --circle-speed 1.0 \
  --circle-yaw-rate 12 \
  --circle-time 4

---

5. 常见问题（FAQ）

Q1: offboard.start() 失败

优先检查：

1. 是否先发送了至少一次 setpoint
2. 是否已经起飞并状态稳定
3. 链路是否抖动
4. 飞控参数是否限制 Offboard

Q2: 飞机方向和预期相反

优先检查：

1. 你用的是机体系还是 NED
2. 是否把 vz 正方向（向下）理解反了
3. yaw_rate 正负是否按脚本约定

Q3: 实机上可以直接用仿真参数吗

不建议。必须先缩小速度、时长和高度，再逐步放开。

---