tempCommit

src/fc_network_adapter/fc_network_adapter/serialManager.py

@@ -311,8 +311,8 @@ 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 封包連續丟失的累計數
@@ -336,7 +336,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         self.serial_baudrate = 115200
 
         self.udp_transmit_queue: deque[bytearray] = deque()
-        self.poll_scheduler_state = pollStrategy.PollSchedulerState()
+        # self.poll_scheduler_state = pollStrategy.PollSchedulerState()
 
         self.command_pending_event: Optional[asyncio.Event] = None 
         self.poll_done_event: Optional[asyncio.Event] = None
@@ -346,7 +346,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
 
         self.last_discovery_time = 0.0 # 這個是最後做廣播 discovery 的時間  
         self.last_recieve_mavlink = 0.0 # 這個是最後收到 mavlink payload 時間 為了定義 poll-done 之間不要超時用的
-        self.mavPack_interval_timeout = 150  # (msec) poll 期間 每筆 MAVLink 資料 /DONE 最大閒置間隔
+        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.01 # 
@@ -395,9 +395,9 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
     # ---- 對外契約 ----
 
     def process_outgoing(self, data: bytes) -> bytes:
-        """這裡不在用 Base 的方式送出"""
+        """這裡不在用 Base 的方式 直接送出 而是先暫存到一處"""
         self.enqueue_serial_transmit(data)
-        self.wake_operator()
+        # self.wake_operator()
         return b''  
 
     # ---- UDP 到 Serial 佇列 ----
@@ -448,16 +448,17 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         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(
@@ -555,12 +556,12 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
 
         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
+            # 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}")
@@ -649,6 +650,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
 
         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
@@ -665,15 +667,12 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
             pollStrategy.PollDevice(
                 address_64=address_64,
                 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:
@@ -714,7 +713,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(
@@ -732,24 +732,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:
         """
@@ -803,7 +804,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(
@@ -886,7 +887,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         self.pending_manual_discovery = False
 
     # async def _wait_poll_done_with_idle_timeout(self) -> bool:
-    #     idle_timeout_sec = self.mavPack_interval_timeout / 1000.0
+    #     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():
@@ -927,31 +928,41 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         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 # 第一筆故意給多一些等待時間
 
+        # 這邊會做傳送等待
         completed = await self._wait_poll_done_with_idle_timeout()
-        if not completed:
-            remote_device.remain_bytes -= remote_device.received_len
-            logger.warning(
+
+        # 這邊代表的是 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"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
-            # await asyncio.sleep(self.guard_milliseconds / 1000.0)
 
     # poll 程序中 只要目標的載具還有在傳資訊 就不會終止資料的接收
     async def _wait_poll_done_with_idle_timeout(self) -> bool:
-        idle_timeout_sec = self.mavPack_interval_timeout / 1000.0
+        # 容許時間 
+        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:
-                # logger.debug(f"mark A poll timeout")
                 return False
             try:
                 await asyncio.wait_for(
@@ -1580,11 +1591,10 @@ if __name__ == '__main__':
     serial_id = 1
     processor = sm.get_espv1_processor(serial_id)
     if processor is not None:
-        processor.request_discovery()
-        time.sleep(1)
-        processor.request_discovery()
-        time.sleep(1)
-        processor.request_discovery()
+        # 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())

src/fc_network_adapter/fc_network_adapter/utils/pollStrategy.py

@@ -7,41 +7,51 @@ POLL 輪詢策略，供 XBeeFrameProcessor_ESPv1 使用。
 from dataclasses import dataclass
 from typing import List
 
+MIN_TRIGGER_INTERVAL_MS = 50
 
 @dataclass(frozen=True)
 class PollDevice:
     """從 esp32AddrMapping 抽出的唯讀快照。"""
     address_64: bytes
     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
+
+    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