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

Compare commits

merge into: chiyu1468:master
Branches Tags
chiyu1468:chiyu
chiyu1468:master
chiyu1468:wenchun
chiyu1468:chiyuTemp
chiyu1468:lunu
chiyu1468:ken
...
pull from: chiyu1468:chiyuTemp
Branches Tags
chiyu1468:chiyu
chiyu1468:master
chiyu1468:wenchun
chiyu1468:chiyuTemp
chiyu1468:lunu
chiyu1468:ken

1 Commits
master ... chiyuTemp

Author SHA1 Message Date
ken910606 a2c1d23f3d temp 1 month ago
2 changed files with 82 additions and 73 deletions
Show Stats

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

@ -1600,7 +1600,7 @@ class Orchestrator:
# 定義通訊類型映射表 # 定義通訊類型映射表
COMM_TYPE_MAP = { COMM_TYPE_MAP = {
"XBee(API-AT)": sm.SerialMode.XBEEAPI2AT, "XBee(API-AT)": sm.SerialMode.XBEEAPI2AT,
"XBee(API-AT)": sm.SerialMode.XBEEAPI_espv1, "XBee(API-API)espv1": sm.SerialMode.XBEEAPI_espv1,
"TELEMETRY": sm.SerialMode.STRAIGHT, "TELEMETRY": sm.SerialMode.STRAIGHT,
# 新增區 # 新增區
} }

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

