tmpCommit

src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py

@@ -491,6 +491,10 @@ class ControlPanel:
                                 menu_stack.pop()
                                 idx_stack.pop()
                     
+                    elif selected.action == "DO_DISCOVERY_PANEL":
+                        if hasattr(selected, 'serial_id'):
+                            cmd_q.put(("DO_DISCOVERY", selected.serial_id))
+
                     # related with -> 列表選單
                     elif selected.action in ("PREV_PAGE", "NEXT_PAGE"):
                         if hasattr(selected, 'page'):
@@ -868,6 +872,7 @@ class ControlPanel:
                 obj_menu = MenuNode(f"Serial #{serial_id}", f"連結口 {serial_id}", None, children=[
                     MenuNode("Info", "查看詳細資訊", "INSPECT_LINKED_SERIAL"),
                     MenuNode("Remove", "移除此連結口", "REMOVE_LINKED_SERIAL"),
+                    MenuNode("Discovery", "搜尋遠端RF(Only XBEE-ESP32)", "DO_DISCOVERY_PANEL"),
                     # MenuNode("Change UDP Target", "變更目標 UDP (工程)", "CHANGE_LINKED_SERIAL_TARGET"),
                     MenuNode("GoUp", "回到列表", "BACK"),
                 ])
@@ -1319,6 +1324,11 @@ class Orchestrator:
                         elif action == "REMOVE_LINKED_SERIAL":
                             serial_id = cmd[1]
                             self.plumber.remove_serial_link(serial_id)
+                        elif action == "DO_DISCOVERY":
+                            serial_id = cmd[1]
+                            processor = self.plumber.get_espv1_processor(serial_id)
+                            if processor:
+                                processor.request_discovery()
 
                         elif action == "INSPECT_VEHICLE":
                             sysid, compid = cmd[1], cmd[2]

src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py

@@ -37,6 +37,7 @@ class ComponentType(Enum):
 class RFModuleType(Enum):
     """RF模組類型"""
     XBEE = "xbee"
+    XBEE_ESP = "xbee_esp"
     UDP = "udp"
     TCP = "tcp"
     OTHER = "other"
@@ -181,6 +182,7 @@ class RFStatus:
     at_response: Optional[str] = None  # AT 命令回應
     link_quality: Optional[int] = None  # 連接品質
     timestamp: Optional[float] = None  # 時間戳記
+    avg_bandwidth: Optional[float] = None # 量測到的傳輸速度 bytes/ms
     custom_status: Dict[str, Any] = field(default_factory=dict)  # 其他自定義狀態
 
 
@@ -275,6 +277,17 @@ 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
+
+    def update_avg_bandwidth(self, bandwidth: float, timestamp: Optional[float] = None) -> None:
+        self.status.avg_bandwidth = bandwidth
+        if timestamp:
+            self.status.timestamp = timestamp
+    
     def update_rssi(self, rssi: int, timestamp: Optional[float] = None) -> None:
         """更新RSSI"""
         self.status.rssi = rssi

src/fc_network_adapter/fc_network_adapter/serialManager.py

@@ -135,24 +135,25 @@ class XBeeFrameProcessor_Base(FrameProcessor):
     韌體版本 (Firmware Version): 2014 -> 2 (XBee 3 平台) + 0 (802.15.4 協議代碼) + 14 (次要版本號)。
     """
 
-    # XBee API frame type
-    FRAME_TYPE_TX_REQUEST  = 0x10
-    FRAME_TYPE_AT_RESPONSE = 0x88
-    FRAME_TYPE_TX_STATUS   = 0x8B
-    FRAME_TYPE_RX_PACKET   = 0x90
+    # # XBee API frame type
+    # FRAME_TYPE_TX_REQUEST  = 0x10
+    # FRAME_TYPE_AT_RESPONSE = 0x88
+    # FRAME_TYPE_TX_STATUS   = 0x8B
+    # FRAME_TYPE_RX_PACKET   = 0x90
 
-    # ADDR16 選項
-    DEST_ADDR16_UNICAST = b'\xFF\xFE'
-    DEST_ADDR16_BRAODCAST = b'\xFF\xFF'
+    # # ADDR16 選項
+    # DEST_ADDR16_UNICAST = b'\xFF\xFE'
+    # DEST_ADDR16_BRAODCAST = b'\xFF\xFF'
 
-    DEST_ADDR64_BRAODCAST = b'\x00\x00\x00\x00\x00\x00\x00\x00' 
+    # DEST_ADDR64_BRAODCAST = b'\x00\x00\x00\x00\x00\x00\x00\x00' 
     
 
     def __init__(self, at_handler: "ATCommandHandler" = None):
         super().__init__()
         self.at_handler = at_handler
-
-
+        self.DEST_ADDR16_UNICAST = b'\xFF\xFE'
+        self.DEST_ADDR16_BRAODCAST = b'\xFF\xFF'
+        self.DEST_ADDR64_BRAODCAST = b'\x00\x00\x00\x00\x00\x00\x00\x00' 
 
     # ---- 對外契約 ----
     def process_incoming(self, data: bytes) -> bytes:
@@ -172,7 +173,11 @@ class XBeeFrameProcessor_Base(FrameProcessor):
 
     def process_outgoing(self, data: bytes) -> bytes:
         """將數據封裝為 XBee API 傳輸幀"""
-        return self._encapsulate(data)
+        return self._encapsulate(
+            data,
+            dest_addr64 = self.DEST_ADDR64_BRAODCAST,
+            dest_addr16 = self.DEST_ADDR16_BRAODCAST,
+            frame_id = 0x01)
 
     # ---- 內部：拆幀與分派 ----
     def _try_extract_frame(self) -> bytes:
@@ -209,21 +214,21 @@ class XBeeFrameProcessor_Base(FrameProcessor):
         """
         frame_type = frame[3]
 
-        if frame_type == self.FRAME_TYPE_RX_PACKET: # mavlink
+        if frame_type == ATCommandHandler.FRAME_TYPE_RX_PACKET: # mavlink
             return self._decapsulate(frame)[0]
 
-        if frame_type == self.FRAME_TYPE_AT_RESPONSE: # AT command
+        if frame_type == ATCommandHandler.FRAME_TYPE_AT_RESPONSE: # AT command
             if self.at_handler is not None:
                 self.at_handler.handle_frame(frame)
             return None
 
-        if frame_type == self.FRAME_TYPE_TX_STATUS:
-            length = (frame[1] << 8) | frame[2] # for debug
-            logger.info(
-                f"TX Status raw={frame.hex()}, api_len={length}, "
-                f"fid=0x{frame[4]:02X}, dest16=0x{(frame[5]<<8)|frame[6]:04X}, "
-                f"retry={frame[7]}, delivery={frame[8]}, discovery={frame[9]}"
-            ) # for debug
+        if frame_type == ATCommandHandler.FRAME_TYPE_TX_STATUS:
+            # length = (frame[1] << 8) | frame[2] # for debug
+            # logger.info(
+            #     f"TX Status raw={frame.hex()}, api_len={length}, "
+            #     f"fid=0x{frame[4]:02X}, dest16=0x{(frame[5]<<8)|frame[6]:04X}, "
+            #     f"retry={frame[7]}, delivery={frame[8]}, discovery={frame[9]}"
+            # ) # for debug
             return None
 
         logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}")
@@ -233,18 +238,18 @@ class XBeeFrameProcessor_Base(FrameProcessor):
     @staticmethod
     def _encapsulate(
         data: bytes,
-        dest_addr64: bytes = DEST_ADDR64_BRAODCAST,
-        dest_addr16: bytes = DEST_ADDR16_BRAODCAST,
-        frame_id: int = 0x01,
+        dest_addr64: bytes,
+        dest_addr16: bytes,
+        frame_id: int = 0x00,
+        options: int = 0x00,
     ) -> bytes:
         """
         將 payload 包成 XBee TX Request (0x10)
         - 使用廣播地址
         - 添加適當的頭部和校驗和
         """
-        frame_type = XBeeFrameProcessor_Base.FRAME_TYPE_TX_REQUEST
+        frame_type = ATCommandHandler.FRAME_TYPE_TX_REQUEST
         broadcast_radius = 0x00
-        options = 0x00
 
         frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id)
         frame += dest_addr64 + dest_addr16
@@ -279,6 +284,8 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
 
     這邊認定 esp_sysid 會等於 mavlink sysid
 
+    poll time = 0.2305 * grant_bytes + 58.094 (R平方為0.979 高線性相關)
+
     硬體產品系列 (Product Family) : XBP9B-DM
     晶片世代: XBee PRO 900HP 200K
     運作頻段: 900 MHz RF。
@@ -293,7 +300,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
 
     MAX_BYTES_PER_FLUSH = 150
     MAX_PAYLOAD_PER_FRAME = 80
-    CHUNK_SEND_INTERVAL_SEC = 0.01
+    CHUNK_SEND_INTERVAL_SEC = 0.005
 
 
 
@@ -304,18 +311,18 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
             self.last_hello_time = last_hello_time
 
             self.remain_bytes = 0 # 剩餘 buffer 量
