tmpCommit1

src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py

@@ -29,7 +29,7 @@ from .utils import acquireSerial, acquirePort
 from .utils.acquirePort import find_available_port
 
 logger = setup_logger(os.path.basename(__file__))
-PROJECT_VER = "v1.20"
+PROJECT_VER = "v1.25"
 
 class PanelState:
     def __init__(self):
@@ -1696,4 +1696,8 @@ if __name__ == "__main__":
 2025.05.28
 1. 工程模式 新增 Node 重啟的功能
 
+2026.08.05 (ver 1.25)
+1. 增加對於 esp32 連線模式的支援
+
+
 '''

src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py

@@ -277,11 +277,12 @@ class RFModule:
         self.status = RFStatus()
         self.socket_info = SocketInfo()
 
-        self.poll_data_times: deque[int] = deque([0] * N,maxlen=N) # ms
-        self.poll_data_sizes: deque[int] = deque([0] * N, maxlen=N) # num
-        self.poll_total_time = 0 # ms
-        self.poll_total_size = 0 # num
-        self.remain_bytes = 0 # num
+        # # only for ESP32
+        # self.poll_data_times: deque[int] = None
+        # self.poll_data_sizes: deque[int] = None
+        # self.poll_total_time = None
+        # self.poll_total_size = None
+        # self.remain_bytes = None
 
     def update_avg_bandwidth(self, bandwidth: float, timestamp: Optional[float] = None) -> None:
         self.status.avg_bandwidth = bandwidth

src/fc_network_adapter/fc_network_adapter/serialManager.py

@@ -81,6 +81,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:
@@ -101,13 +111,33 @@ class FrameProcessor(ABC):
 
 class RawFrameProcessor(FrameProcessor):
     """原始數據直通處理器"""
-    
+
     def process_incoming(self, data: bytes) -> bytes:
         """直接返回原始數據，不進行緩衝"""
+        
+        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)
+
         return [data] if data else []
     
     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)
+            self.tx_interval_rate.append(self.tx_record_byte/interval)
+            self.rx_record_time = time.time()
+            self.rx_record_byte = 0
+        else:
+            self.rx_record_byte += len(data)
+
         return data
 
 
@@ -158,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 = []
 
@@ -173,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,
@@ -300,7 +350,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
 
     MAX_BYTES_PER_FLUSH = 150
     MAX_PAYLOAD_PER_FRAME = 80
-    CHUNK_SEND_INTERVAL_SEC = 0.005
+    CHUNK_SEND_INTERVAL_SEC = 0.020
 
 
 
@@ -316,6 +366,10 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
             self.received_len = 0 # 收到封包累計
 
             self.poll_miss_count = 0 # poll 封包連續丟失的累計數
+            self.esp_state = 1  # 紀錄每個 esp 連線狀態
+                                # 1 : 物件剛創建 剛接到 HELO 但是還沒有在 mavlinkVehicleView 建立物件
+                                # 2 : 已經擁有 mavlinkVehicleView 物件
+                                # 9 : 連續的 poll failure 視為斷線
 
     def __init__(self, at_handler: "ATCommandHandler" = None):
         super().__init__(at_handler)
@@ -586,17 +640,6 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
                 system_id, sender_address_64, time.time()
             )
   
-            vehicle = vehicle_registry.get(system_id)
-            if vehicle and not vehicle.rf_module:
-                N = 10
-                vehicle.rf_module = RFModule(RFModuleType.XBEE_ESP)
-                vehicle.rf_module.poll_data_times: deque[int] = deque([0] * N,maxlen=N) # ms
-                vehicle.rf_module.poll_data_sizes: deque[int] = deque([0] * N, maxlen=N) # num
-                vehicle.rf_module.poll_total_time = 0 # ms
-                vehicle.rf_module.poll_total_size = 0 # num
-                vehicle.rf_module.remain_bytes = 0 # num
-                logger.debug(f"mark A")
-
             logger.debug(
                 f"new HELO system_id={system_id}, address_64={sender_address_64.hex()}")
         else:
@@ -640,14 +683,14 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
 
         # logger.debug(f"Done!  payload size:{remote_device.received_len} / time(ms):{transmit_time:.2f}")
 
-        # if vehicle and not vehicle.rf_module:
-        #     vehicle.rf_module.poll_total_size -= vehicle.rf_module.poll_data_sizes[0]
-        #     vehicle.rf_module.poll_total_size += remote_device.received_len
-        #     vehicle.rf_module.poll_total_time -= vehicle.rf_module.poll_data_times[0]
-        #     vehicle.rf_module.poll_total_time += transmit_time
-        #     vehicle.rf_module.poll_data_sizes.append(remote_device.received_len)
-        #     vehicle.rf_module.poll_data_times.append(transmit_time)
-        #     logger.debug(f"{vehicle.rf_module.poll_total_size}")
+        if vehicle and not vehicle.rf_module:
+            vehicle.rf_module.poll_total_size -= vehicle.rf_module.poll_data_sizes[0]
+            vehicle.rf_module.poll_total_size += remote_device.received_len
+            vehicle.rf_module.poll_total_time -= vehicle.rf_module.poll_data_times[0]
+            vehicle.rf_module.poll_total_time += transmit_time
+            vehicle.rf_module.poll_data_sizes.append(remote_device.received_len)
+            vehicle.rf_module.poll_data_times.append(transmit_time)
+            logger.debug(f"{vehicle.rf_module.poll_total_size}")
 
         remote_device.received_len = 0
         remote_device.last_done_time = time.time()
@@ -818,17 +861,33 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
 
     # 主要迴圈 Main Loop
     async def _operator_tick(self) -> None:
-        # 1. 更新資訊
-        # for src64, INFO in self.esp32_address_mapping.items():
-        #     vehicle = vehicle_registry.get(INFO.system_id)
-        #     if vehicle and vehicle.rf_module:
+        # 1. 更新資訊 對於每個 ESP 裝置的資訊更新
+        for src64, INFO in self.esp32_address_mapping.items():
 
-        #         speed = vehicle.rf_module.poll_total_size / (vehicle.rf_module.poll_total_time + 1)
+            vehicle = vehicle_registry.get(INFO.system_id)
+
+            # 初始化 mavlinkVehicleView
+            if vehicle and INFO.esp_state == 1 :
+                N = 10
+                vehicle.rf_module = RFModule(RFModuleType.XBEE_ESP)
+                vehicle.rf_module.poll_data_times: deque[int] = deque([0] * N,maxlen=N) # ms
+                vehicle.rf_module.poll_data_sizes: deque[int] = deque([0] * N, maxlen=N) # num
+                vehicle.rf_module.poll_total_time = 0 # ms
+                vehicle.rf_module.poll_total_size = 0 # num
+                vehicle.rf_module.remain_bytes = 0 # num
+                vehicle.rf_module.rssi = 0 # db
+                INFO.esp_state = 2
+
+            elif vehicle and INFO.esp_state == 2:
+                vehicle.rf_module.remain_bytes = INFO.remain_bytes
+                # vehicle.rf_module.rssi = something # TODO 
+
+                pass
+
+                # speed = vehicle.rf_module.poll_total_size / (vehicle.rf_module.poll_total_time + 1)
             
-        #         logger.debug(f"now speed: {speed:.2f} / total_size: {vehicle.rf_module.poll_total_size}")
+                # logger.debug(f"now speed: {speed:.2f} / total_size: {vehicle.rf_module.poll_total_size}")
         
-
-
         # 2. 移除沒反應 dongle
         self._prune_stale_devices()
 
@@ -1013,8 +1072,9 @@ class ATCommandHandler:
             b'SL': self._handle_serial_low,
             # 可擴展其他 AT 指令
         }
+        self.at_response_buffer = deque(maxlen=N) # dict {topic:value}
 
-    # ---- 發送端 ----  # 這個不應該出現在這 越權了
+    # ---- 發送端 ----  # 這個不應該出現在這 越權了?
     def set_writer(self, writer):
         self.writer = writer
 
@@ -1104,8 +1164,6 @@ class ATCommandHandler:
     def _handle_rssi(self, response: ATResponse):
         """處理 DB (RSSI) 回應：單 byte 無號值，單位 dBm"""
 
-        ack_time = (time.time() - self.send_command_time)*1000 # for dev
-        # logger.debug(f"{ack_time:.2f}") # for dev
         logger.debug(f"[{self.serial_port}] RSSI = -{response.data[0]} dBm / AT ack time:{ack_time}") # dev
 
     def _handle_serial_high(self, response: ATResponse):
@@ -1591,25 +1649,31 @@ if __name__ == '__main__':
 
     serial_id = 1
     processor = sm.get_espv1_processor(serial_id)
-    if processor is not None:
-        # for x in range(10):
-        #     processor.request_discovery()
-        #     time.sleep(1)
-        #     print(f"mark N{x}")
-        # processor.request_poll(target_system_id=1)
-        # processor.request_poll(target_system_id=1, grant_bytes=200)
-        print(processor.get_status_snapshot())
-        print(processor.get_gcs_queue_byte_count())
+    if processor is None:
+        sys.exit(1)
+
+    # for x in range(10):
+    #     processor.request_discovery()
+    #     time.sleep(1)
+    #     print(f"mark N{x}")
+    # processor.request_poll(target_system_id=1)
+    # processor.request_poll(target_system_id=1, grant_bytes=200)
+    print(processor.get_status_snapshot())
+    print(processor.get_gcs_queue_byte_count())
 
     # rssi_request = ATRequest(command=b'DB', parameter=b'', frame_id=0xFE)
     # for i in range(3):
-    #     processor = sm.get_espv1_processor(serial_id)
-    #     if processor is not None:
-    #         deadline = time.time() + 5.0  # 最多等 5 秒，避免永遠卡住
-    #         while processor.operator_busy and time.time() < deadline:
-    #             time.sleep(0.01)
-    #     sm.send_at_command(1, rssi_request)
-    #     time.sleep(1)
+        # deadline = time.time() + 5.0  # 最多等 5 秒，避免永遠卡住
+        # while processor.operator_busy and time.time() < deadline:
+            # time.sleep(0.01)
+            # sm.send_at_command(1, rssi_request)
+            # time.sleep(5)
+
+    for i in range(20):
+        print(f"received: {processor.tx_interval_bytes}")
+        print(f"remain: {processor.remain_bytes}")
+        time.sleep(1)
+
 
     time.sleep(120)
     sm.remove_serial_link(serial_id)

src/unitdev02/unitdev02/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)
 
-    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))
+    await loop.create_datagram_endpoint(
+        lambda: udp_handler,
+        local_addr=('0.0.0.0', comm_listen_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)