沉浸式远航固定翼无人机系统-Part 3-数据中转服务器搭建

数据中转服务器搭建

34122: 34123,  # 链路 1: 数传控制 (GCS <-> Drone)
41995: 41996,  # 链路 2: 体感反馈 (SimTools)
51174: 51175   # 链路 3: 4G 遥控器/头追 (Remote)

自动转发脚本

import socket
import select
import time

class MavlinkSmartRelay:
    def __init__(self):
        # 端口定义
        self.PORT_AIR_TELEM = 34122   # 飞机 ESP32 连这里 (数传+遥控)
        self.PORT_GND_MP = 34123      # Mission Planner 连这里
        self.PORT_GND_RC = 51175      # 地面遥控 ESP32 连这里
        
        # 客户端地址存储
        self.addr_air = None
        self.addr_mp = None
        self.addr_rc = None

        # 初始化 Sockets
        self.sock_air = self._create_socket(self.PORT_AIR_TELEM)
        self.sock_mp = self._create_socket(self.PORT_GND_MP)
        self.sock_rc = self._create_socket(self.PORT_GND_RC)
        
        self.sockets = [self.sock_air, self.sock_mp, self.sock_rc]
        print("--- MAVLink 4G Hybrid Bridge Started ---")

    def _create_socket(self, port):
        s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        s.bind(('0.0.0.0', port))
        s.setblocking(False)
        print(f"[*] Listening on port {port}")
        return s

    def run(self):
        while True:
            # 使用 select 监听，效率远高于多线程
            readable, _, _ = select.select(self.sockets, [], [], 0.05)
            
            for s in readable:
                data, addr = s.recvfrom(8192)
                if not data: continue

                # 1. 来自飞机的流量 (Downlink)
                if s is self.sock_air:
                    self.addr_air = addr # 学习飞机地址
                    # 转发给地面站 (MP)
                    if self.addr_mp:
                        self.sock_mp.sendto(data, self.addr_mp)
                    # 转发给遥控端 (用于地面端 ESP32 显示回传数据)
                    if self.addr_rc:
                        self.sock_rc.sendto(data, self.addr_rc)

                # 2. 来自地面站 MP 的流量 (Uplink Telem)
                elif s is self.sock_mp:
                    self.addr_mp = addr
                    if self.addr_air:
                        self.sock_air.sendto(data, self.addr_air)

                # 3. 来自遥控端 ESP32 的流量 (Uplink RC)
                elif s is self.sock_rc:
                    self.addr_rc = addr
                    if self.addr_air:
                        # 遥控数据具有最高优先级，直接发往飞机
                        self.sock_air.sendto(data, self.addr_air)

if __name__ == "__main__":
    relay = MavlinkSmartRelay()
    try:
        relay.run()
    except KeyboardInterrupt:
        print("\nStopping...")

空中端esp32 S3代码

#include <WiFi.h>
#include <WiFiUdp.h>
#include <MAVLink.h> // 请确保安装 MAVLink 库

// ================= 配置区 =================
const char* ssid = "UAV1002";     // 4G路由器或手机热点
const char* password = "11223344";       // 密码
const char* server_ip = "1111111111111111111111111111111111111";   // 云服务器公网IP
const int server_port = 34122;           // 对应服务器 PORT_AIR_TELEM

#define RXD2 48  // ESP32-S3 引脚：接飞控 TX
#define TXD2 47  // ESP32-S3 引脚：接飞控 RX
// =========================================

WiFiUDP udp;
HardwareSerial PixSerial(2);

void setup() {
    Serial.begin(115200);
    // 飞控串口初始化 (波特率必须与飞控 SERIALx_BAUD 一致)
    PixSerial.begin(57600, SERIAL_8N1, RXD2, TXD2);

    WiFi.begin(ssid, password);
    while (WiFi.status() != WL_CONNECTED) {
        delay(500);
        Serial.print(".");
    }
    Serial.println("\nWiFi Connected!");
    udp.begin(34122); // 本地监听
}

void loop() {
    // 1. 下行：从飞控读取 -> 发往服务器
    while (PixSerial.available()) {
        uint8_t c = PixSerial.read();
        udp.beginPacket(server_ip, server_port);
        udp.write(c);
        udp.endPacket();
    }

    // 2. 上行：从服务器接收 (数传指令+遥控指令) -> 发往飞控
    int packetSize = udp.parsePacket();
    if (packetSize > 0) {
        uint8_t buf[1024];
        int len = udp.read(buf, sizeof(buf));
        if (len > 0) {
            PixSerial.write(buf, len);
        }
    }

    // 3. 心跳/打洞 (每秒发送一次)
    static unsigned long last_ping = 0;
    if (millis() - last_ping > 1000) {
        last_ping = millis();
        udp.beginPacket(server_ip, server_port);
        udp.write((uint8_t*)"p", 1); // 维持 NAT 映射
        udp.endPacket();
    }
}