-            self.last_poll_time = 0.0
-            self.last_done_time = 0.0 # 最後送出Done的時間
+            self.last_poll_time = 0.0 # 最後送出Done的時間 用於判斷poll順序
+            self.last_done_time = 0.0 # 最後收到Done的時間 用於判斷離線
             self.received_len = 0 # 收到封包累計
 
-            
+            self.poll_miss_count = 0 # poll 封包連續丟失的累計數
 
     def __init__(self, at_handler: "ATCommandHandler" = None):
         super().__init__(at_handler)
 
-        self.max_discovery_window_ms = 220
-        self.is_discovery_phase = False
-        self.esp32_address_mapping = {}
+        self.max_discovery_window_ms = 0.22
+        self.is_discovery_phase = False # 只是 show 狀態 用不太到
+        self.esp32_address_mapping = {} # bytes[Addr64] : Esp32DeviceInfo
 
         self.operator_busy = False
         self.operator_running = False
@@ -328,24 +335,33 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         self.event_loop: Optional[asyncio.AbstractEventLoop] = None
         self.serial_baudrate = 115200
 
-        self.gcs_transmit_queue: deque[bytearray] = deque()
-        self.poll_scheduler_state = pollStrategy.PollSchedulerState()
+        self.udp_transmit_queue: deque[bytearray] = deque()
+        # self.poll_scheduler_state = pollStrategy.PollSchedulerState()
 
         self.command_pending_event: Optional[asyncio.Event] = None 
         self.poll_done_event: Optional[asyncio.Event] = None
         self.current_poll_address_64: Optional[bytes] = None
+        self.current_poll_missing: bool = False
+        self.current_poll_time = 0 # 本次 poll 丟出的時間紀錄 用來運算傳輸效能
 
         self.last_discovery_time = 0.0 # 這個是最後做廣播 discovery 的時間  
         self.last_recieve_mavlink = 0.0 # 這個是最後收到 mavlink payload 時間 為了定義 poll-done 之間不要超時用的
-        self.mavPack_interval_timeout = 150  # ms，poll 期間 MAVLink/DONE 最大閒置間隔
-        self.discovery_interval_seconds = 200.0 # 每次做 discovery 程序的間隔時間
+        self.max_time_tolerance_poll_interval = 150  # (msec) poll 期間 每筆 MAVLink 資料 /DONE 最大閒置間隔
+        self.discovery_interval_seconds = 200.0 # (sec) 每次做 discovery 程序的間隔時間
         self.device_offline_timeout = self.discovery_interval_seconds * 2 # 遠端沒有回應會被踢出 超時時限
-        self.operator_tick_interval_seconds = 0.03 # 
+        self.operator_tick_interval_seconds = 0.005 # 
         self.guard_milliseconds = 50 # POLL DONE 的保底時間間隔
 
         self.pending_manual_discovery = False
         self.pending_manual_poll: Optional[tuple[int, Optional[int]]] = None
         
+        self.rssi_request = ATRequest(command=b'DB', parameter=b'', frame_id=0xFE)
+
+        self._pending_tx_count = 0
+        self._last_tx_sent_time = 0.0
+        self._next_gcs_frame_id = 0x10
+        self._rf_busy_timeout_sec = 2
+
     # ---- 注入與設定 ----
 
     def set_writer(self, writer: Callable[[bytes], None]) -> None:
@@ -359,6 +375,13 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         self.serial_baudrate = baudrate
 
     def _ensure_async_primitives(self) -> None:
+        """ 
+        因為 FrameProcessor 被歸屬在 SerialHandler 裡面 
+        SerialHandler 物件在初始化時 是在 main thread 
+
+        然而 以下兩個 Event 物件是在分支 thread 運作的
+        所以這裡不能放到 __init__ 中
+        """
         if self.command_pending_event is None:
             self.command_pending_event = asyncio.Event()
         if self.poll_done_event is None:
@@ -369,12 +392,144 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         self._ensure_async_primitives()
         self.command_pending_event.set()
 
+    # ---- 對外契約 ----
+
+    def process_outgoing(self, data: bytes) -> bytes:
+        """這裡不在用 Base 的方式 直接送出 而是先暫存到一處"""
+        self.enqueue_serial_transmit(data)
+        # self.wake_operator()
+        return b''  
+
+    # ---- UDP 到 Serial 佇列 ----
+
+    # 把 UDP 的資料先放到 udp_transmit_queue 
+    def enqueue_serial_transmit(self, payload: bytes) -> None:
+        if not payload:
+            return
+        self.udp_transmit_queue.append(bytearray(payload))
+
+    # 只是 show 狀態
+    def get_gcs_queue_byte_count(self) -> int:
+        return sum(len(packet) for packet in self.udp_transmit_queue)
+
+    # 只是 show 狀態
+    def get_gcs_queue_packet_count(self) -> int:
+        return len(self.udp_transmit_queue)
+
+    # ---- UDP 佇列透過 RF 模組 TX  ----
+
+    # 判斷RF模組是不是已經完成送出
+    @property
+    def rf_busy(self) -> bool:
+        if self._pending_tx_count > 0:
+            if time.time() - self._last_tx_sent_time > self._rf_busy_timeout_sec:
+                logger.warning("RF busy timeout, force idle")
+                self._pending_tx_count = 0
+            return self._pending_tx_count > 0
+        return False
+
+    # 動態分配 frame_id 從 0x10~0xF0
+    def _alloc_tx_frame_id(self) -> int:
+        frame_id = self._next_gcs_frame_id
+        self._next_gcs_frame_id += 1
+        if self._next_gcs_frame_id > 0xF0:
+            self._next_gcs_frame_id = 0x10
+        return frame_id
+
+    # def _notify_tx_sent(self, frame_id: int) -> None:
+    #     if frame_id == 0x00:
+    #         return
+    #     self._pending_tx_count += 1
+    #     self._last_tx_sent_time = time.time()
+
+    def _handle_tx_status(self, frame: bytes) -> None:
+        if len(frame) < 9 :
+            return
+        frame_id = frame[4]
+        delivery = frame[8]
+
+        # logger.debug(f"mark C {self._pending_tx_count}")
+        if frame_id == 0x02: # poll pack 的回傳處理
+            # 若封包丟失 則設立旗標並停下等待迴圈
+            if delivery != 0x00:
+                self.current_poll_missing = True
+                self.poll_done_event.set()
+
+        elif (frame_id >= 0x10) and (frame_id > 0xF0): # UDP 上傳到 RF 的處理
+            if self._pending_tx_count > 0:
+                self._pending_tx_count -= 1
+                logger.debug(f"send Completed !")
+        
+        if delivery != 0x00:
+            logger.warning( f"TX failed fid=0x{frame[4]:02X} delivery=0x{delivery:02X}" )
+
+    # 把 udp_transmit_queue 的 mavlink 封包依照大小打包出來
+    def _pop_flush_batch(self, max_bytes: int) -> List[bytes]:
+        if not self.udp_transmit_queue:
+            return []
+
+        batch: List[bytes] = []
+        total_bytes = 0
+
+        while self.udp_transmit_queue:
+            next_packet = bytes(self.udp_transmit_queue[0])
+            packet_length = len(next_packet)
+
+            # batch 沒東西 就轉移一筆 udp_transmit_queue 的資料進去
+            # 如果有就算一下這筆加進去會不會超出 
+            if (not batch) or (total_bytes + packet_length <= max_bytes):
+                batch.append(self.udp_transmit_queue.popleft())
+                total_bytes += packet_length
+            else:
+                break
+
+        return batch
+
+    # # 把數據封裝好以後 交給 serial_writer 排程送出
+    # async def _send_udp_packet(self, packet: bytes) -> None:
+    #     sent_offset = 0
+    #     while sent_offset < len(packet):
+    #         chunk_end = min(
+    #             sent_offset + self.MAX_PAYLOAD_PER_FRAME,
+    #             len(packet),
+    #         )
+    #         chunk = packet[sent_offset:chunk_end]
+    #         sent_offset = chunk_end
+    #         frame_id = self._alloc_tx_frame_id()
+    #         self.serial_writer(
+    #             self._encapsulate(
+    #                 chunk, 
+    #                 dest_addr16 = self.DEST_ADDR16_BRAODCAST, 
+    #                 dest_addr64 = self.DEST_ADDR64_BRAODCAST,
+    #                 frame_id=frame_id))
+    #         self._notify_tx_sent(frame_id)
+    #         await asyncio.sleep(self.CHUNK_SEND_INTERVAL_SEC)
+
+    # 處理從 UDP 來的資訊 丟給 Serial
+    async def flush_udp_transmit_queue(self) -> None:
+        if self.serial_writer is None:
+            logger.warning("GCS flush skipped: serial writer not ready")
+            return
+
+        for packet in self._pop_flush_batch(self.MAX_BYTES_PER_FLUSH):
+            # await self._send_udp_packet(packet)
+            frame_id = self._alloc_tx_frame_id()
+            self.serial_writer(
+                self._encapsulate(
+                    packet, 
+                    dest_addr16 = self.DEST_ADDR16_BRAODCAST, 
+                    dest_addr64 = self.DEST_ADDR64_BRAODCAST,
+                    frame_id=frame_id))
+            # self._notify_tx_sent(frame_id)
+            self._pending_tx_count += 1
+            self._last_tx_sent_time = time.time()
+
     # ---- 拆幀分派 ----
 
     def _dispatch_frame(self, frame: bytes) -> Optional[bytes]:
         frame_type = frame[3]
 
