Python控制Ardupilot和gazebo

跑通Ardupilot和Gazebo

查看主机IP（Python程序所在电脑的IP）
bash 复制代码
```
ipconfig

172.22.46.xx
```

启动gazebo和ardupilot

启动gazebo

bash 复制代码

cd ardupilot_gazebo
gz sim -v4 -r iris_runway.sdf --render-engine ogre

启动ardupilot 将数据通过14550端口转发出去，并可以通过该端口接受命令，双向通信
bash 复制代码
```
cd ardupilot
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --map --console --out=udp:172.22.46.xx:14550
```

运行Python代码

python 复制代码

from pymavlink import mavutil
import time

# 建立连接
master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
master.wait_heartbeat()
print("✅ 已连接飞控！")


def set_target_location(n, e, d):
    """
    发送本地 NED 坐标目标点
    n: 北向位移 (米)
    e: 东向位移 (米)
    d: 高度 (注意：NED系中，高度为负值，-5代表上方5米)
    """
    master.mav.set_position_target_local_ned_send(
        0,  # 时间戳
        master.target_system, master.target_component,
        mavutil.mavlink.MAV_FRAME_LOCAL_NED,  # 使用本地坐标系
        0b0000111111111000,  # 掩码：只关注位置信息 (X, Y, Z)
        n, e, d,  # 目标位置
        0, 0, 0,  # 速度
        0, 0, 0,  # 加速度
        0, 0  # 偏航角
    )


def wait_reach_target(target_n, target_e, threshold=0.3):
    """等待无人机到达目标点误差范围内 + 实时打印姿态数据"""
    while True:
        # --------------------------
        # 【核心新增】实时获取并打印数据
        # --------------------------
        # 1. 获取位置 (NED)
        pos_msg = master.recv_match(type='LOCAL_POSITION_NED', blocking=True)

        # 2. 获取姿态 (角速率 / 欧拉角)
        att_msg = master.recv_match(type='ATTITUDE', blocking=True)

        # 解析数据
        # 位置
        north = pos_msg.x  # 北
        east = pos_msg.y  # 东
        down = pos_msg.z  # 下（负值=高度）
        alt_abs = -down  # 真实高度（米）

        # 姿态 (弧度转角度)
        roll = att_msg.roll * 180 / 3.14159  # 横滚
        pitch = att_msg.pitch * 180 / 3.14159  # 俯仰
        yaw = att_msg.yaw * 180 / 3.14159  # 偏航

        # 速度
        v_n = pos_msg.vx
        v_e = pos_msg.vy
        v_d = pos_msg.vz

        # --------------------------
        # 【打印】位置 + 姿态 + 高度
        # --------------------------
        print(f"📊 位置(N/E): {north:6.2f} / {east:6.2f} m | "
              f"高度: {alt_abs:5.2f} m | "
              f"姿态(R/P/Y): {roll:6.1f} / {pitch:6.1f} / {yaw:6.1f} °")

        # 判断是否到达目标点
        dist = ((pos_msg.x - target_n) ** 2 + (pos_msg.y - target_e) ** 2) ** 0.5
        if dist < threshold:
            print(f"\n🎯 到达目标点: ({target_n}, {target_e})")
            break
        time.sleep(0.2)


# --- 任务开始 ---

# 1. 切换模式、解锁、起飞
print("🔄 切换到 GUIDED 模式")
master.mav.set_mode_send(master.target_system, mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, 4)  # GUIDED
time.sleep(1)

print("🔓 解锁电机")
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)
time.sleep(2)

print("🚀 起飞到 5米 高度")
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)

# 等待起飞并打印数据
print("起飞中... 实时数据如下：\n")
time.sleep(8)

# 2. 绘制正方形 (边长 5m)
waypoints = [
    (5, 0),  # 向北 5m
    (5, 5),  # 向东 5m
    (0, 5),  # 向南 5m
    (0, 0)  # 回到原点上方
]

for n, e in waypoints:
    print(f"\n➡️  正在前往: 北 {n}, 东 {e}")
    set_target_location(n, e, -5)  # 保持高度在 -5m (NED系)
    wait_reach_target(n, e)

# 3. 降落
print("\n⏬ 回到原点，准备降落...")
master.mav.command_long_send(
    master.target_system, master.target_component,
    mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0, 0, 0, 0
)

# 4. 确认锁定 (Disarm)
while True:
    heartbeat = master.recv_match(type='HEARTBEAT', blocking=True)
    if not (heartbeat.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED):
        print("\n🛬 降落完成，电机已锁定。")
        break
    time.sleep(1)

结果