转发多台px4仿真UDP数据到地面站

转发脚本的任务需求

仿真采用UDP通信方式，在 wsl 中仿真三台飞机，项目需要将一台电脑中的三台飞机的数据打包发送到另一台飞机的地面站，但地面站是无法直接访问另一台主机的 wsl 中的端口的，wsl 中的端口需要本机才能访问，接收数据，因此，需要编写一个脚本在主机运行，接收 wsl 中仿真飞机的数据，通过主机的 ip 与端口发送给 app 的主机 ip 与端口()，并且，接收 app 的命令，转发给三台飞机(命令中会包含飞机id，因此脚本不需要做筛选，直接转发给三个即可)。

目标端口与本机都需要在同一个局域网下(连接同一个wifi)，整体框图如下：

脚本编写的注意点

仿真飞机的发送端口

仿真飞控在没有收到地面站心跳包时，是往 14550 端口发送数据的，只有在收到自己预先配置的接收端口接收到数据后，才会往配置的接收端口发送数据，一般配置的端口都是 18570，多台飞机就往后顺延，18571，18572

地面站发送心跳也是必须往飞机配置的端口发送心跳才行，例如飞机是18570，那么必须往这个端口发心跳，飞控才会改为往18570发送数据。

代码：

#! /usr/bin/env python
# -*-coding:utf-8-*
import socket

import socket
import threading
import time

# 监控gcs发送的心跳包并转发给飞机的心跳端口 14550 (心跳端口为多机共用端口)
def udp_listen_gcs_to_uav(gcs_sock, uav1_sock, uav2_sock, uav3_sock, uav_ip, uav1_port, uav2_port, uav3_port):
    # print("listen GCS UDP port") 
       
    while True:
        # 接收数据        
        buffer_size = 255  #缓冲区大小        
        gcs_data, gcs_addr = gcs_sock.recvfrom(buffer_size)  #读取gcs发送过来的数据包，转发给wsl的飞控 
        print("addr:", gcs_addr, "data:", gcs_data)

        #发送数据到wsl的飞机地址与端口        
        uav1_sock.sendto(gcs_data, (uav_ip, uav1_port))
        uav2_sock.sendto(gcs_data, (uav_ip, uav2_port))
        uav3_sock.sendto(gcs_data, (uav_ip, uav3_port))


# 监控uav1 18570 端口发送的消息并转发给gcs端口
def udp_listen_uav_to_gcs(uav_sock, gsc_sock, gcs_ip, gcs_port):
    # print("listen uav UDP port:") 
       
    while True:
        # 接收数据        
        buffer_size = 255  #缓冲区大小        
        uav_data, uav_addr = uav_sock.recvfrom(buffer_size)  #读取gcs发送过来的数据包，转发给wsl的飞控 
        # print("uav_addr:", uav_addr, "uav_data:", uav_data)  
              
        # 发送数据到wsl的飞机地址与端口        
        gsc_sock.sendto(uav_data, (gcs_ip, gcs_port))

def main():
    self_UDP_IP = "172.16.20.86"  # 本机IP UDP通信可以不使用此字段    
    gcs_tx_ip = "172.16.20.16"    # gcs发送ip    
    gcs_tx_port = 14650  # gcs发送端口    
    gcs_rx_ip = "172.16.20.16"   # gcs接收ip    
    gcs_rx_port = 14650  # gcs接收端口    
    uav_ip = "172.27.21.175"  # 飞机的ip都是相同的    
    uav1_port = 18570   # 飞机1的端口号  在发送了心跳包后，需要与飞机在此端口进行通信    
    uav2_port = 18571   # 飞机1的端口号  在发送了心跳包后，需要与飞机在此端口进行通信    
    uav3_port = 18572   # 飞机1的端口号  在发送了心跳包后，需要与飞机在此端口进行通信    
    
    #创建与地面站以及与飞机通信的sock，之后传入各个线程    
    gcs_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    gcs_sock.bind(('', gcs_tx_port))
    # 绑定使用哪个端口发送  要让飞机往18570发送数据，因此心跳包也要从18570发送，飞控从18570接收到心跳包后，就会往18570发送自身数据    
    uav1_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    uav1_sock.bind(('', uav1_port))
    uav2_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    uav2_sock.bind(('', uav2_port))
    uav3_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    uav3_sock.bind(('', uav3_port))


    # 创建线程 监听gcs的包之后转发给飞控心跳端口 14550    
    listen_gcs_to_uav_thread = threading.Thread(target=udp_listen_gcs_to_uav, args=[gcs_sock, uav1_sock, uav2_sock, uav3_sock, uav_ip, uav1_port, uav2_port, uav3_port])

    # 创建线程 监听wsl中的uav1,2,3端口消息 18570,18571,18572  并转发给gcs端口    
    listen_uav1_to_gcs_thread = threading.Thread(target=udp_listen_uav_to_gcs, args=[uav1_sock, gcs_sock, gcs_rx_ip, gcs_rx_port])
    listen_uav2_to_gcs_thread = threading.Thread(target=udp_listen_uav_to_gcs, args=[uav2_sock, gcs_sock, gcs_rx_ip, gcs_rx_port])
    listen_uav3_to_gcs_thread = threading.Thread(target=udp_listen_uav_to_gcs, args=[uav3_sock, gcs_sock, gcs_rx_ip, gcs_rx_port])

    listen_gcs_to_uav_thread.start()
    listen_uav1_to_gcs_thread.start()
    listen_uav2_to_gcs_thread.start()
    listen_uav3_to_gcs_thread.start()

    # 等待线程结束    
    listen_gcs_to_uav_thread.join()  # 等待心跳包发送完成后就启动下一个任务    
    listen_uav1_to_gcs_thread.join()
    listen_uav2_to_gcs_thread.join()
    listen_uav3_to_gcs_thread.join()

# Press the green button in the gutter to run the script.
if __name__ == '__main__':
    main()

在上述代码中，UDP 在接收时是用不到此ip的，也就是 gcs_sock.bind(('', gcs_tx_port))，函数中的 ip 可以不填，只需要将端口号填对即可，但在发送时，需要指定 ip，而且在给飞控发送心跳时，需要绑定端口号为飞机的端口号