-        if frame_type == self.FRAME_TYPE_RX_PACKET:
+        if frame_type == ATCommandHandler.FRAME_TYPE_RX_PACKET:
             payload, sender_address_64 = self._decapsulate(frame)
 
             if payload.startswith(self.HELLO_HEADER) and len(payload) == 5:
@@ -392,66 +547,83 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
                     self.last_recieve_mavlink = time.time()
             return payload
 
-        if frame_type == self.FRAME_TYPE_AT_RESPONSE:
+        if frame_type == ATCommandHandler.FRAME_TYPE_AT_RESPONSE:
             if self.at_handler is not None:
                 self.at_handler.handle_frame(frame)
             return None
 
-        if frame_type == self.FRAME_TYPE_TX_STATUS:
-            # length = (frame[1] << 8) | frame[2]
-            # logger.debug(
+        if frame_type == ATCommandHandler.FRAME_TYPE_TX_STATUS:
+            self._handle_tx_status(frame)
+            # length = (frame[1] << 8) | frame[2] # for debug
+            # logger.info(
             #     f"TX Status raw={frame.hex()}, api_len={length}, "
             #     f"fid=0x{frame[4]:02X}, dest16=0x{(frame[5]<<8)|frame[6]:04X}, "
             #     f"retry={frame[7]}, delivery={frame[8]}, discovery={frame[9]}"
-            # )
+            # ) # for debug
             return None
 
         logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}")
         return None
 
-    # ---- DISC / POLL 封裝 ----
+    # ---- DISC 封裝 / HELLO 處理 ----
 
     def pack_discovery(self) -> bytes:
-        # logger.debug(f"pack discovery")
-        return self._encapsulate(self.DISC_HEADER, dest_addr16 = self.DEST_ADDR16_BRAODCAST, dest_addr64 = self.DEST_ADDR64_BRAODCAST , frame_id=0x00)
+        logger.debug(f"pack discovery")
+        return self._encapsulate(
+            self.DISC_HEADER, 
+            dest_addr16 = self.DEST_ADDR16_BRAODCAST, 
+            dest_addr64 = self.DEST_ADDR64_BRAODCAST, 
+            frame_id=0x05)
 
     # 處理每個裝置回傳的 Hello 訊息
     def handle_hello_report(self, payload: bytes, sender_address_64: bytes) -> None:   
         system_id = payload[4]
         remote_device = self.esp32_address_mapping.get(sender_address_64)
 
+        # 若無則新建
         if remote_device is None:
             self.esp32_address_mapping[sender_address_64] = self.Esp32DeviceInfo(
                 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()}")
-        elif remote_device.address_64 == sender_address_64:
-            remote_device.last_hello_time = time.time()
         else:
-            logger.warning(
-                f"SYSID duplicated system_id={system_id}, "
-                f"address_64={sender_address_64.hex()}"
-            )
+            remote_device.last_hello_time = time.time()
 
-        if vehicle:=vehicle_registry.get(system_id):
-            if not vehicle.rf_module:
-                vehicle.rf_module = RFModule(RFModuleType.XBEE)
+        # TODO 檢驗重複 sysid 
+        # elif remote_device.system_id != system_id :
+        #     logger.warning(
+        #         f"SYSID duplicated system_id={system_id}, "
+        #         f"address_64={sender_address_64.hex()}"
+        #     )
 
-    def pack_poll(self, target_address_64: bytes, grant_bytes: int = 0) -> Optional[bytes]:
-        remote_device = self.esp32_address_mapping.get(target_address_64)
-        if remote_device is None:
-            return None
 
+    # ---- POLL 封裝 / DONE 處理  ----
+
+    def pack_poll(self, remote_device, grant_bytes: int = 0) -> Optional[bytes]:
+        # logger.debug(f"pack poll {remote_device.address_64.hex()} / {grant_bytes}")
         poll_payload = self.POLL_HEADER + struct.pack(
             '>BH', remote_device.system_id, grant_bytes
         )
-        remote_device.received_len = 0
+        
         return self._encapsulate(
             poll_payload,
             dest_addr64=remote_device.address_64,
             dest_addr16=self.DEST_ADDR16_UNICAST,
             frame_id=0x02,
+            options=0x01 # xbee auto retry
         )
 
     def handle_done_report(self, payload: bytes, sender_address_64: bytes) -> None:
@@ -459,21 +631,27 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         if remote_device is None:
             return
 
-
         system_id, sent_length, remain_length = struct.unpack('>BHH', payload[4:9])
         remote_device.remain_bytes = remain_length
 
-        # 這段是有問題的 因為會有整數封包切割問題 以及載具端的 buffer 存量不足 故回傳的資訊量會與要求的不一致
-        # if sent_length != remote_device.received_len:
-        #     logger.info(
-        #         f"POLL may be missing packets sent={sent_length} "
-        #         f"received={remote_device.received_len} system_id={system_id}"
-        #     )
+        # 紀錄傳輸狀況 以利後續算速率 
+        transmit_time = int((time.time() - self.current_poll_time)*1000) # ms
+        vehicle = vehicle_registry.get(system_id)
+
+        # 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}")
 
-        # TODO 傳送速率
-        # TODO 累積速率預測
         remote_device.received_len = 0
         remote_device.last_done_time = time.time()
+        remote_device.last_poll_time = self.current_poll_time
 
         if (
             self.current_poll_address_64 is not None
@@ -482,77 +660,6 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         ):
             self.poll_done_event.set()
 
-    # ---- UDP到Serial 佇列 ----
-
-    def enqueue_gcs_transmit(self, payload: bytes) -> None:
-        if not payload:
-            return
-        self.gcs_transmit_queue.append(bytearray(payload))
-
-    # 只是 show 狀態
-    def get_gcs_queue_byte_count(self) -> int:
-        return sum(len(packet) for packet in self.gcs_transmit_queue)
-
-    # 只是 show 狀態
-    def get_gcs_queue_packet_count(self) -> int:
-        return len(self.gcs_transmit_queue)
-
-    # 把 gcs_transmit_queue 的 mavlink 封包依照大小打包出來
-    def _pop_flush_batch(self, max_bytes: int) -> List[bytes]:
-        if not self.gcs_transmit_queue:
-            return []
-
-        batch: List[bytes] = []
-        total_bytes = 0
-
-        while self.gcs_transmit_queue:
-            next_packet = bytes(self.gcs_transmit_queue[0])
-            packet_length = len(next_packet)
-
-            if not batch:
-                batch.append(self.gcs_transmit_queue.popleft())
-                total_bytes = packet_length
-                if packet_length > max_bytes:
-                    break
-                continue
-
-            if total_bytes + packet_length <= max_bytes:
-                batch.append(self.gcs_transmit_queue.popleft())
-                total_bytes += packet_length
-            else:
-                break
-
-        return batch
-
-    # 把數據封裝好以後 交給 serial_writer 排程送出
-    async def _send_gcs_packet(self, packet: bytes) -> None:
-        # 重複判斷
-        # if self.serial_writer is None:
-        #     return
-
-        sent_offset = 0
-        while sent_offset < len(packet):
-            chunk_end = min(
-                sent_offset + self.MAX_PAYLOAD_PER_FRAME,
-                len(packet),
-            )
-            chunk = packet[sent_offset:chunk_end]
-            sent_offset = chunk_end
-            self.serial_writer(self._encapsulate(chunk))
-            await asyncio.sleep(self.CHUNK_SEND_INTERVAL_SEC)
-
-    # 把上面兩個步驟打包起來 處理從 UDP 來的資訊 丟給 Serial
-    async def flush_gcs_transmit_queue(
-        self,
-        max_bytes: int = MAX_BYTES_PER_FLUSH,
-    ) -> None:
-        if self.serial_writer is None:
-            logger.warning("GCS flush skipped: serial writer not ready")
-            return
-
-        for packet in self._pop_flush_batch(max_bytes):
-            await self._send_gcs_packet(packet)
-
     # ---- POLL 排程輔助 ----
 
     # 計算下次要 poll 的對象跟大小
@@ -560,17 +667,13 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         poll_devices = [
             pollStrategy.PollDevice(
                 address_64=address_64,
-                # system_id=device.system_id,
                 remain_bytes=device.remain_bytes,
-                last_done_time=device.last_done_time,
+                last_poll_time=device.last_poll_time,
             )
             for address_64, device in self.esp32_address_mapping.items()
         ]
 
-        return pollStrategy.pick_next(
-            poll_devices,
-            self.poll_scheduler_state
-        )
+        return pollStrategy.pick_next(poll_devices, time.time())
 
     # 移除長時間沒有 HELLO 或 DONE 的 Dongle
     def _prune_stale_devices(self) -> None:
@@ -593,7 +696,8 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
     def _should_run_discovery(self) -> bool:
         # 條件1. 目前沒有任何遠端ESP裝置被紀錄 或者 手動啟動
         if (not self.esp32_address_mapping) or (self.pending_manual_discovery):
-            return (time.time() - self.last_discovery_time) > 2
+            # logger.debug(f"mark D : {(time.time() - self.last_discovery_time) > (1 + 5 * self.max_discovery_window_ms)}")
+            return (time.time() - self.last_discovery_time) > (1 + 5 * self.max_discovery_window_ms)
         # 條件2. 每個固定週期 會做一次
         return (time.time() - self.last_discovery_time) > self.discovery_interval_seconds
 
