wsl2 ubuntu22.04+win11，只在子系统中开启px4的gzsim仿真节点，其余功能放在windows系统中完成。我在子系统里面用tkinter做了一个ui，显示效果不好（仅此而已），因此计划把编队代码和gui都放在Windows端执行。这个改动的工程意义和使用场景还不明朗，后续有时间补充。

一、最小修改法

之前代码参考：Ubuntu22.04+PX4+MAVSDK+Gazebo+QGC固定翼编队仿真遇到的问题记录_ubuntu22.04 px4 ros gazebo-CSDN博客

1）Windows 侧

step1.

-------------好像不需要做任何修改，打开qgc就好了

2）WSL Ubuntu 端

【注意】实测发现，WSL2里面不需要配置这个也能直接用！！！以下操作没啥用

step1.

用 mavlink-router 转发

# WSL2 内
sudo apt update
sudo apt install -y mavlink-router

我用的源里面没有这个包，因此会报错：E: Unable to locate package mavlink-router

所以我使用源码安装

# 依赖
sudo apt update
sudo apt install git meson ninja-build gcc g++ pkg-config \
                 libsystemd-dev python3-pip
# 旧版需要安装 autoconf-archive（如遇到构建宏缺失时再装）：
# sudo apt install autoconf-archive

# 拉取与构建
git clone https://github.com/intel/mavlink-router.git
cd mavlink-router
meson setup build
ninja -C build
sudo ninja -C build install

可能是我没挂梯子的原因，构建时出现

meson.build:127:0: ERROR: Include dir modules/mavlink_c_library_v2/ardupilotmega does not exist.

因此需要重新拉取子模块

git submodule sync
git submodule update --init modules/mavlink_c_library_v2

或者直接进入并把子模块拉齐

cd ~/mavlink-router
git reset --hard
git clean -fdx
git submodule sync --recursive
git submodule update --init --recursive

可自检一下是否有 ardupilotmega 这些头文件：

