chiyu1468/AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

tempCommit

Browse Source
This commit is contained in:
chiyu1468
2026-07-27 15:30:59 +08:00
parent 1e07d1fa54
commit 9707be2252
2 changed files with 88 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/fc_network_adapter/fc_network_adapter/serialManager.py
+58 -48
View File
@@ -311,8 +311,8 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
self.last_hello_time = last_hello_time self.last_hello_time = last_hello_time
self.remain_bytes = 0 # 剩餘 buffer 量 self.remain_bytes = 0 # 剩餘 buffer 量
# self.last_poll_time = 0.0 # 目前沒用到 self.last_poll_time = 0.0 # 最後送出Done的時間 用於判斷poll順序
self.last_done_time = 0.0 # 最後送出Done的時間 self.last_done_time = 0.0 # 最後收到Done的時間 用於判斷離線
self.received_len = 0 # 收到封包累計 self.received_len = 0 # 收到封包累計
self.poll_miss_count = 0 # poll 封包連續丟失的累計數 self.poll_miss_count = 0 # poll 封包連續丟失的累計數
@@ -336,7 +336,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
self.serial_baudrate = 115200 self.serial_baudrate = 115200
self.udp_transmit_queue: deque[bytearray] = deque() 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.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_discovery_time = 0.0 # 這個是最後做廣播 discovery 的時間
self.last_recieve_mavlink = 0.0 # 這個是最後收到 mavlink payload 時間 為了定義 poll-done 之間不要超時用的 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.discovery_interval_seconds = 200.0 # (sec) 每次做 discovery 程序的間隔時間
self.device_offline_timeout = self.discovery_interval_seconds * 2 # 遠端沒有回應會被踢出 超時時限 self.device_offline_timeout = self.discovery_interval_seconds * 2 # 遠端沒有回應會被踢出 超時時限
self.operator_tick_interval_seconds = 0.01 # self.operator_tick_interval_seconds = 0.01 #
@@ -395,9 +395,9 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
# ---- 對外契約 ---- # ---- 對外契約 ----
def process_outgoing(self, data: bytes) -> bytes: def process_outgoing(self, data: bytes) -> bytes:
"""這裡不在用 Base 的方式送出""" """這裡不在用 Base 的方式 直接送出 而是先暫存到一處"""
self.enqueue_serial_transmit(data) self.enqueue_serial_transmit(data)
self.wake_operator() # self.wake_operator()
return b'' return b''
# ---- UDP 到 Serial 佇列 ---- # ---- UDP 到 Serial 佇列 ----
@@ -448,16 +448,17 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
frame_id = frame[4] frame_id = frame[4]
delivery = frame[8] delivery = frame[8]
# logger.debug(f"mark C {self._pending_tx_count}")
if frame_id == 0x02: # poll pack 的回傳處理 if frame_id == 0x02: # poll pack 的回傳處理
# 若封包丟失 則設立旗標並停下等待迴圈 # 若封包丟失 則設立旗標並停下等待迴圈
if delivery != 0x00: if delivery != 0x00:
self.current_poll_missing = True self.current_poll_missing = True
self.poll_done_event.set() self.poll_done_event.set()
elif (frame_id >= 0x10) and (frame_id > 0xF0): # UDP 上傳到 RF 的處理 elif (frame_id >= 0x10) and (frame_id > 0xF0): # UDP 上傳到 RF 的處理
if self._pending_tx_count > 0: if self._pending_tx_count > 0:
self._pending_tx_count -= 1 self._pending_tx_count -= 1
logger.debug(f"send Completed !")
if delivery != 0x00: if delivery != 0x00:
logger.warning( logger.warning(
@@ -555,12 +556,12 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
if frame_type == ATCommandHandler.FRAME_TYPE_TX_STATUS: if frame_type == ATCommandHandler.FRAME_TYPE_TX_STATUS:
self._handle_tx_status(frame) self._handle_tx_status(frame)
length = (frame[1] << 8) | frame[2] # for debug # length = (frame[1] << 8) | frame[2] # for debug
logger.info( # logger.info(
f"TX Status raw={frame.hex()}, api_len={length}, " # f"TX Status raw={frame.hex()}, api_len={length}, "
f"fid=0x{frame[4]:02X}, dest16=0x{(frame[5]<<8)|frame[6]:04X}, " # f"fid=0x{frame[4]:02X}, dest16=0x{(frame[5]<<8)|frame[6]:04X}, "
f"retry={frame[7]}, delivery={frame[8]}, discovery={frame[9]}" # f"retry={frame[7]}, delivery={frame[8]}, discovery={frame[9]}"
) # for debug # ) # for debug
return None return None
logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}") 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.received_len = 0
remote_device.last_done_time = time.time() remote_device.last_done_time = time.time()
remote_device.last_poll_time = self.current_poll_time
if ( if (
self.current_poll_address_64 is not None self.current_poll_address_64 is not None
@@ -665,15 +667,12 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
pollStrategy.PollDevice( pollStrategy.PollDevice(
address_64=address_64, address_64=address_64,
remain_bytes=device.remain_bytes, 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() for address_64, device in self.esp32_address_mapping.items()
] ]
return pollStrategy.pick_next( return pollStrategy.pick_next(poll_devices, time.time())
poll_devices,
self.poll_scheduler_state
)
# 移除長時間沒有 HELLO 或 DONE 的 Dongle # 移除長時間沒有 HELLO 或 DONE 的 Dongle
def _prune_stale_devices(self) -> None: def _prune_stale_devices(self) -> None:
@@ -714,7 +713,8 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
if not self.operator_running: if not self.operator_running:
logger.warning("ESPv1 request_discovery: operator not running") logger.warning("ESPv1 request_discovery: operator not running")
return False 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 return True
def request_poll( def request_poll(
@@ -732,24 +732,25 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
if not self.operator_running: if not self.operator_running:
logger.warning("ESPv1 request_poll: operator not running") logger.warning("ESPv1 request_poll: operator not running")
return False return False
self.event_loop.call_soon_threadsafe( # self.event_loop.call_soon_threadsafe(
self._apply_request_poll, # self._apply_request_poll,
target_system_id, # target_system_id,
grant_bytes, # grant_bytes,
) # ) # [del function] 每個 tick 間隔很短 不想要搞這麼複雜
self.pending_manual_poll = (target_system_id, grant_bytes)
return True return True
def _apply_request_discovery(self) -> None: # def _apply_request_discovery(self) -> None: # [del function] 每個 tick 間隔很短 不想要搞這麼複雜
self.pending_manual_discovery = True # self.pending_manual_discovery = True
self.wake_operator() # self.wake_operator()
def _apply_request_poll( # def _apply_request_poll(
self, # self,
target_system_id: int, # target_system_id: int,
grant_bytes: Optional[int], # grant_bytes: Optional[int],
) -> None: # ) -> None: # [del function] 每個 tick 間隔很短 不想要搞這麼複雜
self.pending_manual_poll = (target_system_id, grant_bytes) # self.pending_manual_poll = (target_system_id, grant_bytes)
self.wake_operator() # self.wake_operator()
def get_status_snapshot(self) -> dict: 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) ) sleep_task = asyncio.create_task( asyncio.sleep(self.operator_tick_interval_seconds) )
# 有"急事"被叫醒 # 有"急事"被叫醒 : 程序中斷時
wake_task = asyncio.create_task(self.command_pending_event.wait()) wake_task = asyncio.create_task(self.command_pending_event.wait())
done, pending = await asyncio.wait( done, pending = await asyncio.wait(
@@ -886,7 +887,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
self.pending_manual_discovery = False self.pending_manual_discovery = False
# async def _wait_poll_done_with_idle_timeout(self) -> bool: # 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) # poll_tick = min(0.02, idle_timeout_sec)
# while not self.poll_done_event.is_set(): # while not self.poll_done_event.is_set():
@@ -927,31 +928,41 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
self.current_poll_missing = False self.current_poll_missing = False
self.poll_done_event.clear() self.poll_done_event.clear()
self.serial_writer(poll_frame) 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() completed = await self._wait_poll_done_with_idle_timeout()
if not completed:
remote_device.remain_bytes -= remote_device.received_len # 這邊代表的是 poll 有觸發 也有收到資料 但是 done 沒有收到
logger.warning( if (not completed) and (not remote_device.received_len):
logger.debug(
f"POLL timeout address_64={target_address_64.hex()} " 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 # 判斷是否 poll pack 丟失 並紀錄連續丟失 清空 remain_bytes
if self.current_poll_missing: if self.current_poll_missing:
remote_device.poll_miss_count += 1 remote_device.poll_miss_count += 1
remote_device.remain_bytes = 0 remote_device.remain_bytes = 0
self.operator_busy = False self.operator_busy = False
self.current_poll_address_64 = None self.current_poll_address_64 = None
# await asyncio.sleep(self.guard_milliseconds / 1000.0)
# poll 程序中 只要目標的載具還有在傳資訊 就不會終止資料的接收 # poll 程序中 只要目標的載具還有在傳資訊 就不會終止資料的接收
async def _wait_poll_done_with_idle_timeout(self) -> bool: 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) poll_tick = min(0.02, idle_timeout_sec / 2)
while not self.poll_done_event.is_set(): while not self.poll_done_event.is_set():
if time.time() - self.last_recieve_mavlink >= idle_timeout_sec: if time.time() - self.last_recieve_mavlink >= idle_timeout_sec:
# logger.debug(f"mark A poll timeout")
return False return False
try: try:
await asyncio.wait_for( await asyncio.wait_for(
@@ -1580,11 +1591,10 @@ if __name__ == '__main__':
serial_id = 1 serial_id = 1
processor = sm.get_espv1_processor(serial_id) processor = sm.get_espv1_processor(serial_id)
if processor is not None: if processor is not None:
processor.request_discovery() # for x in range(10):
time.sleep(1) # processor.request_discovery()
processor.request_discovery() # time.sleep(1)
time.sleep(1) # print(f"mark N{x}")
processor.request_discovery()
# processor.request_poll(target_system_id=1) # processor.request_poll(target_system_id=1)
# processor.request_poll(target_system_id=1, grant_bytes=200) # processor.request_poll(target_system_id=1, grant_bytes=200)
print(processor.get_status_snapshot()) print(processor.get_status_snapshot())
src/fc_network_adapter/fc_network_adapter/utils/pollStrategy.py
+28 -18
View File
@@ -7,41 +7,51 @@ POLL 輪詢策略，供 XBeeFrameProcessor_ESPv1 使用。
from dataclasses import dataclass from dataclasses import dataclass
from typing import List from typing import List
MIN_TRIGGER_INTERVAL_MS = 50
@dataclass(frozen=True) @dataclass(frozen=True)
class PollDevice: class PollDevice:
"""從 esp32AddrMapping 抽出的唯讀快照。""" """從 esp32AddrMapping 抽出的唯讀快照。"""
address_64: bytes address_64: bytes
remain_bytes: int remain_bytes: int
last_done_time: float last_poll_time: float
@dataclass def pick_next(devices: List[PollDevice], now_time):
class PollSchedulerState:
"""每條 serial link 一份，保存 round-robin 索引。"""
round_robin_index: int = 0
def pick_next(
devices: List[PollDevice],
scheduler_state: PollSchedulerState
):
""" """
選下一個 POLL 目標。 選下一個 POLL 目標。
回傳 (target_address_64, grant_bytes)；devices 為空時回傳 (None, 0)。 回傳 (target_address_64, grant_bytes)；devices 為空時回傳 (None, 0)。
poll time = 0.2305 * grant_bytes + 58.094 (R平方為0.979 高線性相關) @-40db
""" """
if not devices: if not devices:
return None, 0 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 grant_bytes = 0
if selected_device.remain_bytes > 0: device_count = len(devices)
grant_bytes = min( 65535, max(0, selected_device.remain_bytes)) 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 return selected_device.address_64, grant_bytes