From 5075d99ad61607a413110967bc59f8d1cda81b64 Mon Sep 17 00:00:00 2001 From: lenting89 Date: Tue, 16 Jun 2026 15:09:29 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AD=B8=E9=95=B7=E5=BB=BA=E8=AD=B0=E4=BF=AE?= =?UTF-8?q?=E6=94=B9=E5=85=A7=E5=AE=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unitdev04/esp32.py | 29 +++++----- src/unitdev04/udptest8.py | 109 ++++++++++++++++++++++++++++---------- 2 files changed, 93 insertions(+), 45 deletions(-) diff --git a/src/unitdev04/esp32.py b/src/unitdev04/esp32.py index 719b5c9..ae7f08a 100644 --- a/src/unitdev04/esp32.py +++ b/src/unitdev04/esp32.py @@ -20,6 +20,7 @@ import gc # GCS -> UAV: # DISC # POLL + target_sysid(1) + grant_bytes(2) +# COMM / RTK broadcast downlink bytes # # UAV -> GCS: # HELO + sysid(1) @@ -29,7 +30,8 @@ import gc # - HELO 不帶 remain。 # - DONE 保留 remain。 # - MY_SYSID 不寫死，由飛控 MAVLink 自動學。 -# - DEST_64 不寫死，由 GCS 封包的 XBee 0x90 src64 自動學。 +# - DEST_64 不寫死，由 DISC 的 XBee 0x90 src64 自動學。 +# - POLL 只負責 TDMA 授權；COMM / RTK 直接 forward。 # ========================================================= @@ -341,6 +343,10 @@ def learn_my_sysid_from_tx_buf(): global HELLO_SENT global tx_buf + # 已經學到 SYSID 後，不再掃 tx_buf，減少 ESP32 運算量 + if MY_SYSID is not None: + return + if len(tx_buf) == 0: return @@ -361,9 +367,6 @@ def learn_my_sysid_from_tx_buf(): sysid = get_mavlink_sysid(tx_buf, start) if sysid is not None and sysid > 0: - if MY_SYSID is not None: - return - if _sysid_candidate == sysid: _sysid_candidate_count += 1 else: @@ -458,9 +461,7 @@ def trim_tx_buffer_if_needed(): def flush_tx_buffer(grant_bytes): global tx_buf - if MY_SYSID is None: - return - + # DEST_64 未學到時不能送資料，保留防護避免先 pop tx_buf 造成資料遺失 if DEST_64 is None: return @@ -564,20 +565,14 @@ def process_xbee_buffer(): target_sysid, grant_bytes = parse_poll_payload(real_data) if target_sysid is not None: - learn_dest64_from_src64(src64) - learn_my_sysid_from_tx_buf() - try_send_pending_hello() - + # POLL 只負責 TDMA 授權；DISC 已負責 GCS address / SYSID 學習 if MY_SYSID is not None and target_sysid == MY_SYSID: flush_tx_buffer(grant_bytes) else: - # 一般 GCS -> FC MAVLink 下行資料 - if DEST_64 is None: - learn_dest64_from_src64(src64) - - if DEST_64 is not None and src64 == DEST_64: - uart_fc.write(real_data) + # COMM / RTK / 一般 GCS broadcast 下行資料 + # DISC 與 POLL 已經在前面處理掉，剩下資料直接轉給 FC / RTK module + uart_fc.write(real_data) rx_buf = rx_buf[total_len:] diff --git a/src/unitdev04/udptest8.py b/src/unitdev04/udptest8.py index db9011e..292bda1 100644 --- a/src/unitdev04/udptest8.py +++ b/src/unitdev04/udptest8.py @@ -26,8 +26,9 @@ from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg # # 封包格式： # GCS -> UAV: -# DISC -# POLL + target_sysid(1) + grant_bytes(2) +# DISC broadcast +# POLL + target_sysid(1) + grant_bytes(2) unicast if src64 known +# COMM / RTK downlink bytes broadcast # # UAV -> GCS: # HELO + sysid(1) @@ -37,19 +38,33 @@ from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg # ================= 多組設備設定 ================= CONFIGS = [ - {"serial_port": "/dev/ttyUSB0", "udp_port": 14551}, - {"serial_port": "COM15", "udp_port": 14590}, - {"serial_port": "/dev/ttyUSB2", "udp_port": 14553}, - {"serial_port": "/dev/ttyUSB3", "udp_port": 14554}, + # udp_output_port： + # UAV MAVLink 上行資料會由 udptest 轉送到這個 UDP port 給 Mission Planner / MAVProxy。 + # RTK / COMM 下行輸入 port 會自動使用 udp_output_port + COMM_LISTEN_PORT_OFFSET。 + {"serial_port": "/dev/ttyUSB0", "udp_output_port": 14551}, + {"serial_port": "COM15", "udp_output_port": 14590}, + {"serial_port": "/dev/ttyUSB2", "udp_output_port": 14553}, + {"serial_port": "/dev/ttyUSB3", "udp_output_port": 14554}, ] SERIAL_BAUDRATE = 115200 UDP_REMOTE_IP = '127.0.0.1' -UDP_LISTEN_FIXED_PORT = False +# COMM / RTK 下行使用固定 UDP port 接收，避免 Mission Planner 的 UDP output port 被佔用。 +# 例如 COM15 預設： +# Mission Planner / MAVProxy listen : udp_output_port = 14590 +# RTK / COMM input to udptest : 14590 + 1000 = 15590 +COMM_LISTEN_PORT_OFFSET = 1000 TARGET_ADDR64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' +# COMM / RTK 下行固定走 broadcast。 +# POLL 仍由 get_poll_dest_addr64() 決定 unicast / fallback broadcast。 +COMM_BROADCAST_ADDR64 = TARGET_ADDR64 +COMM_QUEUE_FLUSH_BYTES = 150 +COMM_MAX_PAYLOAD = 80 +COMM_CHUNK_GAP_SEC = 0.01 + INITIAL_ACTIVE_SYSIDS = [] ACTIVE_SYSIDS = list(INITIAL_ACTIVE_SYSIDS) @@ -127,6 +142,19 @@ def format_addr64(addr): return ''.join(f'{b:02X}' for b in addr) +def get_udp_output_port(config): + """UAV uplink MAVLink 轉送給 Mission Planner / MAVProxy 的 UDP output port。""" + return config.get('udp_output_port', config.get('udp_port')) + + +def get_comm_listen_port(config): + """ + RTK / COMM 下行資料進入 udptest 的固定 UDP input port。 + 由 udp_output_port 自動推算，避免每組 CONFIG 重複手寫。 + """ + return get_udp_output_port(config) + COMM_LISTEN_PORT_OFFSET + + def ensure_active_sysid(sysid): if sysid is None: return @@ -317,7 +345,7 @@ class SerialToUDP(asyncio.Protocol): self.udp_protocol = udp_protocol self.serial_port = serial_port self.buffer = bytearray() - self.gcs_tx_queue = bytearray() + self.comm_tx_queue = bytearray() self.transport = None self.current_poll_sysid = None @@ -484,29 +512,46 @@ class SerialToUDP(asyncio.Protocol): record_rssi(sysid, rssi_value, src64=src64) def write_to_serial(self, data): - self.gcs_tx_queue.extend(data) + """ + UDPHandler 收到 RTK / COMM 下行資料後會呼叫這裡。 + 注意：這不是 TDMA POLL，不做 sysid 分流；一律排入 COMM broadcast queue。 + """ + self.queue_comm_broadcast(data) + + def queue_comm_broadcast(self, data): + """COMM / RTK 下行資料佇列；後續用 XBee FFFF broadcast 送給所有 UAV。""" + self.comm_tx_queue.extend(data) def flush_gcs_queue(self): - if not self.gcs_tx_queue: + """相容舊呼叫名稱；實際執行 COMM broadcast queue flush。""" + self.flush_comm_broadcast_queue() + + def flush_comm_broadcast_queue(self): + if not self.comm_tx_queue: return - send_limit = min(len(self.gcs_tx_queue), 150) - data_to_send = self.gcs_tx_queue[:send_limit] - self.gcs_tx_queue = self.gcs_tx_queue[send_limit:] - asyncio.create_task(self._async_send_chunks(data_to_send)) + send_limit = min(len(self.comm_tx_queue), COMM_QUEUE_FLUSH_BYTES) + data_to_send = self.comm_tx_queue[:send_limit] + self.comm_tx_queue = self.comm_tx_queue[send_limit:] + asyncio.create_task(self._async_send_comm_broadcast_chunks(data_to_send)) - async def _async_send_chunks(self, data): + async def _async_send_comm_broadcast_chunks(self, data): + """ + COMM / RTK broadcast path： + - XBee dest64 固定為 FFFF broadcast + - 不使用 learned_sysid_to_addr64 + - 不影響 send_poll() 的 unicast POLL path + """ try: - max_payload = 80 sent_len = 0 while sent_len < len(data): - end_len = min(sent_len + max_payload, len(data)) + end_len = min(sent_len + COMM_MAX_PAYLOAD, len(data)) chunk = data[sent_len:end_len] sent_len = end_len - api_frame = build_api_tx_frame(chunk, TARGET_ADDR64, 0x00) + api_frame = build_api_tx_frame(chunk, COMM_BROADCAST_ADDR64, 0x00) self.transport.write(api_frame) - await asyncio.sleep(0.01) + await asyncio.sleep(COMM_CHUNK_GAP_SEC) except Exception: pass @@ -516,8 +561,8 @@ class SerialToUDP(asyncio.Protocol): # ========================================================= class UDPHandler(asyncio.DatagramProtocol): - def __init__(self, udp_port): - self.udp_port = udp_port + def __init__(self, udp_output_port): + self.udp_output_port = udp_output_port self.serial_transport = None self.transport = None self.mav_decoder = mavutil.mavlink.MAVLink(None) @@ -529,6 +574,7 @@ class UDPHandler(asyncio.DatagramProtocol): self.serial_transport = serial_transport def datagram_received(self, data, addr): + # 固定 UDP input 收到 RTK / COMM，下行一律走 XBee broadcast。 if self.serial_transport: self.serial_transport.write_to_serial(data) @@ -606,7 +652,7 @@ class UDPHandler(asyncio.DatagramProtocol): pass if self.transport: - self.transport.sendto(rf_data, (UDP_REMOTE_IP, self.udp_port)) + self.transport.sendto(rf_data, (UDP_REMOTE_IP, self.udp_output_port)) return None @@ -616,7 +662,9 @@ class UDPHandler(asyncio.DatagramProtocol): # ========================================================= async def setup_bridge(config): - port, udp = config['serial_port'], config['udp_port'] + port = config['serial_port'] + udp_output_port = get_udp_output_port(config) + comm_listen_port = get_comm_listen_port(config) try: ser = serial.Serial(port, SERIAL_BAUDRATE) @@ -626,10 +674,14 @@ async def setup_bridge(config): return None loop = asyncio.get_running_loop() - udp_handler = UDPHandler(udp) + udp_handler = UDPHandler(udp_output_port) + + await loop.create_datagram_endpoint( + lambda: udp_handler, + local_addr=('0.0.0.0', comm_listen_port) + ) - local_port = udp if UDP_LISTEN_FIXED_PORT else 0 - await loop.create_datagram_endpoint(lambda: udp_handler, local_addr=('0.0.0.0', local_port)) + print(f"[{port}] UDP output -> {UDP_REMOTE_IP}:{udp_output_port}, COMM input <- 0.0.0.0:{comm_listen_port}") serial_proto = SerialToUDP(udp_handler, port) await serial_asyncio.create_serial_connection(loop, lambda: serial_proto, port, baudrate=SERIAL_BAUDRATE) @@ -677,9 +729,10 @@ async def tdma_scheduler(serial_protocols): tdma_done_events[sysid] = asyncio.Event() while True: + # COMM / RTK 下行 broadcast queue；與 POLL unicast path 分開 for sp in serial_protocols: - if hasattr(sp, 'flush_gcs_queue'): - sp.flush_gcs_queue() + if hasattr(sp, 'flush_comm_broadcast_queue'): + sp.flush_comm_broadcast_queue() await asyncio.sleep(current_guard_ms / 1000.0)