@@ -610,7 +714,8 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         if not self.operator_running:
             logger.warning("ESPv1 request_discovery: operator not running")
             return False
-        self.event_loop.call_soon_threadsafe(self._apply_request_discovery)
+        # self.event_loop.call_soon_threadsafe(self._apply_request_discovery) # [del function] 每個 tick 間隔很短 不想要搞這麼複雜
+        self.pending_manual_discovery = True
         return True
 
     def request_poll(
@@ -628,24 +733,25 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         if not self.operator_running:
             logger.warning("ESPv1 request_poll: operator not running")
             return False
-        self.event_loop.call_soon_threadsafe(
-            self._apply_request_poll,
-            target_system_id,
-            grant_bytes,
-        )
+        # self.event_loop.call_soon_threadsafe(
+        #     self._apply_request_poll,
+        #     target_system_id,
+        #     grant_bytes,
+        # ) # [del function] 每個 tick 間隔很短 不想要搞這麼複雜
+        self.pending_manual_poll = (target_system_id, grant_bytes)
         return True
 
-    def _apply_request_discovery(self) -> None:
-        self.pending_manual_discovery = True
-        self.wake_operator()
+    # def _apply_request_discovery(self) -> None: # [del function] 每個 tick 間隔很短 不想要搞這麼複雜
+    #     self.pending_manual_discovery = True
+    #     self.wake_operator()
 
-    def _apply_request_poll(
-        self,
-        target_system_id: int,
-        grant_bytes: Optional[int],
-    ) -> None:
-        self.pending_manual_poll = (target_system_id, grant_bytes)
-        self.wake_operator()
+    # def _apply_request_poll(
+    #     self,
+    #     target_system_id: int,
+    #     grant_bytes: Optional[int],
+    # ) -> None: # [del function] 每個 tick 間隔很短 不想要搞這麼複雜
+    #     self.pending_manual_poll = (target_system_id, grant_bytes)
+    #     self.wake_operator()
 
     def get_status_snapshot(self) -> dict:
         """
@@ -666,6 +772,8 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
             })
         return {
             "operator_busy": self.operator_busy,
+            "rf_busy": self.rf_busy,
+            "pending_tx_count": self._pending_tx_count,
             "is_discovery_phase": self.is_discovery_phase,
             "gcs_queue_bytes": self.get_gcs_queue_byte_count(),
             "gcs_queue_packets": self.get_gcs_queue_packet_count(),
@@ -697,7 +805,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
 
         # 固定時間醒來
         sleep_task = asyncio.create_task( asyncio.sleep(self.operator_tick_interval_seconds) )
-        # 有"急事"被叫醒
+        # 有"急事"被叫醒 : 程序中斷時
         wake_task = asyncio.create_task(self.command_pending_event.wait())
 
         done, pending = await asyncio.wait(
@@ -708,25 +816,41 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         for task in pending:
             task.cancel()
 
+    # 主要迴圈 Main Loop
     async def _operator_tick(self) -> None:
-        # 1. 移除沒反應 dongle
+        # 1. 更新資訊
+        # for src64, INFO in self.esp32_address_mapping.items():
+        #     vehicle = vehicle_registry.get(INFO.system_id)
+        #     if vehicle and vehicle.rf_module:
+
+        #         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}")
+        
+
+
+        # 2. 移除沒反應 dongle
         self._prune_stale_devices()
 
-        # 2. 忙碌時 略過這次循環
+        # 3. 忙碌時 略過這次循環
         if self.operator_busy:
             return
 
-        # 3. (最優先) 處理從 UDP 過來的封包 並且透過 Serial 送出
-        if self.gcs_transmit_queue:
-            await self.flush_gcs_transmit_queue()
+        # 4. (最優先) 處理從 UDP 過來的封包 並且透過 Serial 送出
+        if self.udp_transmit_queue:
+            await self.flush_udp_transmit_queue()
             return
 
-        # 4. 檢測要不要跑 discovery 程序
+        # 5. RF 仍在傳輸，略過 discovery / poll
+        if self.rf_busy:
+            return
+
+        # 6. 檢測要不要跑 discovery 程序
         if self._should_run_discovery():
             await self._run_discovery()
             return
 
-        # 5. POLL 程序 (若有手動先執行 若無自動則策略決策)
+        # 7. POLL 程序 (若有手動先執行 若無自動則策略決策)
         if self.pending_manual_poll is not None:
             target_system_id, grant_bytes = self.pending_manual_poll
             self.pending_manual_poll = None
@@ -738,6 +862,8 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
             return
         await self._run_one_poll(target_address, grant_bytes if grant_bytes is not None else 0)
 
+        # self.at_handler.send_command(self.rssi_request) # 送出 RSSI 請求封包
+
     def _find_address_by_system_id(self, target_system_id: int) -> Optional[bytes]:
         for address_64, device in self.esp32_address_mapping.items():
             if device.system_id == target_system_id:
@@ -761,22 +887,22 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         self.is_discovery_phase = False
         self.pending_manual_discovery = False
 
-    async def _wait_poll_done_with_idle_timeout(self) -> bool:
-        idle_timeout_sec = self.mavPack_interval_timeout / 1000.0
-        poll_tick = min(0.02, idle_timeout_sec)
+    # async def _wait_poll_done_with_idle_timeout(self) -> bool:
+    #     idle_timeout_sec = self.max_time_tolerance_poll_interval / 1000.0
+    #     poll_tick = min(0.02, idle_timeout_sec)
 
-        while not self.poll_done_event.is_set():
-            if time.time() - self.last_recieve_mavlink >= idle_timeout_sec:
-                return False
-            # try:
-            await asyncio.wait_for(
-                self.poll_done_event.wait(),
-                timeout=poll_tick,
-            )
+    #     while not self.poll_done_event.is_set():
+    #         if time.time() - self.last_recieve_mavlink >= idle_timeout_sec:
+    #             return False
+    #         # try:
+    #         await asyncio.wait_for(
+    #             self.poll_done_event.wait(),
+    #             timeout=poll_tick,
+    #         )
             
-            # except asyncio.TimeoutError:
-                # continue
-        return True
+    #         # except asyncio.TimeoutError:
+    #             # continue
+    #     return True
 
     # poll 程序
     async def _run_one_poll(self, target_address_64: bytes, grant_bytes: int) -> None:
@@ -785,34 +911,72 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
 
         # 這邊把要求封包最小量放在36 是讓載具端正好可以回傳一個有簽章的 HEARTBEAT 封包的大小
         # 算是 "探測封包" 這樣讓系統可以更快速的知道載具端殘餘的資料量
-        grant_bytes = max(36, min(int(grant_bytes), 65535))
+        # 這個別定義在這邊了 讓 pollStategy 好好做自己的事情
+        # grant_bytes = max(36, min(int(grant_bytes), 65535))
 
-        poll_frame = self.pack_poll(target_address_64, grant_bytes)
-        if poll_frame is None:
-            return
+        remote_device = self.esp32_address_mapping.get(target_address_64)
+        if remote_device is None:
+            return None
+
+        # 打包 poll pack
+        poll_frame = self.pack_poll(remote_device, grant_bytes)
 
         self._ensure_async_primitives()
         self.operator_busy = True
+        remote_device.received_len = 0
         self.current_poll_address_64 = target_address_64
+        self.current_poll_time = time.time()
+        self.current_poll_missing = False
         self.poll_done_event.clear()
         self.serial_writer(poll_frame)
-        self.last_recieve_mavlink = time.time()
+        self.last_recieve_mavlink = time.time() - 0.025 # 第一筆故意給多一些等待時間
 
-        try:
-            completed = await self._wait_poll_done_with_idle_timeout()
-            if not completed:
-                logger.warning(
-                    f"POLL timeout address_64={target_address_64.hex()} "
-                    f"grant_bytes={grant_bytes}"
+        # 這邊會做傳送等待
+        completed = await self._wait_poll_done_with_idle_timeout()
+
+        # 這邊代表的是 poll 有觸發 也有收到資料 但是 done 沒有收到
+        if (not completed) and (not remote_device.received_len):
+            logger.debug(
+                f"POLL timeout address_64={target_address_64.hex()} "
+                f"grant_bytes={grant_bytes} "
+                f"get={remote_device.received_len}"
+            )
+
+            remote_device.remain_bytes = max(remote_device.remain_bytes - remote_device.received_len, 0)
+            remote_device.last_done_time = time.time()
+            remote_device.last_poll_time = self.current_poll_time
+
+        # 判斷是否 poll pack 丟失 並紀錄連續丟失 清空 remain_bytes
+        if self.current_poll_missing:
+            remote_device.poll_miss_count += 1
+            remote_device.remain_bytes = 0
+
+        self.operator_busy = False
+        self.current_poll_address_64 = None
+
+    # poll 程序中 只要目標的載具還有在傳資訊 就不會終止資料的接收
+    async def _wait_poll_done_with_idle_timeout(self) -> bool:
+        # 容許時間 
+        idle_timeout_sec = self.max_time_tolerance_poll_interval / 1000.0
+
+        # 每次檢查間隔
+        poll_tick = min(0.02, idle_timeout_sec / 2)
+
+        while not self.poll_done_event.is_set():
+            if time.time() - self.last_recieve_mavlink >= idle_timeout_sec:
+                return False
+            try:
+                await asyncio.wait_for(
+                    self.poll_done_event.wait(),
+                    timeout=poll_tick,
                 )
-        finally:
-            self.operator_busy = False
-            self.current_poll_address_64 = None
-            await asyncio.sleep(self.guard_milliseconds / 1000.0)
+            except asyncio.TimeoutError:
+                continue
+        return True
 
     def stop_operator(self) -> None:
         self.operator_running = False
-        self.gcs_transmit_queue.clear()
+        self.udp_transmit_queue.clear()
         self.wake_operator()
 
 
@@ -832,8 +996,15 @@ class ATCommandHandler:
     """
 
     FRAME_TYPE_AT_COMMAND = 0x08
+    FRAME_TYPE_AT_RESPONSE = 0x88
+
+    FRAME_TYPE_TX_REQUEST  = 0x10
+    FRAME_TYPE_TX_STATUS   = 0x8B
+
+    FRAME_TYPE_RX_PACKET   = 0x90
 
     def __init__(self, serial_port: str):
+        self.send_command_time = 0.0 # for dev
         self.serial_port = serial_port
         self.writer = None
         self.handlers = {
@@ -843,9 +1014,8 @@ class ATCommandHandler:
             # 可擴展其他 AT 指令
         }
 
-    # ---- 發送端 ----
+    # ---- 發送端 ----  # 這個不應該出現在這 越權了
     def set_writer(self, writer):
-        """由 SerialHandler 注入實際寫入 serial 的 callable"""
         self.writer = writer
 
     def send_command(self, request: ATRequest):
@@ -857,6 +1027,8 @@ class ATCommandHandler:
             return
 
         self.writer(self._build_at_request(request))
+
+        self.send_command_time = time.time()
         # logger.debug(
         #     f"[{self.serial_port}] send AT {request.command.decode(errors='replace')} "
         #     f"(frame_id=0x{request.frame_id:02X})"
@@ -884,10 +1056,12 @@ class ATCommandHandler:
         if parsed_at_ack is None:
             return
 
-        if not rx_module_ack.put(parsed_at_ack):
-            logger.warning(
-                f"[{self.serial_port}] rx_module_ack overflow, drop {parsed_at_ack.command!r}"
-            )
+        # TODO 本來是打算把 serialManager 的資訊傳給 mavlinkObject 去處理
+        # 但是可以考慮 直接寫到 mavlinkVehicleView 裡面
+        # if not rx_module_ack.put(parsed_at_ack):
+        #     logger.warning(
+        #         f"[{self.serial_port}] rx_module_ack overflow, drop {parsed_at_ack.command!r}"
+        #     )
 
         self._dispatch(parsed_at_ack)
 
@@ -930,11 +1104,9 @@ class ATCommandHandler:
     def _handle_rssi(self, response: ATResponse):
         """處理 DB (RSSI) 回應：單 byte 無號值，單位 dBm"""
 
-        # print(f"[{self.serial_port}] RSSI = -{data[0]} dBm")  # dev
-        logger.debug(f"[{self.serial_port}] RSSI = -{response.data[0]} dBm") # dev
-
-
-
+        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):
         """處理 SH (Serial Number High)"""
