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

(modify) serialManager.py

Browse Source 
- 修正 POLL-DONE 程序的 timeout 判定方式
- 修正為 py3.8 語法
- 不在判定要求量跟實收量一致性
chiyuTemp
Chiyu Chen 1 month ago
parent d734a7bfee
commit fd949fca47
4 changed files with 139 additions and 108 deletions
Show Stats Download Patch File Download Diff File

4
src/fc_network_adapter/fc_network_adapter/mavlinkObject.py
Unescape Escape View File

@ -47,8 +47,8 @@ from pymavlink.dialects.v20 import ardupilotmega as mav_ardupilot
# 自定義的 import
from .mavlinkVehicleView import (
vehicle_registry,
VehicleView,
vehicle_registry, # 儲存全部物件的地方
VehicleView, # 代表每台載具的最外層
VehicleComponent,
ComponentType,
StatusTextEntry,

2
src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py
Unescape Escape View File

@ -187,6 +187,7 @@ class RFStatus:
@dataclass
class SocketInfo:
"""Socket連接資訊"""
src64_addr: Optional[bytes] = None # 模組的物理定址
ip: Optional[str] = None # IP位址
port: Optional[int] = None # 埠號
local_ip: Optional[str] = None # 本地IP
@ -310,6 +311,7 @@ class RFModule:
class VehicleView:
"""
最上層
載具視圖 - 純狀態容器
特點:

214
src/fc_network_adapter/fc_network_adapter/serialManager.py
Unescape Escape View File

@ -25,6 +25,12 @@ from typing import List
# from xbee.frame import APIFrame
# 自定義的 import
from .mavlinkVehicleView import (
vehicle_registry, # 儲存全部物件的地方
VehicleView,
RFModule,
RFModuleType,
)
from .utils import RingBuffer, setup_logger
from .utils import pollStrategy
@ -139,6 +145,8 @@ class XBeeFrameProcessor_Base(FrameProcessor):
DEST_ADDR16_UNICAST = b'\xFF\xFE'
DEST_ADDR16_BRAODCAST = b'\xFF\xFF'
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
@ -222,7 +230,7 @@ class XBeeFrameProcessor_Base(FrameProcessor):
@staticmethod
def _encapsulate(
data: bytes,
dest_addr64: bytes = b'\x00\x00\x00\x00\x00\x00\x00\x00',
dest_addr64: bytes = DEST_ADDR64_BRAODCAST,
dest_addr16 = DEST_ADDR16_BRAODCAST,
frame_id: int = 0x01,
) -> bytes:
@ -252,14 +260,26 @@ class XBeeFrameProcessor_Base(FrameProcessor):
class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
# GCS -> UAV:
# DISC
# POLL + esp_sysid(1) + grant_bytes(2)
#
# UAV -> GCS:
# HELO + esp_sysid(1)
# DONE + sysid(1) + sent_len(2) + remain_len(2)
'''
ESP32 封包分類
- Mavlink 資料封包 :
- Payload = mavlink [方向] GCS -> UAV (broadcast)
- Payload = mavlink [方向] UAV -> GCS (with src64 Addr)
- Discovery 階段
- Payload = DISC_HEADER [方向] GCS -> UAV (broadcast)
- Payload = HELLO_HEADER + esp_sysid(1) [方向] UAV -> GCS (with src64 Addr)
- Poll 階段
- Payload = POLL_HEADER + esp_sysid(1) + grant_bytes(2) [方向] GCS -> UAV (with src64 Addr)
- Payload = DONE_HEADER + sysid(1) + sent_len(2) + remain_len(2) [方向] UAV -> GCS (with src64 Addr)
這邊認定 esp_sysid 會等於 mavlink sysid
硬體產品系列 (Product Family) : XBP9B-DM
晶片世代: XBee PRO 900HP 200K
運作頻段: 900 MHz RF。
運行協議 (Protocol / Function Set): DigiMesh
韌體版本 (Firmware Version): 8075
'''
DISC_HEADER = b'DISC'
HELLO_HEADER = b'HELO'
@ -270,6 +290,11 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
MAX_PAYLOAD_PER_FRAME = 80
CHUNK_SEND_INTERVAL_SEC = 0.01
# ADDR16 選項
DEST_ADDR16_BRAODCAST = b'\xFF\xFF'
DEST_ADDR64_BRAODCAST = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'
class Esp32DeviceInfo:
def __init__(self, system_id, address_64, last_hello_time):
self.system_id = system_id
@ -277,6 +302,7 @@ 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.received_len = 0 # 收到封包累計
@ -301,7 +327,9 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
self.poll_done_event: Optional[asyncio.Event] = None
self.current_poll_address_64: Optional[bytes] = None
self.last_discovery_time = 0.0
self.last_discovery_time = 0.0 # 這個是最後做廣播 discovery 的時間
self.last_recieve_mavlink = 0.0 # 這個是最後收到 mavlink payload 時間 為了定義 poll-done 之間不要超時用的
self.MAX_mavPack_interval_timeout = 100 # ms，poll 期間 MAVLink/DONE 最大閒置間隔
self.discovery_interval_seconds = 30.0 # 每次做 discovery 程序的間隔時間
self.device_offline_timeout = self.discovery_interval_seconds * 2 # 遠端沒有回應會被踢出 超時時限
self.operator_tick_interval_seconds = 0.03 #
@ -352,6 +380,8 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
remote_device = self.esp32_address_mapping.get(sender_address_64)
if remote_device is not None:
remote_device.received_len += len(payload)
if (self.operator_busy and self.current_poll_address_64 == sender_address_64):
self.last_recieve_mavlink = time.time()
return payload
if frame_type == self.FRAME_TYPE_AT_RESPONSE:
@ -376,6 +406,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
def pack_discovery(self) -> bytes:
return self._encapsulate(self.DISC_HEADER, frame_id=0x00)
# 處理每個裝置回傳的 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)
@ -385,8 +416,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
system_id, sender_address_64, time.time()
)
logger.debug(
f"new HELO system_id={system_id}, address_64={sender_address_64.hex()}"
)
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:
@ -395,6 +425,10 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
f"address_64={sender_address_64.hex()}"
)
if vehicle:=vehicle_registry.get(system_id):
if not vehicle.rf_module:
vehicle.rf_module = RFModule(RFModuleType.XBEE)
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:
@ -416,13 +450,16 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
if remote_device is None:
return
system_id, sent_length, remain_length = struct.unpack('>BHH', payload[4:9])
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}"
)
# 這段是有問題的 因為會有整數封包切割問題 以及載具端的 buffer 存量不足 故回傳的資訊量會與要求的不一致
# system_id, sent_length, remain_length = struct.unpack('>BHH', payload[4:9])
# 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}"
# )
# TODO 傳送速率
# TODO 累積速率預測
remote_device.received_len = 0
remote_device.remain_bytes = remain_length
remote_device.last_done_time = time.time()
@ -716,6 +753,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.MAX_mavPack_interval_timeout / 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,
)
except asyncio.TimeoutError:
continue
return True
# poll 程序
async def _run_one_poll(self, target_address_64: bytes, grant_bytes: int) -> None:
if self.serial_writer is None:
@ -734,22 +787,15 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
self.current_poll_address_64 = target_address_64
self.poll_done_event.clear()
self.serial_writer(poll_frame)
self.last_recieve_mavlink = time.time()
timeout_seconds = pollStrategy.estimate_poll_timeout(
grant_bytes,
self.serial_baudrate,
self.guard_milliseconds,
)
try:
await asyncio.wait_for(
self.poll_done_event.wait(),
timeout=timeout_seconds,
)
except asyncio.TimeoutError:
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()
if not completed:
logger.warning(
f"POLL timeout address_64={target_address_64.hex()} "
f"grant_bytes={grant_bytes}"
)
finally:
self.operator_busy = False
self.current_poll_address_64 = None
@ -820,21 +866,21 @@ class ATCommandHandler:
# ---- 接收端 ----
def handle_frame(self, frame: bytes) -> None:
"""
接收一整個 AT Response frame：
接收一整個 AT Response frame:
1. 解析成 ATResponse
2. 推進 rx_module_ack 供其他模組消費
3. 本地 dispatch 給對應的 _handle_xxx
"""
parsed = self._parse(frame)
if parsed is None:
parsed_at_ack = self._parse(frame)
if parsed_at_ack is None:
return
if not rx_module_ack.put(parsed):
if not rx_module_ack.put(parsed_at_ack):
logger.warning(
f"[{self.serial_port}] rx_module_ack overflow, drop {parsed.command!r}"
f"[{self.serial_port}] rx_module_ack overflow, drop {parsed_at_ack.command!r}"
)
self._dispatch(parsed)
self._dispatch(parsed_at_ack)
@staticmethod
def _parse(frame: bytes) -> ATResponse:
@ -865,26 +911,27 @@ class ATCommandHandler:
handler = self.handlers.get(response.command)
if handler:
handler(response.data)
handler(response)
else:
logger.debug(
f"[{self.serial_port}] 未處理的 AT 指令: "
f"{response.command.decode()}"
)
def _handle_rssi(self, data: bytes):
def _handle_rssi(self, response: ATResponse):
"""處理 DB (RSSI) 回應：單 byte 無號值，單位 dBm"""
pass
if data:
print(f"[{self.serial_port}] RSSI = -{data[0]} dBm") # dev
# logger.debug(f"[{self.serial_port}] RSSI = -{data[0]} dBm") # dev
pass
def _handle_serial_high(self, data: bytes):
# print(f"[{self.serial_port}] RSSI = -{data[0]} dBm") # dev
logger.debug(f"[{self.serial_port}] RSSI = -{response.data[0]} dBm") # dev
def _handle_serial_high(self, response: ATResponse):
"""處理 SH (Serial Number High)"""
pass
def _handle_serial_low(self, data: bytes):
def _handle_serial_low(self, response: ATResponse):
"""處理 SL (Serial Number Low)"""
pass
@ -1246,7 +1293,7 @@ class serial_manager:
def send_at_command(self, serial_id, request: ATRequest) -> bool:
"""
對指定 serial_id 的 XBee dongle 發送一筆 AT 指令（thread-safe）
對指定 serial_id 的 XBee dongle 發送一筆 AT 指令 (thread-safe)
- serial_id: create_serial_link 取得的編號
- request: ATRequest 物件，攜帶 command / parameter / frame_id
回傳是否成功排進事件圈
@ -1327,46 +1374,51 @@ if __name__ == '__main__':
# UDP_REMOTE_PORT = 14571
# sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.STRAIGHT)
# # 測試項二
# SERIAL_PORT = '/dev/ttyUSB0' # 手動指定
# SERIAL_BAUDRATE = 115200
# UDP_REMOTE_PORT = 14561
# sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
# 測試項二
print("運行 測試項二")
SERIAL_PORT = '/dev/ttyUSB0' # 手動指定
SERIAL_BAUDRATE = 115200
UDP_REMOTE_PORT = 14561
sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
serial_id = 1
device_sys_id = 10
# linked_serial = sm.get_serial_link()
# print(linked_serial)
linked_serial = sm.get_serial_link()
print(f"連結完成 : {linked_serial}. 等待兩秒")
# # 等 connection_made 完成 writer 注入，再發一筆 AT 指令測試
# time.sleep(5)
# rssi_request = ATRequest(command=b'DB', parameter=b'', frame_id=0x52)
# for i in range(60):
# sm.send_at_command(1, rssi_request)
# time.sleep(1)
# 等 connection_made 完成 writer 注入，再發一筆 AT 指令測試
time.sleep(2)
rssi_request = ATRequest(command=b'DB', parameter=b'', frame_id=device_sys_id)
print(f"手動送出 DB AT Command:")
for i in range(20):
sm.send_at_command(1, rssi_request)
time.sleep(1)
# sm.remove_serial_link(1)
# time.sleep(3)
# sm.shutdown()
sm.remove_serial_link(1)
time.sleep(2)
sm.shutdown()
print("結束運行")
# # 測試項三
SERIAL_PORT = '/dev/ttyUSB0'
SERIAL_BAUDRATE = 115200
UDP_REMOTE_PORT = 14561
sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI_espv1)
# SERIAL_PORT = '/dev/ttyUSB0'
# SERIAL_BAUDRATE = 115200
# UDP_REMOTE_PORT = 14561
# sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI_espv1)
time.sleep(2) # 等 serial 連線與 operator 啟動
# 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)
print(processor.get_status_snapshot())
print(processor.get_gcs_queue_byte_count())
sm.remove_serial_link(serial_id)
time.sleep(3)
sm.shutdown()
# 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)
# print(processor.get_status_snapshot())
# print(processor.get_gcs_queue_byte_count())
# sm.remove_serial_link(serial_id)
# time.sleep(30)
# sm.shutdown()
'''
================= 改版記錄 ============================

23
src/fc_network_adapter/fc_network_adapter/utils/pollStrategy.py
Unescape Escape View File

@ -46,26 +46,3 @@ def pick_next(
return selected_device.address_64, grant_bytes
def estimate_poll_timeout(
grant_bytes: int,
baudrate: int,
guard_milliseconds: int = 20,
) -> float:
"""
估算 POLL 後等待 DONE 的超時秒數。
移植自 udptest8 estimate_tdma_timeout。
"""
grant_bytes = max(0, int(grant_bytes))
max_payload_per_chunk = 100
chunk_count = max(1, (grant_bytes + max_payload_per_chunk - 1) // max_payload_per_chunk)
uart_time = (grant_bytes + chunk_count * 18 + 32) * 10.0 / baudrate
timeout = (
uart_time
+ chunk_count * 0.012
+ (guard_milliseconds / 1000.0)
+ 0.35
)
return timeout

Write Preview
Loading…
Cancel
Save