tmp commit

src/fc_network_adapter/fc_network_adapter/serialManager.py

@@ -82,6 +82,16 @@ class FrameProcessor(ABC):
     def __init__(self):
         self.buffer = bytearray()
         
+        N = 10
+        self.tx_interval_bytes: deque[int] = deque([0] * N,maxlen=N)
+        self.tx_interval_rate: deque[float] = deque([0] * N,maxlen=N)
+        self.tx_record_byte: int = 0
+        self.tx_record_time = time.time()
+        self.rx_interval_bytes: deque[int] = deque([0] * N, maxlen=N)
+        self.rx_interval_rate: deque[float] = deque([0] * N,maxlen=N)
+        self.rx_record_byte: int = 0
+        self.rx_record_time = time.time()
+    
     @abstractmethod
     def process_incoming(self, data: bytes) -> bytes:
         """
@@ -102,17 +112,6 @@ class FrameProcessor(ABC):
 class RawFrameProcessor(FrameProcessor):
     """原始數據直通處理器"""
 
-    def __init__(self):
-        N = 10
-        self.tx_interval_bytes: deque[int] = deque([0] * N,maxlen=N)
-        self.tx_interval_rate: deque[float] = deque([0] * N,maxlen=N)
-        self.tx_record_byte: int = 0
-        self.tx_record_time = time.time()
-        self.rx_interval_bytes: deque[int] = deque([0] * N, maxlen=N)
-        self.rx_interval_rate: deque[float] = deque([0] * N,maxlen=N)
-        self.rx_record_byte: int = 0
-        self.rx_record_time = time.time()
-
     def process_incoming(self, data: bytes) -> bytes:
         """直接返回原始數據，不進行緩衝"""
         
@@ -129,6 +128,7 @@ class RawFrameProcessor(FrameProcessor):
     
     def process_outgoing(self, data: bytes) -> bytes:
         """直接返回原始數據，不進行封裝"""
+
         interval = time.time() - self.rx_record_time
         if interval >= 1:
             self.rx_interval_bytes.append(self.rx_record_byte)
@@ -188,6 +188,16 @@ class XBeeFrameProcessor_Base(FrameProcessor):
     # ---- 對外契約 ----
     def process_incoming(self, data: bytes) -> bytes:
         """處理 XBee API 幀並提取 payload"""
+
+        interval = time.time() - self.tx_record_time
+        if interval >= 1:
+            self.tx_interval_bytes.append(self.tx_record_byte)
+            self.tx_interval_rate.append(self.tx_record_byte/interval)
+            self.tx_record_time = time.time()
+            self.tx_record_byte = 0
+        else:
+            self.tx_record_byte += len(data)
+
         self.buffer.extend(data)
         payloads = []
 
@@ -203,6 +213,16 @@ class XBeeFrameProcessor_Base(FrameProcessor):
 
     def process_outgoing(self, data: bytes) -> bytes:
         """將數據封裝為 XBee API 傳輸幀"""
+
+        interval = time.time() - self.rx_record_time
+        if interval >= 1:
+            self.rx_interval_bytes.append(self.rx_record_byte)
+            self.rx_interval_rate.append(self.rx_record_byte/interval)
+            self.rx_record_time = time.time()
+            self.rx_record_byte = 0
+        else:
+            self.rx_record_byte += len(data)
+
         return self._encapsulate(
             data,
             dest_addr64 = self.DEST_ADDR64_BRAODCAST,
@@ -1629,7 +1649,7 @@ if __name__ == '__main__':
     serial_id = 1
     processor = sm.get_espv1_processor(serial_id)
     if processor is None:
-        return
+        sys.exit(1)
 
     # for x in range(10):
     #     processor.request_discovery()

src/unitdev02/unitdev02/udptest8.py

@@ -26,8 +26,9 @@ from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg
 #
 # 封包格式：
 # GCS -> UAV:
-#   DISC
+#   DISC                                broadcast
-#   POLL + target_sysid(1) + grant_bytes(2)
+#   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},
+    # udp_output_port：
-    {"serial_port": "COM15", "udp_port": 14590},
+    #   UAV MAVLink 上行資料會由 udptest 轉送到這個 UDP port 給 Mission Planner / MAVProxy。
-    {"serial_port": "/dev/ttyUSB2", "udp_port": 14553},
+    #   RTK / COMM 下行輸入 port 會自動使用 udp_output_port + COMM_LISTEN_PORT_OFFSET。
-    {"serial_port": "/dev/ttyUSB3", "udp_port": 14554},
+    {"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)
+        send_limit = min(len(self.comm_tx_queue), COMM_QUEUE_FLUSH_BYTES)
-        data_to_send = self.gcs_tx_queue[:send_limit]
+        data_to_send = self.comm_tx_queue[:send_limit]
-        self.gcs_tx_queue = self.gcs_tx_queue[send_limit:]
+        self.comm_tx_queue = self.comm_tx_queue[send_limit:]
-        asyncio.create_task(self._async_send_chunks(data_to_send))
+        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):
+    def __init__(self, udp_output_port):
-        self.udp_port = udp_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
+    print(f"[{port}] UDP output -> {UDP_REMOTE_IP}:{udp_output_port}, COMM input <- 0.0.0.0:{comm_listen_port}")
-    await loop.create_datagram_endpoint(lambda: udp_handler, local_addr=('0.0.0.0', local_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'):
+            if hasattr(sp, 'flush_comm_broadcast_queue'):
-                sp.flush_gcs_queue()
+                sp.flush_comm_broadcast_queue()
 
         await asyncio.sleep(current_guard_ms / 1000.0)