MAVLink GUIDED Code

MAVLink GUIDED 波浪高度飞行任务

在前进过程中用正弦函数生成目标高度，演示水平速度与垂直速度的组合控制。

这些脚本用于学习和验证 ArduPilot/PX4 平台上的 pymavlink GUIDED 控制。实机测试前，请先在 SITL 或空旷安全环境验证，确认螺旋桨、GPS、EKF、遥控器接管、降落模式和急停流程都可用。

任务目标

起飞到 5 米高度，向前低速飞行，同时让高度在 4 到 6 米之间按正弦曲线变化，最后降落。

连接方式： 默认使用 udp:127.0.0.1:14550，适合 SITL 或本机 MAVLink 转发。实机使用时需要按自己的飞控串口或网络端口修改连接地址。

学习重点

使用 math.sin 生成目标高度
NED 坐标中 vz 为负表示上升、为正表示下降
适合测试高度控制、缓慢轨迹和传感器稳定性

脚本结构

函数	作用
`connect()`	脚本流程中的控制步骤
`set_mode()`	脚本流程中的控制步骤
`wait_for_gps()`	脚本流程中的控制步骤
`get_altitude()`	脚本流程中的控制步骤
`wait_for_position_estimate()`	脚本流程中的控制步骤
`arm()`	脚本流程中的控制步骤
`takeoff()`	脚本流程中的控制步骤
`send_velocity()`	脚本流程中的控制步骤
`stop()`	脚本流程中的控制步骤
`fly_wavy()`	脚本流程中的控制步骤
`land()`	脚本流程中的控制步骤
`main()`	核心任务函数

运行前检查

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

完整代码：move_wavy.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=5.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):
    master.mav.set_position_target_local_ned_send(
        0,
        master.target_system,
        master.target_component,
        mavutil.mavlink.MAV_FRAME_BODY_NED,
        0b0000111111000111,
        0, 0, 0,
        vx, vy, vz,
        0, 0, 0,
        0, 0
    )

def stop(master):
    print(" Stopping")
    for _ in range(20):
        send_velocity(master, 0, 0, 0)
        time.sleep(0.1)

def fly_wavy(master, center_alt=5.0, amplitude=1.0, duration=50.0, forward_speed=0.2):
    print(" Flying wavy altitude pattern")
    print(f"   center_alt={center_alt}m amplitude={amplitude}m")
    print(f"   altitude range={center_alt - amplitude}m to {center_alt + amplitude}m")

    wave_period = 10.0
    max_vz = 0.4
    altitude_gain = 0.6

    start = time.time()

    while time.time() - start < duration:
        elapsed = time.time() - start

        desired_alt = center_alt + amplitude * math.sin(2.0 * math.pi * elapsed / wave_period)
        current_alt = get_altitude(master)

        if current_alt is None:
            send_velocity(master, vx=forward_speed, vy=0, vz=0)
            time.sleep(0.1)
            continue

        altitude_error = desired_alt - current_alt

        # In NED frame: negative vz goes up, positive vz goes down.
        vz = -altitude_gain * altitude_error
        vz = max(min(vz, max_vz), -max_vz)

        send_velocity(master, vx=forward_speed, vy=0, vz=vz)

        print(f"alt={current_alt:.2f}m target={desired_alt:.2f}m vz={vz:.2f}")
        time.sleep(0.1)

    stop(master)

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=5.0)
    time.sleep(2)

    fly_wavy(
        master,
        center_alt=5.0,
        amplitude=1.0,
        duration=50.0,
        forward_speed=0.2
    )

    time.sleep(1)

    land(master)

    print(" Done")

if __name__ == "__main__":
    main()

下一步

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

返回 DRF450 MAVLink 章节