@@ -1031,15 +1203,9 @@ class UDPHandler(asyncio.DatagramProtocol):
             logger.warning("Serial handler not set, dropping UDP packet")
             return
 
-        if self.serial_mode == SerialMode.XBEEAPI_espv1:
-            processor = self.serial_handler.processor
-            # if isinstance(processor, XBeeFrameProcessor_ESPv1): # 預設唯一綁定 少一個 if 多一點效率
-            processor.enqueue_gcs_transmit(data)
-            processor.wake_operator()
-            return
-
         processed_data = self.serial_handler.processor.process_outgoing(data)
-        self.serial_handler.transport.write(processed_data)
+        if processed_data:
+            self.serial_handler.transport.write(processed_data)
 
 
 # ====================== Serial Manager =====================
@@ -1315,12 +1481,14 @@ class serial_manager:
             logger.error(f"Serial object {serial_id} not found")
             return False
 
+        
         # TODO 這邊的防呆 應該可以不用 if 有空再改
         serial_obj = self.serial_objects[serial_id]
-        if serial_obj.serial_mode != SerialMode.XBEEAPI2AT:
+        processor = serial_obj.serial_handler.processor
+        if not isinstance(processor, XBeeFrameProcessor_Base):
             logger.error(
                 f"Serial {serial_id} mode is {serial_obj.serial_mode.name}, "
-                f"AT command only supported in XBEEAPI2AT mode"
+                f"AT Command NOT supported yet!"
             )
             return False
 
@@ -1414,17 +1582,35 @@ if __name__ == '__main__':
     UDP_REMOTE_PORT = 14561
     sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI_espv1)
 
+    # test_sysid = 20
+    # vehicle = vehicle_registry.register(test_sysid)
+    # if vehicle.rf_module is None:
+    #     vehicle.rf_module = RFModule(RFModuleType.XBEE_ESP)
+
     time.sleep(2)  # 等 serial 連線與 operator 啟動
 
     serial_id = 1
     processor = sm.get_espv1_processor(serial_id)
     if processor is not None:
-        processor.request_discovery()
-        processor.request_poll(target_system_id=1)
-        processor.request_poll(target_system_id=1, grant_bytes=200)
+        # 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)
+
     time.sleep(120)
     sm.remove_serial_link(serial_id)
     sm.shutdown()

src/fc_network_adapter/fc_network_adapter/utils/pollStrategy.py

@@ -7,42 +7,54 @@ POLL 輪詢策略，供 XBeeFrameProcessor_ESPv1 使用。
 from dataclasses import dataclass
 from typing import List
 
+MIN_TRIGGER_INTERVAL_MS = 5
 
 @dataclass(frozen=True)
 class PollDevice:
     """從 esp32AddrMapping 抽出的唯讀快照。"""
     address_64: bytes
-    # system_id: int
     remain_bytes: int
-    last_done_time: float
+    last_poll_time: float
 
 
-@dataclass
-class PollSchedulerState:
-    """每條 serial link 一份，保存 round-robin 索引。"""
-    round_robin_index: int = 0
-
-
-def pick_next(
-    devices: List[PollDevice],
-    scheduler_state: PollSchedulerState
-):
+def pick_next(devices: List[PollDevice], now_time):
     """
     選下一個 POLL 目標。
 
     回傳 (target_address_64, grant_bytes)；devices 為空時回傳 (None, 0)。
+
+    poll time = 0.2305 * grant_bytes + 58.094 (R平方為0.979 高線性相關)  @-40db 
     """
     if not devices:
         return None, 0
 
-    device_count = len(devices)
-    selected_index = scheduler_state.round_robin_index % device_count
-    selected_device = devices[selected_index]
-    scheduler_state.round_robin_index += 1
-
     grant_bytes = 0
-    if selected_device.remain_bytes > 0:
-        grant_bytes = min( 65535, max(0, selected_device.remain_bytes))
+    device_count = len(devices)
+    weights = [0] * device_count
+
+    for i in range(device_count):
+        gap = int((now_time - devices[i].last_poll_time)*1000)
+        # # 時間大於觸發間隔 才會計算
+        # if gap >= MIN_TRIGGER_INTERVAL_MS:
+        #     weights[i] += devices[i].remain_bytes
+        #     weights[i] += gap
+
+        weights[i] += devices[i].remain_bytes
+        weights[i] += gap
+
+    maxarg = weights.index(max(weights))
+
+    if weights[maxarg] == 0:
+        return None, 0
+    
+    selected_device = devices[maxarg]
+
+    if selected_device.remain_bytes < 500:
+        grant_bytes = 170  # 這個大概是 100 ms 總傳輸時間
+    elif selected_device.remain_bytes < 1200:
+        grant_bytes = 610  # 這個大概是 200 ms 總傳輸時間
+    else:
+        grant_bytes = 1050 # 這個大概是 300 ms 總傳輸時間
 
     return selected_device.address_64, grant_bytes
 

src/unitdev02/unitdev02/devnote.txt

@@ -27,6 +27,7 @@
 python -m fc_network_adapter.tests.test_vehicleStatusPublisher
 python -m fc_network_adapter.tests.test_ringBuffer
 python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
+python -m fc_network_adapter.fc_network_adapter.serialManager
 
 python -m someotherpkg.src.example_takeoff_land
 python -m someotherpkg.src.example_change_mode

src/unitdev02/unitdev02/esp32.py

