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

转发脚本的任务需求

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

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

脚本编写的注意点

仿真飞机的发送端口

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

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

代码:

python 复制代码
#! /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,而且在给飞控发送心跳时,需要绑定端口号为飞机的端口号

相关推荐
孞㐑¥1 小时前
Linux之Socket 编程 UDP
linux·服务器·c++·经验分享·笔记·网络协议·udp
2501_915921433 小时前
iOS IPA 混淆实测分析:从逆向视角验证加固效果与防护流程
websocket·网络协议·tcp/ip·http·网络安全·https·udp
2501_915918413 小时前
打造可观测的 iOS CICD 流程:调试、追踪与质量保障全记录
websocket·网络协议·tcp/ip·http·网络安全·https·udp
2501_915909067 小时前
调试 WebView 旧资源缓存问题:一次从偶发到复现的实战经历
websocket·网络协议·tcp/ip·http·网络安全·https·udp
wanhengidc9 小时前
UDP服务器主要是指什么意思?
服务器·网络协议·udp
2501_9159214310 小时前
请求未达服务端?iOS端HTTPS链路异常的多工具抓包排查记录
websocket·网络协议·tcp/ip·http·网络安全·https·udp
2501_9160074714 小时前
iOS 接口频繁请求导致流量激增?抓包分析定位与修复全流程
websocket·网络协议·tcp/ip·http·网络安全·https·udp
2501_9160137415 小时前
用Fiddler中文版抓包工具掌控微服务架构中的接口调试:联合Postman与Charles的高效实践
websocket·网络协议·tcp/ip·http·网络安全·https·udp
00后程序员张17 小时前
调试 WebView 接口时间戳签名问题:一次精细化排查和修复过程
websocket·网络协议·tcp/ip·http·网络安全·https·udp
2501_9159214310 天前
后台发热、掉电严重?iOS 应用性能问题实战分析全过程
websocket·网络协议·tcp/ip·http·网络安全·https·udp