ls modules/* -d
ls modules/mavlink_c_library_v2/ardupilotmega 2>/dev/null | head

会显示

ardupilotmega.h
mavlink.h
mavlink_msg_adap_tuning.h
mavlink_msg_ahrs.h
mavlink_msg_ahrs2.h
mavlink_msg_ahrs3.h
mavlink_msg_airspeed_autocal.h
mavlink_msg_aoa_ssa.h
mavlink_msg_ap_adc.h
mavlink_msg_autopilot_version_request.h

然后再build一下就可以了

rm -rf build
meson setup build
ninja -C build
sudo ninja -C build install

继续在命令行输入

mavlink-routerd -h

显示Could not open conf file '/etc/mavlink-router/main.conf' (No such file or directory)就可以了，下一步开始配置main.conf

step2.

创建 /etc/mavlink-router/main.conf

cd /etc
sudo mkdir mavlink-router
cd mavlink-router/
sudo touch main.conf
sudo chmod 777 main.conf

以 6 机为例，把 <WIN_IP> 换成你 Windows IPv4，当前正在使用的真实连外网的那个，不是 127.0.0.1 或 WSL 内部的地址，也不是VMware、VirtualBox、WSL Loopback、meta之类的虚拟网卡：

[General]
TcpServerPort=0
ReportStats=false

# --- 监听 WSL 本地的 SITL 端口（14540+i）---
[UdpEndpoint sitl_0]
Mode=Eavesdropping
Address=0.0.0.0
Port=14540
[UdpEndpoint sitl_1]
Mode=Eavesdropping
Address=0.0.0.0
Port=14541
[UdpEndpoint sitl_2]
Mode=Eavesdropping
Address=0.0.0.0
Port=14542
[UdpEndpoint sitl_3]
Mode=Eavesdropping
Address=0.0.0.0
Port=14543
[UdpEndpoint sitl_4]
Mode=Eavesdropping
Address=0.0.0.0
Port=14544
[UdpEndpoint sitl_5]
Mode=Eavesdropping
Address=0.0.0.0
Port=14545

# --- 转发到 Windows GUI 端口 ---
[UdpEndpoint win_gui_0]
Address=<WIN_IP>
Port=14540
[UdpEndpoint win_gui_1]
Address=<WIN_IP>
Port=14541
[UdpEndpoint win_gui_2]
Address=<WIN_IP>
Port=14542
[UdpEndpoint win_gui_3]
Address=<WIN_IP>
Port=14543
[UdpEndpoint win_gui_4]
Address=<WIN_IP>
Port=14544
[UdpEndpoint win_gui_5]
Address=<WIN_IP>
Port=14545

step3.

启动./start_sih.sh

#!/bin/bash

tmux new-session -d -s formation 

tmux new-window -t formation -n Gazebo "python3 simgz"
# tmux new-window -t formation -n QGC "./QGroundControl-x86_64.AppImage"   

for i in $(seq 0 5); do
    
    # 启动PX4实例
    pose_y=0
    pose_x=$((i * 50))
    pose="$pose_x,$pose_y,0,0,0,1.57079632679"
    cmd="cd ~/PX4-Autopilot && \
    PX4_GZ_STANDALONE=1 \
    PX4_GZ_MODEL_POSE=$pose \
    PX4_SYS_AUTOSTART=4003 \
    PX4_SIM_MODEL=gz_rc_cessna \
    ./build/px4_sitl_default/bin/px4 -i $i "

    tmux new-window -t formation -n PX4_$i "$cmd"
done

tmux attach -t formation 

原则上来说，应该要先启动mavlink_routerd服务，不知道为啥我这直接启动了

sudo systemctl enable --now mavlink-routerd

然后无人机就自动连到windows的地面站了

之后在windows端直接执行formationMain那三个代码就可以啦

二、直接让 PX4 指向 Windows

这个要改PX4 的 mavlink 启动项，等做24机编队的时候再改

三、编队代码（无GUI）

gui版本等做好了我再补充，下面这些还是基于在cmd扣字的编队方法，当然用了gui原理也是一样，我会做一个独立的gui，用local socket和编队代码交互，其实就是用点击按钮代替了扣字而已

加入了队形变换功能。

#!/usr/bin/env python3
"""
formationMain.py
"""

import asyncio, math, random
from mavsdk import System
from dronesConfig import UAV_CFGS, connect_uav, set_speed
from dronesBehavior import rtl_all, land_all, takeoff_all
from dronesBehavior import leader_task, follower_task, TRAIL_DIST   # 新增：引入默认间距

HOME_LAT = 39.91401032678762
HOME_LON = 116.39717200165842

async def keyboard_listen(tasks, drones:System, home_lat, home_lon):
    """
    监听键盘输入，处理命令
    """
    # 新增：共享状态（航向、编队类型与间距）
    dir_ref  = {"hdg": 0.0}
    form_ref = {"type": 1, "spacing": TRAIL_DIST}  # 1=三角；2=横向一字；3=纵向一字

    while True:
        cmd = await asyncio.to_thread(input, ">> ")
        s = cmd.strip().lower()

        if s == "takeoff":
            for t in tasks: t.cancel()
            await asyncio.gather(*tasks, return_exceptions=True)
            tasks.clear()
            await takeoff_all(drones)

        elif s == "rtl":
            for t in tasks: t.cancel()
            await asyncio.gather(*tasks, return_exceptions=True)
            tasks.clear()
            await rtl_all(drones)

        elif s == "land":
            for t in tasks: t.cancel()
            await asyncio.gather(*tasks, return_exceptions=True)
            tasks.clear()
            await land_all(drones)

        elif s == "formation-test-1":
            # 启动领机/僚机任务（默认三角形，间距=TRAIL_DIST）
            for t in tasks: t.cancel()
            await asyncio.gather(*tasks, return_exceptions=True)
            tasks.clear()

            tasks.append(asyncio.create_task(
                leader_task(drones[0], home_lat, home_lon, dir_ref)
            ))
            for i in range(1, len(drones)):
                tasks.append(asyncio.create_task(
                    follower_task(drones[i], drones[0], dir_ref, form_ref, i)
                ))
            print(f"[formation] 三角形，间距 {form_ref['spacing']} m 已启动")

        else:
            # 新增：解析 (类型,间距)，兼容中文全角符号
            norm = s.replace('（','(').replace('）',')').replace('，',',').replace(' ', '')
            if norm.startswith('(') and norm.endswith(')') and ',' in norm:
                try:
                    a, b = norm[1:-1].split(',', 1)
                    ftype   = int(a)
                    spacing = float(b)
                    if ftype in (1,2,3) and spacing > 0:
                        form_ref["type"]    = ftype
                        form_ref["spacing"] = spacing
                        name = {1:"三角形", 2:"横向一字", 3:"纵向一字"}[ftype]
                        print(f"[formation] 切换为 {name}，间距 {spacing} m")
                    else:
                        print("参数错误：类型须为1/2/3，间距>0")
                except Exception:
                    print("格式应为 (类型,间距)，例如 (1,500)")
            # 其它输入不处理
            continue


async def task_init():
    print('ready to start !')


async def main():
    """
    主协程，连接所有无人机，起飞，启动领机跟随任务(测试)
    """
    # 连接所有无人机
    drones_tuple = await asyncio.gather(*(connect_uav(c) for c in UAV_CFGS))
    drones = list(drones_tuple)                # 序保持与 UAV_CFGS 对应
    print('ready to start !')

    # tasks 用列表统一管理
    tasks = []

    await keyboard_listen(tasks, drones, HOME_LAT, HOME_LON)

if __name__ == "__main__":
    asyncio.run(main())

#!/usr/bin/env python3
"""
dronesBehavior.py
"""

import asyncio
import math
import random
from mavsdk import System

from dronesConfig import calculate_new_coordinates, calculate_positions_distance, calculate_relative_distance
from dronesConfig import set_speed

# ========== 飞行参数 ==========
ALT_TKOF    = 50.0
LEG_DIST    = 1500*2
TRAIL_DIST  = 200     # 默认间距（仍保留，作为初始值）
HOME_R      = 200
REACH_R     = 200
CHECK_INT   = 1.0
SQRT3_2     = math.sqrt(3) / 2

# ========== 编队偏移计算（新增的极小函数） ==========
def get_offset(i: int, form_type: int, s: float):
    """
    返回僚机 i 在领机机体系下的 (dx_body, dy_body)
    form_type: 1=三角形；2=横向一字；3=纵向一字
    s: 编队间距（米）
    """
    if form_type == 1:
        # 等边三角（与原 _OFFSETS_BODY 保持几何一致，只是把 TRAIL_DIST 改为 s）
        mapping = {
            1: (-s*SQRT3_2, -0.5*s),
            2: (-s*SQRT3_2,  0.5*s),
            3: (-2*s*SQRT3_2, 0.0),
            4: (-2*s*SQRT3_2, -1.0*s),
            5: (-2*s*SQRT3_2,  1.0*s),
        }
        return mapping.get(i, (0.0, 0.0))

    if form_type == 2:
        # 横向一字：整体略微后移，左右铺开
        mapping = {
            1: (-0.5*s, -2.0*s),
            2: (-0.5*s, -1.0*s),
            3: (-0.5*s,  0.0*s),
            4: (-0.5*s,  1.0*s),
            5: (-0.5*s,  2.0*s),
        }
        return mapping.get(i, (0.0, 0.0))

    if form_type == 3:
        # 纵向一字：顺航向依次排开
        mapping = {
            1: (-1.0*s, 0.0),
            2: (-2.0*s, 0.0),
            3: (-3.0*s, 0.0),
            4: (-4.0*s, 0.0),
            5: (-5.0*s, 0.0),
        }
        return mapping.get(i, (0.0, 0.0))

    return (0.0, 0.0)


# ========== 飞行逻辑（新版） ==========
async def leader_task(lead:System, start_lat, start_lon, dir_ref):
    """
    起飞后首先向正北飞行一个航段（LEG_DIST），
    之后每段在当前航向上随机偏转 -60°…+60° 并继续前行。
    dir_ref["hdg"] 实时存储领机航向（真方位角，0°=北，顺时针为正）。
    """
    cur_lat, cur_lon = start_lat, start_lon
    heading          = 0.0
    print("Leader task started")
    while True:
        rad   =             math.radians(heading)
        n_off =  LEG_DIST * math.cos(rad)
        e_off =  LEG_DIST * math.sin(rad)
        tgt_lat, tgt_lon = calculate_new_coordinates(cur_lat, cur_lon, n_off, e_off)

        await lead.action.goto_location(tgt_lat, tgt_lon, ALT_TKOF,0)

        lead_true_heading = await lead.telemetry.heading().__anext__()
        dir_ref["hdg"]    = round(lead_true_heading.heading_deg)

        while True:
            lead_true_heading = await lead.telemetry.heading().__anext__()
            pos               = await lead.telemetry.position().__anext__()
            dir_ref["hdg"]    = round(lead_true_heading.heading_deg)
            if calculate_positions_distance(pos.latitude_deg, pos.longitude_deg, tgt_lat, tgt_lon) <= REACH_R:
                break
            await asyncio.sleep(0.5)

        await asyncio.sleep(0.5)
        cur_lat, cur_lon = tgt_lat, tgt_lon
        heading = (heading + random.uniform(-90.0, 90.0)) % 360.0


async def follower_task(fol:System, lead:System, dir_ref, form_ref, i):
    """
    僚机保持指定编队（form_ref["type"]）与间距（form_ref["spacing"]）。
    所有计算均随领机实时航向旋转。
    """
    temp_delta = 300.0   # “无穷远”延伸量，用于保证 goto 的航向一致
    print(f"follower_task[{i+1}] started")
    while True:
        # 读取共享的编队形态与间距（可被键盘命令实时更新）
        spacing   = float(form_ref.get("spacing", TRAIL_DIST))
        form_type = int(form_ref.get("type", 1))

        # 读取领机状态
        pos_lead          = await lead.telemetry.position().__anext__()
        lead_true_heading = await lead.telemetry.heading().__anext__()
        pos_true_heading  = await fol.telemetry.heading().__anext__()
        dir_ref["hdg"]    = round(lead_true_heading.heading_deg)
        hdg               = dir_ref["hdg"]
        rad               = math.radians(hdg)

        # 当前形态下的机体系偏移
        dx_body, dy_body  = get_offset(i, form_type, spacing)

        # 将机体坐标系偏移量旋转到地理坐标系 (北、东)
        n_off =  dx_body * math.cos(rad) - dy_body * math.sin(rad)
        e_off =  dx_body * math.sin(rad) + dy_body * math.cos(rad)

        # 期望位置及沿航向的“远点”坐标
        tgt_lat, tgt_lon = calculate_new_coordinates(pos_lead.latitude_deg, pos_lead.longitude_deg, n_off, e_off)
        n_inf = n_off + (spacing + temp_delta) * math.cos(rad)
        e_inf = e_off + (spacing + temp_delta) * math.sin(rad)
        tgt_lat_inf, tgt_lon_inf = calculate_new_coordinates(pos_lead.latitude_deg, pos_lead.longitude_deg, n_inf, e_inf)

        # 导航至“远点”，PX4 自动插值
        await fol.action.goto_location(tgt_lat_inf, tgt_lon_inf, ALT_TKOF,0)

        # 依据沿航向误差调整空速
        pos_fol = await fol.telemetry.position().__anext__()
        dn, de  = calculate_relative_distance(pos_fol.latitude_deg, pos_fol.longitude_deg, tgt_lat, tgt_lon)

        s =  dn * math.cos(rad) + de * math.sin(rad)            # 正值表示超前
        d = -dn * math.sin(rad) + de * math.cos(rad)            # 横向误差

        sign_s = 1 if s > 0 else -1
        tgt_spd = 30.0 - (1/5)*s - (1e-4)*(d**2)*sign_s
        tgt_spd = max(10.0, min(50.0, tgt_spd))
        await fol.param.set_param_float("FW_AIRSPD_TRIM", round(tgt_spd, 1))

        await asyncio.sleep(CHECK_INT)


# 起飞/返航/降落（无改动）
async def takeoff_all(drone:System):
    for i, d in enumerate(drone):
        await d.action.arm()
        await asyncio.sleep(0.5)
        await d.action.takeoff()
        await asyncio.sleep(1)
        print(f"drone[{i+1}] armed and taking off...")
    await asyncio.sleep(1)

    for i, d in enumerate(drone):
        await set_speed(d)
        smax = await d.param.get_param_float("FW_AIRSPD_MAX",)
        smin = await d.param.get_param_float("FW_AIRSPD_MIN")
        scur = await d.param.get_param_float("FW_AIRSPD_TRIM")
        print(f"drone[{i+1}] speed set: max={smax}, min={smin}, trim={scur}")
    await asyncio.sleep(0.5)


async def rtl_all(drones:System):
    await asyncio.gather(*(d.param.set_param_float("FW_AIRSPD_TRIM", 40) for d in drones))
    await asyncio.gather(*(d.action.return_to_launch() for d in drones))


async def land_all(drones:System):
    await asyncio.gather(*(d.action.land() for d in drones))

#!/usr/bin/env python3
"""
dronesConfig.py
"""

import asyncio, math, random
from mavsdk import System
from geopy import Point, distance
from geopy.distance import geodesic, great_circle
from math import radians, cos


# ========== 机队配置==========
UAV_CFGS = [
    {"tag": "LEAD",   "ctl": 14540, "grpc": 50051, "system_address": "udp://0.0.0.0:14540"},   # 领机
    {"tag": "DRONE1", "ctl": 14541, "grpc": 50052, "system_address": "udp://0.0.0.0:14541"},   # 僚机 1
    {"tag": "DRONE2", "ctl": 14542, "grpc": 50053, "system_address": "udp://0.0.0.0:14542"},   # 僚机 2
    {"tag": "DRONE3", "ctl": 14543, "grpc": 50054, "system_address": "udp://0.0.0.0:14543"},   # 僚机 3
    {"tag": "DRONE4", "ctl": 14544, "grpc": 50055, "system_address": "udp://0.0.0.0:14544"},   # 僚机 4
    {"tag": "DRONE5", "ctl": 14545, "grpc": 50056, "system_address": "udp://0.0.0.0:14545"},   # 僚机 5
    # …继续追加 {"tag": "DRONE3", ...}
] # 要和仿真启动的数量一致，才发现system_address没用上，懒得删了

async def set_speed(drone:System):
    """设置无人机速度/加速度等，参数不对的话在这里调"""
    param = drone.param
    await param.set_param_float("FW_AIRSPD_MAX", 50.0)      # 空速
    await param.set_param_float("FW_AIRSPD_TRIM", 30.0)     # 巡航基准
    await param.set_param_float("FW_AIRSPD_MIN", 10.0)      # 最小空速，同步安全下限(默认7会STALL)
    # await param.set_param_float("FW_THR_MAX", 1.0)          # 推力最大值，若推力不足，可放到 1 (=100 %)
    await param.set_param_float("FW_THR_SLEW_MAX", 1)     #  油门变化速度，值越大，油门跃迁越快
    await param.set_param_float("RTL_RETURN_ALT", 50)       # 返航高度
    await param.set_param_float("FW_T_SPDWEIGHT", 2.0)      # 0.0=高度绝对优先，2.0=速度绝对优先
    await param.set_param_float("FW_P_LIM_MAX",45.0)        # 俯仰角最大值，单位度
    await param.set_param_float("FW_P_LIM_MIN",-45.0)       # 俯仰角最小值，单位度
    # await param.set_param_float("TECS_TIME_CONST", 3.0)   # TECS更灵敏，默认 5s，太低会震荡

# ========== 通讯连接 ==========
async def connect_uav(cfg):
    drone = System(port=cfg["grpc"])
    await drone.connect(system_address=f"udp://:{cfg['ctl']}")
    async for h in drone.telemetry.health():
        if h.is_global_position_ok and h.is_home_position_ok:
            break
    return drone


# ========== 经纬换算 ==========

# 将距离转换为纬度lat和经度lon（替换原函数offset）
def calculate_new_coordinates(lat, lon, north_m, east_m):
    """
    支持正北/正南、正东/正西四象限位移：
    - north_m  >0 向北，<0 向南
    - east_m   >0 向东，<0 向西
    """
    origin = Point(lat, lon)

    # 先沿南北方向
    if north_m:
        bearing_ns = 0 if north_m > 0 else 180
        origin = geodesic(meters=abs(north_m)).destination(origin, bearing_ns)

    # 再沿东西方向
    if east_m:
        bearing_ew = 90 if east_m > 0 else 270
        origin = geodesic(meters=abs(east_m)).destination(origin, bearing_ew)

    return round(origin.latitude, 7), round(origin.longitude, 7)

# 计算两个纬度lat和经度lon距离（替换原函数dist_m）
def calculate_positions_distance(x1, y1, x2, y2):
    return distance.distance((x1, y1), (x2, y2)).m

# 计算目标点相对于基准点的东西南北距离
def calculate_relative_distance(lat, lon, ref_lat, ref_lon, earth_radius=6378137.0):
    """
    计算坐标点 (lat, lon) 相对于参考点 (ref_lat, ref_lon) 的
    向北距离 north 和向东距离 east。

    参数
    ----
    lat, lon        : 目标点的纬度、经度 (度)
    ref_lat, ref_lon: 参考点的纬度、经度 (度)
    earth_radius    : 地球半径 (米)，默认值为 6378137.0 米
    返回
    ----
    north : float   向北位移，单位 m；北为正，南为负
    east  : float   向东位移，单位 m；东为正，西为负
    """
    # 经纬度差换算成弧度
    d_lat = radians(lat - ref_lat)
    d_lon = radians(lon - ref_lon)

    # 参考纬度（或两点纬度平均）转弧度，用于计算东西向缩放
    mean_lat = radians((lat + ref_lat) / 2.0)

    # 向北、向东位移
    north = d_lat * earth_radius
    east  = d_lon * earth_radius * cos(mean_lat)

    return north, east

if __name__ == "__main__":

    # 基准点 (纬度, 经度)
    base_point = (40.7128, -74.0060)  # 纽约
    # 目标点 (纬度, 经度)
    target_point = (40.6130, -75.0058)  # 纽约附近某点

    n,e = calculate_relative_distance(*target_point, *base_point) # 前面减后面
    print(n,e)