@@ -0,0 +1,641 @@
+from machine import UART
+import time
+import struct
+import gc
+
+# =========================================================
+# ESP32 / MicroPython：UAV 端 XBee <-> Flight Controller Bridge
+# Packet-size TDMA + DONE + SYSID / GCS address 自動學習版
+#
+# 本版已刪除「一般 DISC / 強制 DSCF」雙模式。
+# 現在只保留一種 discovery 封包，名稱統一叫 DISC。
+#
+# 重要：
+# - DISC 的功能等同於原本的 DSCF，也就是「強制 discovery」。
+# - UAV 收到 DISC 後，只要已知 MY_SYSID 與 DEST_64，就會重新回 HELO。
+# - 若收到 DISC 時 MY_SYSID 尚未學到，會設定 DISC_PENDING；
+#   之後一旦從飛控 MAVLink 學到 SYSID，就自動補送 HELO。
+#
+# 封包格式：
+# GCS -> UAV:
+#   DISC
+#   POLL + target_sysid(1) + grant_bytes(2)
+#
+# UAV -> GCS:
+#   HELO + sysid(1)
+#   DONE + sysid(1) + sent_len(2) + remain_len(2)
+#
+# 注意：
+# - HELO 不帶 remain。
+# - DONE 保留 remain。
+# - MY_SYSID 不寫死，由飛控 MAVLink 自動學。
+# - DEST_64 不寫死，由 GCS 封包的 XBee 0x90 src64 自動學。
+# =========================================================
+
+
+# ================= 設定區 =================
+FC_BAUDRATE = 115200
+XB_BAUDRATE = 115200
+
+DEST_64 = None
+MY_SYSID = None
+
+SYSID_LEARN_CONFIRM_COUNT = 3
+_sysid_candidate = None
+_sysid_candidate_count = 0
+
+# 若收到 DISC 時還沒學到 MY_SYSID，就先記住。
+# 等之後從 FC MAVLink 學到 MY_SYSID 後，自動補送 HELO。
+DISC_PENDING = False
+
+XBEE_MAX_PAYLOAD = 100
+
+# Discovery：只保留 DISC；功能等同原本 DSCF，收到後強制回 HELO。
+DISC_MAGIC = b'DISC'
+
+# UAV -> GCS hello：
+# HELO + sysid(1)
+HELLO_MAGIC = b'HELO'
+
+HELLO_MIN_INTERVAL_MS = 1000
+_last_hello_ms = 0
+
+# 保留此變數只作狀態記錄；本版 DISC 會 force=True，所以不會被 HELLO_SENT 擋掉。
+HELLO_SENT = False
+
+POLL_MAGIC = b'POLL'
+DEFAULT_GRANT_BYTES = 600
+
+DONE_MAGIC = b'DONE'
+
+MAX_BUF_SIZE = 6144
+READ_FC_BYTES = 250
+
+FC_UART_ID = 1
+FC_TX_PIN = 32
+FC_RX_PIN = 33
+
+XB_UART_ID = 2
+XB_TX_PIN = 25
+XB_RX_PIN = 26
+# =========================================
+
+
+uart_fc = UART(FC_UART_ID, baudrate=FC_BAUDRATE, tx=FC_TX_PIN, rx=FC_RX_PIN, rxbuf=4096)
+uart_xb = UART(XB_UART_ID, baudrate=XB_BAUDRATE, tx=XB_TX_PIN, rx=XB_RX_PIN, rxbuf=4096)
+
+tx_buf = bytearray()  # FC -> GCS 等待 TDMA poll 的 MAVLink stream
+rx_buf = bytearray()  # XBee API frame parser buffer
+
+
+# =========================================================
+# 基本工具函式
+# =========================================================
+
+def get_checksum(data):
+    return 0xFF - (sum(data) & 0xFF)
+
+
+def is_valid_addr64(addr):
+    if addr is None:
+        return False
+    if len(addr) != 8:
+        return False
+    if addr == b'\x00\x00\x00\x00\x00\x00\x00\x00':
+        return False
+    return True
+
+
+def learn_dest64_from_src64(src64):
+    """
+    UAV 收到 GCS 的 XBee 0x90 frame 時，
+    0x90 裡面的 src64 就是 GCS / coordinator XBee 的 64-bit address。
+    """
+    global DEST_64
+    global HELLO_SENT
+    global DISC_PENDING
+
+    if not is_valid_addr64(src64):
+        return False
+
+    src64 = bytes(src64)
+
+    if DEST_64 is None:
+        DEST_64 = src64
+        HELLO_SENT = False
+        return True
+
+    if DEST_64 != src64:
+        DEST_64 = src64
+        HELLO_SENT = False
+        DISC_PENDING = False
+        return True
+
+    return False
+
+
+def build_api_tx_frame(payload):
+    """
+    建立 XBee API frame type 0x10，把 payload 送到 DEST_64。
+    若 DEST_64 尚未學到，回傳 None。
+    """
+    if DEST_64 is None:
+        return None
+
+    frame_content = bytearray()
+    frame_content.append(0x10)
+    frame_content.append(0x00)
+    frame_content.extend(DEST_64)
+    frame_content.extend(b'\xFF\xFE')
+    frame_content.append(0x00)
+    frame_content.append(0x00)
+    frame_content.extend(payload)
+
+    length = len(frame_content)
+
+    packet = bytearray()
+    packet.append(0x7E)
+    packet.append((length >> 8) & 0xFF)
+    packet.append(length & 0xFF)
+    packet.extend(frame_content)
+    packet.append(get_checksum(frame_content))
+
+    return packet
+
+
+def send_to_xbee_chunked(payload):
+    """
+    把 payload 依 XBEE_MAX_PAYLOAD 切成多個 XBee API TX frame 送出。
+    """
+    if DEST_64 is None:
+        return False
+
+    total_len = len(payload)
+    sent_len = 0
+
+    while sent_len < total_len:
+        end_len = min(sent_len + XBEE_MAX_PAYLOAD, total_len)
+        chunk = payload[sent_len:end_len]
+        sent_len = end_len
+
+        pkt = build_api_tx_frame(chunk)
+        if pkt is None:
+            return False
+
+        uart_xb.write(pkt)
+        time.sleep_ms(1)
+
+    return True
+
+
+# =========================================================
+# HELO / DONE 回報
+# =========================================================
+
+def send_hello_report(force=False):
+    """
+    UAV 回報自己存在：
+        HELO + MY_SYSID
+
+    本版 DISC 預設以 force=True 呼叫，因此每次收到 DISC 都會重新回 HELO。
+    """
+    global _last_hello_ms
+    global HELLO_SENT
+
+    if MY_SYSID is None:
+        return False
+    if DEST_64 is None:
+        return False
+
+    if HELLO_SENT and not force:
+        return False
+
+    now = time.ticks_ms()
+    if not force:
+        if time.ticks_diff(now, _last_hello_ms) < HELLO_MIN_INTERVAL_MS:
+            return False
+
+    payload = HELLO_MAGIC + struct.pack('>B', MY_SYSID)
+    pkt = build_api_tx_frame(payload)
+    if pkt is None:
+        return False
+
+    uart_xb.write(pkt)
+    _last_hello_ms = now
+    HELLO_SENT = True
+
+    time.sleep_ms(1)
+    return True
+
+
+def try_send_pending_hello():
+    """
+    若先收到 DISC、後學到 MY_SYSID，這裡會自動補送 HELO。
+    """
+    global DISC_PENDING
+
+    if DISC_PENDING and MY_SYSID is not None and DEST_64 is not None:
+        delay_ms = 20 + (((MY_SYSID * 37) + (time.ticks_ms() & 0xFF)) % 180)
+        time.sleep_ms(delay_ms)
+
+        if send_hello_report(force=True):
+            DISC_PENDING = False
+
+
+def send_done_report(sent_len, remain_len):
+    """
+    DONE payload:
+        b'DONE' + sysid(1) + sent_len(2) + remain_len(2)
+    """
+    if MY_SYSID is None:
+        return False
+    if DEST_64 is None:
+        return False
+
+    if sent_len > 65535:
+        sent_len = 65535
+    if remain_len > 65535:
+        remain_len = 65535
+
+    payload = DONE_MAGIC + struct.pack('>BHH', MY_SYSID, sent_len, remain_len)
+    pkt = build_api_tx_frame(payload)
+    if pkt is None:
+        return False
+
+    uart_xb.write(pkt)
+    time.sleep_ms(1)
+    return True
+
+
+# =========================================================
+# MAVLink frame 解析與 MY_SYSID 自動學習
+# =========================================================
+
+def find_first_mavlink_magic(buf):
+    pos_fe = buf.find(b'\xFE')
+    pos_fd = buf.find(b'\xFD')
+
+    if pos_fe == -1:
+        return pos_fd
+    if pos_fd == -1:
+        return pos_fe
+    return pos_fe if pos_fe < pos_fd else pos_fd
+
+
+def mavlink_frame_length(buf, start_idx):
+    if start_idx >= len(buf):
+        return None
+
+    magic = buf[start_idx]
+
+    if magic == 0xFE:
+        if len(buf) - start_idx < 2:
+            return None
+        payload_len = buf[start_idx + 1]
+        total_len = payload_len + 8
+        if len(buf) - start_idx < total_len:
+            return None
+        return total_len
+
+    if magic == 0xFD:
+        if len(buf) - start_idx < 3:
+            return None
+        payload_len = buf[start_idx + 1]
+        incompat_flags = buf[start_idx + 2]
+        signed = (incompat_flags & 0x01) != 0
+        total_len = payload_len + 12 + (13 if signed else 0)
+        if len(buf) - start_idx < total_len:
+            return None
+        return total_len
+
+    return None
+
+
+def get_mavlink_sysid(buf, start_idx):
+    if start_idx >= len(buf):
+        return None
+
+    magic = buf[start_idx]
+
+    if magic == 0xFE:
+        if len(buf) - start_idx >= 6:
+            return buf[start_idx + 3]
+
+    if magic == 0xFD:
+        if len(buf) - start_idx >= 10:
+            return buf[start_idx + 5]
+
+    return None
+
+
+def learn_my_sysid_from_tx_buf():
+    """
+    從 FC -> ESP32 的 tx_buf 中找完整 MAVLink frame，
+    並自動學習飛控的 MAVLink SYSID。
+
+    若更改飛控 SYSID，建議重新上電 ESP32，讓 MY_SYSID 重新學習。
+    """
+    global MY_SYSID
+    global _sysid_candidate
+    global _sysid_candidate_count
+    global HELLO_SENT
+    global tx_buf
+
+    if len(tx_buf) == 0:
+        return
+
+    idx = 0
+    checked = 0
+
+    while idx < len(tx_buf) and checked < 8:
+        sub = tx_buf[idx:]
+        rel_start = find_first_mavlink_magic(sub)
+        if rel_start == -1:
+            return
+
+        start = idx + rel_start
+        frame_len = mavlink_frame_length(tx_buf, start)
+        if frame_len is None:
+            return
+
+        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:
+                _sysid_candidate = sysid
+                _sysid_candidate_count = 1
+
+            if _sysid_candidate_count >= SYSID_LEARN_CONFIRM_COUNT:
+                MY_SYSID = _sysid_candidate
+                HELLO_SENT = False
+                return
+
+        idx = start + frame_len
+        checked += 1
+
+
+# =========================================================
+# tx_buf MAVLink frame 取出與裁切
+# =========================================================
+
+def pop_mavlink_frames_by_quota(quota_bytes):
+    global tx_buf
+
+    if quota_bytes <= 0 or len(tx_buf) == 0:
+        return b''
+
+    start = find_first_mavlink_magic(tx_buf)
+    if start == -1:
+        tx_buf = bytearray()
+        return b''
+
+    if start > 0:
+        tx_buf = tx_buf[start:]
+
+    out = bytearray()
+
+    while len(tx_buf) > 0:
+        if tx_buf[0] not in (0xFE, 0xFD):
+            start = find_first_mavlink_magic(tx_buf)
+            if start == -1:
+                tx_buf = bytearray()
+                break
+            tx_buf = tx_buf[start:]
+
+        frame_len = mavlink_frame_length(tx_buf, 0)
+        if frame_len is None:
+            break
+
+        if len(out) > 0 and (len(out) + frame_len) > quota_bytes:
+            break
+
+        if len(out) == 0 and frame_len > quota_bytes:
+            out.extend(tx_buf[:frame_len])
+            tx_buf = tx_buf[frame_len:]
+            break
+
+        out.extend(tx_buf[:frame_len])
+        tx_buf = tx_buf[frame_len:]
+
+        if len(out) >= quota_bytes:
+            break
+
+    return bytes(out)
+
+
+def trim_tx_buffer_if_needed():
+    global tx_buf
+
+    if len(tx_buf) <= MAX_BUF_SIZE:
+        return
+
+    target_size = MAX_BUF_SIZE // 2
+
+    while len(tx_buf) > target_size:
+        start = find_first_mavlink_magic(tx_buf)
+        if start == -1:
+            tx_buf = bytearray()
+            return
+
+        if start > 0:
+            tx_buf = tx_buf[start:]
+            continue
+
+        frame_len = mavlink_frame_length(tx_buf, 0)
+        if frame_len is None:
+            if len(tx_buf) > target_size:
+                tx_buf = tx_buf[-target_size:]
+            return
+
+        tx_buf = tx_buf[frame_len:]
+
+
+def flush_tx_buffer(grant_bytes):
+    global tx_buf
+
+    if MY_SYSID is None: # TODO 這個重複判斷了 應該刪 不要浪費效率
+        return
+
+    if DEST_64 is None:
+        return
+
+    if grant_bytes <= 0:
+        send_done_report(0, len(tx_buf))
+        return
+
+    data_to_send = pop_mavlink_frames_by_quota(grant_bytes)
+    sent_len = len(data_to_send)
+
+    if sent_len > 0:
+        send_to_xbee_chunked(data_to_send)
+
+    send_done_report(sent_len, len(tx_buf))
+
+
+# =========================================================
+# GCS control payload 解析
+# =========================================================
+
+def parse_poll_payload(real_data):
+    if not real_data.startswith(POLL_MAGIC):
+        return None, None
+
+    if len(real_data) == 5:
+        return real_data[4], DEFAULT_GRANT_BYTES
+
+    if len(real_data) == 7:
+        target_sysid = real_data[4]
+        grant_bytes = (real_data[5] << 8) | real_data[6]
+        return target_sysid, grant_bytes
+
+    return None, None
+
+
+def is_discovery_payload(real_data):
+    return real_data.startswith(DISC_MAGIC)
+
+
+# =========================================================
+# XBee API RX parser
+# =========================================================
+
+def process_xbee_buffer():
+    global rx_buf
+    global DISC_PENDING
+
+    while True:
+        start_pos = rx_buf.find(b'\x7E')
+        if start_pos == -1:
+            rx_buf = bytearray()
+            return
+
+        if start_pos > 0:
+            rx_buf = rx_buf[start_pos:]
+
+        if len(rx_buf) < 3:
+            return
+
+        pkt_len = (rx_buf[1] << 8) | rx_buf[2]
+        total_len = pkt_len + 4
+
+        if pkt_len > 300:
+            rx_buf = rx_buf[1:]
+            continue
+
+        if len(rx_buf) < total_len:
+            return
+
+        packet = rx_buf[3:3 + pkt_len]
+        checksum_recv = rx_buf[3 + pkt_len]
+
+        if get_checksum(packet) == checksum_recv:
+            if len(packet) > 0 and packet[0] == 0x90:
+                # XBee Receive Packet 0x90:
+                # packet = 90 | src64(8) | src16(2) | options(1) | RF data
+                if len(packet) >= 12:
+                    src64 = bytes(packet[1:9])
+                    real_data = packet[12:]
+
+                    # -------------------------------------------------
+                    # DISC：本版唯一 discovery 封包。
+                    # 功能等同原本 DSCF：收到後強制回 HELO。
+                    # -------------------------------------------------
+                    if is_discovery_payload(real_data):
+                        learn_dest64_from_src64(src64)
+                        learn_my_sysid_from_tx_buf() # TODO 加一個 if MY_SYSID is None
+
+                        if MY_SYSID is not None and DEST_64 is not None:
+                            delay_ms = 20 + ((MY_SYSID * 37) + (time.ticks_ms() & 0xFF)) % 180 # TODO 這邊會有多少秒延遲? 為啥是37? 不能21或者17? 20 ~ 199  210 / 30000
+                            time.sleep_ms(delay_ms)
+                            send_hello_report(force=True)
+                            DISC_PENDING = False
+                        else:
+                            DISC_PENDING = True
+
+                    # -------------------------------------------------
+                    # 判斷是不是 POLL
+                    # -------------------------------------------------
+                    else: # TODO 這邊要用 elif 去判斷 POLL
+                        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() # TODO 這邊三行怪怪的 GCS 下 poll 然後會回應 HELO 這個很怪 然後為何要每次重做上面兩行 拖效率
+
+                            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: # TODO 這個概念不對 隨便一個 poll 就把本來有做過 DISC 的權限搶走了 危險!!!
+                                learn_dest64_from_src64(src64)
+
+                            if DEST_64 is not None and src64 == DEST_64: # TODO 從 GCS 到 FC 的 mavlink 封包 切開不要跟 POLL 放在同一個判斷
+                                uart_fc.write(real_data)
+
+            rx_buf = rx_buf[total_len:]
+
+        else:
+            rx_buf = rx_buf[1:]
+
+
+
+# =========================================================
+# 初始清理
+# =========================================================
+
+try:
+    gc.collect()
+except Exception:
+    pass
+
+
+# =========================================================
+# 主迴圈
+# =========================================================
+
+while True:
+    try:
+        # -------------------------------------------------
+        # FC -> ESP32 tx buffer
+        # -------------------------------------------------
+        if uart_fc.any():
+            data = uart_fc.read(READ_FC_BYTES)
+            if data:
+                tx_buf.extend(data)
+
+                learn_my_sysid_from_tx_buf()
+                try_send_pending_hello()
+                trim_tx_buffer_if_needed()
+
+        # -------------------------------------------------
+        # XBee -> ESP32
+        # -------------------------------------------------
+        if uart_xb.any():
+            chunk = uart_xb.read()
+            if chunk:
+                rx_buf.extend(chunk)
+
+            process_xbee_buffer()
+
+        time.sleep_ms(1)
+
+    except MemoryError:
+        tx_buf = bytearray()
+        rx_buf = bytearray()
+
+        try:
+            gc.collect()
+        except Exception:
+            pass
+
+        time.sleep_ms(10)
+
+    except Exception:
+        time.sleep_ms(2)

