diff --git a/src/fc_network_adapter/fc_network_adapter/serialManager.py b/src/fc_network_adapter/fc_network_adapter/serialManager.py index 0d38e44..0f49f9d 100644 --- a/src/fc_network_adapter/fc_network_adapter/serialManager.py +++ b/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,9 +336,9 @@ 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.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 @@ -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()) diff --git a/src/fc_network_adapter/fc_network_adapter/utils/pollStrategy.py b/src/fc_network_adapter/fc_network_adapter/utils/pollStrategy.py index 62634ae..0148ab3 100644 --- a/src/fc_network_adapter/fc_network_adapter/utils/pollStrategy.py +++ b/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 + grant_bytes = 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 + weights = [0] * device_count - grant_bytes = 0 - if selected_device.remain_bytes > 0: - grant_bytes = min( 65535, max(0, selected_device.remain_bytes)) + 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