@ -37,7 +37,7 @@ from .utils import pollStrategy
# ====================== 分割線 ===================== # ====================== 分割線 =====================
logger = setup_logger(os.path.basename(__file__)) logger = setup_logger(os.path.basename(__file__))
MODULE_VER = "2.00" MODULE_VER = "2.02"
rx_module_ack = RingBuffer(capacity=64, buffer_id=253) rx_module_ack = RingBuffer(capacity=64, buffer_id=253)
@ -147,10 +147,13 @@ class XBeeFrameProcessor_Base(FrameProcessor):
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): def __init__(self, at_handler: "ATCommandHandler" = None):
super().__init__() super().__init__()
self.at_handler = at_handler self.at_handler = at_handler
# ---- 對外契約 ---- # ---- 對外契約 ----
def process_incoming(self, data: bytes) -> bytes: def process_incoming(self, data: bytes) -> bytes:
"""處理 XBee API 幀並提取 payload""" """處理 XBee API 幀並提取 payload"""
@ -231,7 +234,7 @@ class XBeeFrameProcessor_Base(FrameProcessor):
def _encapsulate( def _encapsulate(
data: bytes, data: bytes,
dest_addr64: bytes = DEST_ADDR64_BRAODCAST, dest_addr64: bytes = DEST_ADDR64_BRAODCAST,
dest_addr16 = DEST_ADDR16_BRAODCAST, dest_addr16: bytes = DEST_ADDR16_BRAODCAST,
frame_id: int = 0x01, frame_id: int = 0x01,
) -> bytes: ) -> bytes:
""" """
@ -247,6 +250,8 @@ class XBeeFrameProcessor_Base(FrameProcessor):
frame += dest_addr64 + dest_addr16 frame += dest_addr64 + dest_addr16
frame += struct.pack(">BB", broadcast_radius, options) + data frame += struct.pack(">BB", broadcast_radius, options) + data
checksum = 0xFF - (sum(frame) & 0xFF) checksum = 0xFF - (sum(frame) & 0xFF)
# ret = b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
# logger.debug(ret.hex())
return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum) return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
@staticmethod @staticmethod
@ -290,10 +295,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
MAX_PAYLOAD_PER_FRAME = 80 MAX_PAYLOAD_PER_FRAME = 80
CHUNK_SEND_INTERVAL_SEC = 0.01 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: class Esp32DeviceInfo:
def __init__(self, system_id, address_64, last_hello_time): def __init__(self, system_id, address_64, last_hello_time):
@ -306,6 +308,8 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
self.last_done_time = 0.0 # 最後送出Done的時間 self.last_done_time = 0.0 # 最後送出Done的時間
self.received_len = 0 # 收到封包累計 self.received_len = 0 # 收到封包累計
def __init__(self, at_handler: "ATCommandHandler" = None): def __init__(self, at_handler: "ATCommandHandler" = None):
super().__init__(at_handler) super().__init__(at_handler)
@ -316,6 +320,10 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
self.operator_busy = False self.operator_busy = False
self.operator_running = False self.operator_running = False
# ADDR16 選項
self.DEST_ADDR16_BRAODCAST = b'\xFF\xFE'
self.DEST_ADDR64_BRAODCAST = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'
self.serial_writer: Optional[Callable[[bytes], None]] = None self.serial_writer: Optional[Callable[[bytes], None]] = None
self.event_loop: Optional[asyncio.AbstractEventLoop] = None self.event_loop: Optional[asyncio.AbstractEventLoop] = None
self.serial_baudrate = 115200 self.serial_baudrate = 115200
@ -329,8 +337,8 @@ 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.MAX_mavPack_interval_timeout = 100 # ms，poll 期間 MAVLink/DONE 最大閒置間隔 self.mavPack_interval_timeout = 150 # ms，poll 期間 MAVLink/DONE 最大閒置間隔
self.discovery_interval_seconds = 30.0 # 每次做 discovery 程序的間隔時間 self.discovery_interval_seconds = 200.0 # 每次做 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.03 # self.operator_tick_interval_seconds = 0.03 #
self.guard_milliseconds = 50 # POLL DONE 的保底時間間隔 self.guard_milliseconds = 50 # POLL DONE 的保底時間間隔
@ -390,12 +398,12 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
return None return None
if frame_type == self.FRAME_TYPE_TX_STATUS: if frame_type == self.FRAME_TYPE_TX_STATUS:
length = (frame[1] << 8) | frame[2] # length = (frame[1] << 8) | frame[2]
logger.debug( # logger.debug(
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]}"
) # )
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}")
@ -404,7 +412,8 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
# ---- DISC / POLL 封裝 ---- # ---- DISC / POLL 封裝 ----
def pack_discovery(self) -> bytes: def pack_discovery(self) -> bytes:
return self._encapsulate(self.DISC_HEADER, 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=0x00)
# 處理每個裝置回傳的 Hello 訊息 # 處理每個裝置回傳的 Hello 訊息
def handle_hello_report(self, payload: bytes, sender_address_64: bytes) -> None: def handle_hello_report(self, payload: bytes, sender_address_64: bytes) -> None:
@ -450,8 +459,11 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
if remote_device is None: if remote_device is None:
return return
system_id, sent_length, remain_length = struct.unpack('>BHH', payload[4:9])
remote_device.remain_bytes = remain_length
# 這段是有問題的 因為會有整數封包切割問題 以及載具端的 buffer 存量不足 故回傳的資訊量會與要求的不一致 # 這段是有問題的 因為會有整數封包切割問題 以及載具端的 buffer 存量不足 故回傳的資訊量會與要求的不一致
# system_id, sent_length, remain_length = struct.unpack('>BHH', payload[4:9])
# if sent_length != remote_device.received_len: # if sent_length != remote_device.received_len:
# logger.info( # logger.info(
# f"POLL may be missing packets sent={sent_length} " # f"POLL may be missing packets sent={sent_length} "
@ -461,7 +473,6 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
# TODO 傳送速率 # TODO 傳送速率
# TODO 累積速率預測 # TODO 累積速率預測
remote_device.received_len = 0 remote_device.received_len = 0
remote_device.remain_bytes = remain_length
remote_device.last_done_time = time.time() remote_device.last_done_time = time.time()
if ( if (
@ -582,12 +593,9 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
def _should_run_discovery(self) -> bool: def _should_run_discovery(self) -> bool:
# 條件1. 目前沒有任何遠端ESP裝置被紀錄 或者 手動啟動 # 條件1. 目前沒有任何遠端ESP裝置被紀錄 或者 手動啟動
if (not self.esp32_address_mapping) or (self.pending_manual_discovery): if (not self.esp32_address_mapping) or (self.pending_manual_discovery):
return True return (time.time() - self.last_discovery_time) > 2
# 條件2. 每個固定週期 會做一次 # 條件2. 每個固定週期 會做一次
return ( return (time.time() - self.last_discovery_time) > self.discovery_interval_seconds
time.time() - self.last_discovery_time
>= self.discovery_interval_seconds
)
# ---- 手動請求（thread-safe 對外 API）---- # ---- 手動請求（thread-safe 對外 API）----
@ -754,19 +762,20 @@ 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.MAX_mavPack_interval_timeout / 1000.0 idle_timeout_sec = self.mavPack_interval_timeout / 1000.0
poll_tick = min(0.02, idle_timeout_sec / 2) poll_tick = min(0.02, idle_timeout_sec)
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:
return False return False
try: # try:
await asyncio.wait_for( await asyncio.wait_for(
self.poll_done_event.wait(), self.poll_done_event.wait(),
timeout=poll_tick, timeout=poll_tick,
) )
except asyncio.TimeoutError:
continue # except asyncio.TimeoutError:
# continue
return True return True
# poll 程序 # poll 程序
@ -1374,51 +1383,51 @@ if __name__ == '__main__':
# UDP_REMOTE_PORT = 14571 # UDP_REMOTE_PORT = 14571
# sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.STRAIGHT) # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.STRAIGHT)
# 測試項二 # # 測試項二
print("運行 測試項二") # print("運行 測試項二")
SERIAL_PORT = '/dev/ttyUSB0' # 手動指定 # SERIAL_PORT = '/dev/ttyUSB0' # 手動指定
SERIAL_BAUDRATE = 115200 # SERIAL_BAUDRATE = 115200
UDP_REMOTE_PORT = 14561 # UDP_REMOTE_PORT = 14561
sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT) # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
serial_id = 1 # serial_id = 1
device_sys_id = 10 # device_sys_id = 10
linked_serial = sm.get_serial_link() # linked_serial = sm.get_serial_link()
print(f"連結完成 : {linked_serial}. 等待兩秒") # print(f"連結完成 : {linked_serial}. 等待兩秒")
# 等 connection_made 完成 writer 注入，再發一筆 AT 指令測試 # # 等 connection_made 完成 writer 注入，再發一筆 AT 指令測試
time.sleep(2) # time.sleep(2)
rssi_request = ATRequest(command=b'DB', parameter=b'', frame_id=device_sys_id) # rssi_request = ATRequest(command=b'DB', parameter=b'', frame_id=device_sys_id)
print(f"手動送出 DB AT Command:") # print(f"手動送出 DB AT Command:")
for i in range(20): # for i in range(20):
sm.send_at_command(1, rssi_request) # sm.send_at_command(1, rssi_request)
time.sleep(1) # time.sleep(1)
sm.remove_serial_link(1) # sm.remove_serial_link(1)
time.sleep(2) # time.sleep(2)
sm.shutdown() # sm.shutdown()
print("結束運行") # print("結束運行")
# # 測試項三 # 測試項三
# SERIAL_PORT = '/dev/ttyUSB0' SERIAL_PORT = '/dev/ttyUSB0'
# SERIAL_BAUDRATE = 115200 SERIAL_BAUDRATE = 115200
# UDP_REMOTE_PORT = 14561 UDP_REMOTE_PORT = 14561
# sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI_espv1) 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 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() 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())
# print(processor.get_gcs_queue_byte_count()) print(processor.get_gcs_queue_byte_count())
# sm.remove_serial_link(serial_id) time.sleep(120)
# time.sleep(30) sm.remove_serial_link(serial_id)
# sm.shutdown() sm.shutdown()
''' '''
================= 改版記錄 ============================ ================= 改版記錄 ============================

Write Preview
Loading…
Cancel
Save