src/unitdev02/unitdev02/ntrip_dev.py

@@ -0,0 +1,209 @@
+# import socket
+
+# def get_rtk2go_source_table():
+#     host = "rtk2go.com"
+#     port = 2101
+#
+#     print(f"正在連線至 {host}:{port} 獲取掛載點列表...\n")
+#     ...
+
+from __future__ import annotations
+
+import socket
+import base64
+import time
+
+
+# ── NMEA GGA（告訴 Caster「我在哪」，觸發 RTCM 推流）────────────────
+
+def nmea_checksum(body: str) -> str:
+    """body 不含 '$' 與 '*checksum'，例如 'GPGGA,123519,...'"""
+    value = 0
+    for ch in body:
+        value ^= ord(ch)
+    return f"{value:02X}"
+
+
+def decimal_to_nmea_dm(deg: float, *, is_latitude: bool) -> tuple[str, str]:
+    """十進位度 → NMEA 的 (d)dmm.mmmm 與半球字元。"""
+    if is_latitude:
+        hemi = "N" if deg >= 0 else "S"
+        deg_width = 2
+    else:
+        hemi = "E" if deg >= 0 else "W"
+        deg_width = 3
+    deg = abs(deg)
+    d = int(deg)
+    m = (deg - d) * 60.0
+    return f"{d:0{deg_width}d}{m:07.4f}", hemi
+
+
+def build_gga_sentence(lat_deg: float, lon_deg: float, alt_m: float = 100.0) -> bytes:
+    """
+    組一筆 $GPGGA 句子（含 checksum），回傳 bytes 可直接 sock.sendall。
+
+    lat_deg / lon_deg：十進位經緯度（北、東為正）。
+    測試時請改成 mount 服務範圍內的近似位置；正式使用應來自 GNSS 真實定位。
+    """
+    utc = time.gmtime()
+    t_str = f"{utc.tm_hour:02d}{utc.tm_min:02d}{utc.tm_sec:02d}.00"
+
+    lat_dm, ns = decimal_to_nmea_dm(lat_deg, is_latitude=True)
+    lon_dm, ew = decimal_to_nmea_dm(lon_deg, is_latitude=False)
+
+    # quality=1（GPS fix）、8 顆星、HDOP=1.0 僅供測試示意
+    body = (
+        f"GPGGA,{t_str},{lat_dm},{ns},{lon_dm},{ew},"
+        f"1,08,1.0,{alt_m:.1f},M,0.0,M,,"
+    )
+    sentence = f"${body}*{nmea_checksum(body)}\r\n"
+    return sentence.encode("ascii")
+
+
+def send_gga(sock: socket.socket, lat_deg: float, lon_deg: float, alt_m: float = 100.0) -> str:
+    """送出 GGA，回傳可讀句子供列印。"""
+    payload = build_gga_sentence(lat_deg, lon_deg, alt_m)
+    sock.sendall(payload)
+    return payload.decode("ascii").strip()
+
+
+# ── NTRIP 連線 + RTCM 接收 ─────────────────────────────────────────
+
+def get_rtcm_payload(
+    host,
+    port,
+    mountpoint,
+    user="",
+    password="",
+    target_ids=None,
+    *,
+    gga_lat=None,
+    gga_lon=None,
+    gga_alt_m=100.0,
+    gga_interval_sec=5.0,
+    filter_ids=True,
+):
+    """
+  連線 NTRIP caster 並解析 RTCM3。
+
+  gga_lat / gga_lon：若兩者皆有設定，握手成功後會週期送出 GGA（許多 Caster 如 SNIP 至少需一次）。
+  gga_interval_sec：GGA 重送間隔（秒）。
+  filter_ids：True 時只印 target_ids 內的 message；False 印全部（方便學習觀察）。
+    """
+    if target_ids is None:
+        target_ids = []
+    need_gga = gga_lat is not None and gga_lon is not None
+
+    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+    sock.settimeout(10)
+
+    try:
+        sock.connect((host, port))
+
+        auth = base64.b64encode(f"{user}:{password}".encode()).decode()
+        request = f"GET /{mountpoint} HTTP/1.0\r\n"
+        request += "User-Agent: NTRIP PythonClient\r\n"
+        request += f"Authorization: Basic {auth}\r\n"
+        request += "Connection: close\r\n\r\n"
+        sock.sendall(request.encode())
+
+        response = sock.recv(4096)
+        if b"ICY 200 OK" not in response:
+            print(f"連線失敗: {response.decode(errors='ignore')}")
+            return
+
+        print(f"已成功連接至 {mountpoint}")
+        if need_gga:
+            print(
+                f"GGA 模式：每 {gga_interval_sec}s 回報位置 "
+                f"({gga_lat:.6f}, {gga_lon:.6f})，觸發 Caster 推流"
+            )
+        else:
+            print("未設定 GGA（部分 Caster 如 RTK2GO 仍可推流；SNIP/區域站常需 GGA）")
+        print(f"RTCM 過濾目標: {target_ids if filter_ids else '(關閉，顯示全部)'}\n")
+
+        last_gga_time = 0.0
+        buffer = b""
+
+        while True:
+            now = time.time()
+            if need_gga and (now - last_gga_time >= gga_interval_sec):
+                line = send_gga(sock, gga_lat, gga_lon, gga_alt_m)
+                print(f"[GGA] {line}")
+                last_gga_time = now
+
+            try:
+                chunk = sock.recv(4096)
+            except socket.timeout:
+                continue
+
+            if not chunk:
+                print("Caster 關閉連線")
+                break
+
+            buffer += chunk
+
+            while len(buffer) >= 3:
+                if buffer[0] != 0xD3:
+                    buffer = buffer[1:]
+                    continue
+
+                length = ((buffer[1] & 0x03) << 8) | buffer[2]
+                total_len = length + 6
+
+                if len(buffer) < total_len:
+                    break
+
+                packet = buffer[:total_len]
+                buffer = buffer[total_len:]
+                msg_id = (packet[3] << 4) | (packet[4] >> 4)
+
+                if not filter_ids or str(msg_id) in target_ids:
+                    print(f"Found ID: {msg_id} | Length: {total_len} bytes")
+
+    except KeyboardInterrupt:
+        print("\n使用者中斷")
+    except Exception as e:
+        print(f"發生錯誤: {e}")
+    finally:
+        sock.close()
+
+
+if __name__ == "__main__":
+    TARGET_LIST = ["1007", "1230", "1074"]
+
+    # ── Case 1：RTK2GO（通常可不送 GGA）────────────────────────────
+    # USER = "chiyu1468@hotmail.com"
+    # HOST = "rtk2go.com"
+    # PORT = 2101
+    # MOUNT = "No1bio_02"
+    # get_rtcm_payload(
+    #     HOST, PORT, MOUNT,
+    #     user=USER,
+    #     target_ids=TARGET_LIST,
+    #     filter_ids=False,
+    # )
+
+    # ── Case 2：區域 Caster（需 GGA，座標請改成服務區內近似點）──────
+    USER = "uavlab6061"
+    PW = "iamsupersmart"
+    HOST = "210.241.63.193"
+    PORT = 81
+    MOUNT = "2020_GNSS"
+    # 十進位度；請依 SNIP / 基準站服務範圍調
+    GGA_LAT = 24.155792000
+    GGA_LON = 120.630679000
+
+    get_rtcm_payload(
+        HOST,
+        PORT,
+        MOUNT,
+        user=USER,
+        password=PW,
+        target_ids=TARGET_LIST,
+        gga_lat=GGA_LAT,
+        gga_lon=GGA_LON,
+        gga_alt_m=100.0,
+        gga_interval_sec=30.0,
+        filter_ids=False,
+    )

