任务目标

起飞到 2 米高度，先顺时针飞一圈，再逆时针飞一圈，形成 8 字路径后降落。

学习重点

• 复用圆形飞行函数构建组合轨迹
• 通过 yaw_rate 正负号控制方向
• 适合检验连续任务、转向稳定性和路径对称性

脚本结构

运行前检查

1. 确认安装 pymavlink：pip install pymavlink
2. 确认飞控可以收到 GPS，并且 EKF 位置估计已经稳定。
3. 确认遥控器可以随时切换模式或接管。
4. 先拆桨做连接与解锁流程验证，再进入空旷环境低速测试。

完整代码：guided_eight.py

#!/usr/bin/env python3
import time
import math
from pymavlink import mavutil

def connect():
 print(" Connecting...")
 master = mavutil.mavlink_connection("udp:127.0.0.1:14550")
 master.wait_heartbeat()
 print(f" Connected: system={master.target_system}, component={master.target_component}")
 return master

def set_mode(master, mode):
 print(f" Setting mode: {mode}")
 master.set_mode_apm(mode)
 time.sleep(2)

def wait_for_gps(master):
 print(" Waiting for GPS fix...")
 while True:
 msg = master.recv_match(type="GPS_RAW_INT", blocking=True, timeout=1)
 if msg:
 print(f"GPS fix_type={msg.fix_type}, satellites={msg.satellites_visible}")
 if msg.fix_type >= 3 and msg.satellites_visible >= 8:
 print(" GPS OK")
 return
 time.sleep(1)

def get_altitude(master):
 msg = master.recv_match(type="GLOBAL_POSITION_INT", blocking=True, timeout=1)
 if msg:
 return msg.relative_alt / 1000.0
 return None

def wait_for_position_estimate(master):
 print(" Waiting for EKF position estimate / home...")
 start = time.time()
 while time.time() - start < 60:
 msg = master.recv_match(
 type=["EKF_STATUS_REPORT", "GLOBAL_POSITION_INT", "HOME_POSITION", "STATUSTEXT"],
 blocking=True,
 timeout=1
 )
 if not msg:
 continue

 if msg.get_type() == "GLOBAL_POSITION_INT":
 alt = msg.relative_alt / 1000.0
 print(f" Position estimate active, rel_alt={alt:.2f}m")
 return

 raise Exception(" No position estimate / home not ready")

def arm(master):
 print(" Arming...")

 while master.recv_match(blocking=False):
 pass

 master.mav.command_long_send(
 master.target_system,
 master.target_component,
 mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
 0,
 1, 0, 0, 0, 0, 0, 0
 )

 start = time.time()
 while time.time() - start < 15:
 msg = master.recv_match(type=["HEARTBEAT"], blocking=True, timeout=1)
 if msg and (msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED):
 print(" Armed")
 return

 raise Exception(" Failed to arm automatically")

def takeoff(master, target_alt=2.0):
 print(f" Taking off to {target_alt}m")

 master.mav.command_long_send(
 master.target_system,
 master.target_component,
 mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
 0,
 0, 0, 0, 0, 0, 0,
 target_alt
 )

 while True:
 alt = get_altitude(master)
 if alt is not None and alt >= target_alt * 0.95:
 print(" Target altitude reached")
 break
 time.sleep(0.2)

def send_velocity(master, vx=0, vy=0, vz=0, yaw_rate=0):
 # Velocity + yaw_rate enabled. Position, acceleration, and yaw angle ignored.
 type_mask = 0b0000010111000111

 master.mav.set_position_target_local_ned_send(
 0,
 master.target_system,
 master.target_component,
 mavutil.mavlink.MAV_FRAME_BODY_NED,
 type_mask,
 0, 0, 0,
 vx, vy, vz,
 0, 0, 0,
 0,
 yaw_rate
 )

def stop(master, seconds=2.0):
 print(" Stopping")
 start = time.time()

 while time.time() - start < seconds:
 send_velocity(master, 0, 0, 0, 0)
 time.sleep(0.1)

def fly_circle(master, radius=2.0, speed=0.2, clockwise=True):
 yaw_rate = speed / radius

 if not clockwise:
 yaw_rate = -yaw_rate

 circle_time = (2.0 * math.pi * radius) / speed
 direction = "clockwise" if clockwise else "counter-clockwise"

 print(f" Flying {direction} circle")
 print(f" radius={radius}m speed={speed}m/s yaw_rate={yaw_rate:.3f}rad/s")
 print(f" duration={circle_time:.1f}s")

 start = time.time()

 while time.time() - start < circle_time:
 send_velocity(master, vx=speed, vy=0, vz=0, yaw_rate=yaw_rate)
 time.sleep(0.1)

 stop(master, seconds=1.5)

def fly_figure_eight(master, radius=2.0, speed=0.2):
 print(" Flying figure-eight pattern")

 fly_circle(master, radius=radius, speed=speed, clockwise=True)

 time.sleep(1)

 fly_circle(master, radius=radius, speed=speed, clockwise=False)

 stop(master, seconds=2.0)

def land(master):
 print(" Landing...")
 master.set_mode_apm("LAND")
 master.motors_disarmed_wait()
 print(" Disarmed")

def main():
 master = connect()

 wait_for_gps(master)
 time.sleep(5)

 wait_for_position_estimate(master)

 set_mode(master, "GUIDED")
 time.sleep(2)

 arm(master)

 takeoff(master, target_alt=2.0)
 time.sleep(2)

 fly_figure_eight(
 master,
 radius=2.0,
 speed=0.2
 )

 time.sleep(1)

 land(master)

 print(" Done")

if __name__ == "__main__":
 main()

下一步

先从 10 米直线脚本开始，确认连接、解锁、起飞、速度控制和降落都稳定后，再测试圆形、8 字和波浪高度任务。

返回 DRF450 MAVLink 章节