src/unitdev02/unitdev02/tryByteDict.py

@@ -0,0 +1,33 @@
+import timeit
+
+# 建立 1000 個元素的字典，避免單一 Key 的特殊雜湊值干擾
+d_int = {i: True for i in range(1000)}
+d_bytes = {f"k_{i:06d}".encode(): True for i in range(1000)}
+
+# 正確命中中間的值
+target_int = 500
+target_bytes_same = f"k_{500:06d}".encode()
+
+# 將變數傳入 timeit 的全域環境
+setup_env = lambda: None
+globals_dict = {
+    'd_int': d_int,
+    'd_bytes': d_bytes,
+    'target_int': target_int,
+    'target_bytes_same': target_bytes_same
+}
+
+# 1. int 查詢
+t_int = timeit.timeit("d_int.get(target_int)", globals=globals_dict, number=50000000)
+
+# 2. bytes 查詢 (同物件：直接傳入現成變數)
+t_bytes_same = timeit.timeit("d_bytes.get(target_bytes_same)", globals=globals_dict, number=50000000)
+
+# 3. bytes 查詢 (真・新物件：每次都在查詢時動態切片生成新物件，強迫重新計算 Hash)
+globals_dict['dynamic_source'] = f"k_{500:06d}".encode() + b"dummy"
+t_bytes_new = timeit.timeit("d_bytes.get(dynamic_source[:8])", globals=globals_dict, number=50000000)
+
+print(f"修正後測試結果 (50,000,000 次)：")
+print(f"int 查詢          : {t_int:.4f} 秒 (1.0x)")
+print(f"bytes 查詢 (同物件): {t_bytes_same:.4f} 秒 (約 {t_bytes_same/t_int:.2f}x)")
+print(f"bytes 查詢 (新物件): {t_bytes_new:.4f} 秒 (約 {t_bytes_new/t_int:.2f}x)")