diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index 99be627..3bdb19a 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -436,7 +436,7 @@ class ControlPanel: menu_stack.pop() idx_stack.pop() elif selected.action == "SET_SERIAL_COMM_XBEE_ESP": - state.serial_info_temp["CommunicationType"] = "XBee(API-API) espv1" + state.serial_info_temp["CommunicationType"] = "XBee(API-API)espv1" menu_stack.pop() idx_stack.pop() elif selected.action == "SET_SERIAL_COMM_TELEMETRY": @@ -1600,7 +1600,7 @@ class Orchestrator: # 定義通訊類型映射表 COMM_TYPE_MAP = { "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, # 新增區 } diff --git a/src/fc_network_adapter/fc_network_adapter/serialManager.py b/src/fc_network_adapter/fc_network_adapter/serialManager.py index 53714e8..990c7f9 100644 --- a/src/fc_network_adapter/fc_network_adapter/serialManager.py +++ b/src/fc_network_adapter/fc_network_adapter/serialManager.py @@ -37,7 +37,7 @@ from .utils import pollStrategy # ====================== 分割線 ===================== logger = setup_logger(os.path.basename(__file__)) -MODULE_VER = "2.00" +MODULE_VER = "2.02" rx_module_ack = RingBuffer(capacity=64, buffer_id=253) @@ -145,12 +145,15 @@ 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' + 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 + + # ---- 對外契約 ---- def process_incoming(self, data: bytes) -> bytes: """處理 XBee API 幀並提取 payload""" @@ -231,7 +234,7 @@ class XBeeFrameProcessor_Base(FrameProcessor): def _encapsulate( data: bytes, dest_addr64: bytes = DEST_ADDR64_BRAODCAST, - dest_addr16 = DEST_ADDR16_BRAODCAST, + dest_addr16: bytes = DEST_ADDR16_BRAODCAST, frame_id: int = 0x01, ) -> bytes: """ @@ -247,6 +250,8 @@ class XBeeFrameProcessor_Base(FrameProcessor): frame += dest_addr64 + dest_addr16 frame += struct.pack(">BB", broadcast_radius, options) + data 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) @staticmethod @@ -290,10 +295,7 @@ 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): @@ -306,6 +308,8 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base): self.last_done_time = 0.0 # 最後送出Done的時間 self.received_len = 0 # 收到封包累計 + + def __init__(self, at_handler: "ATCommandHandler" = None): super().__init__(at_handler) @@ -316,6 +320,10 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base): self.operator_busy = 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.event_loop: Optional[asyncio.AbstractEventLoop] = None self.serial_baudrate = 115200 @@ -329,8 +337,8 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base): 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.mavPack_interval_timeout = 150 # ms，poll 期間 MAVLink/DONE 最大閒置間隔 + self.discovery_interval_seconds = 200.0 # 每次做 discovery 程序的間隔時間 self.device_offline_timeout = self.discovery_interval_seconds * 2 # 遠端沒有回應會被踢出 超時時限 self.operator_tick_interval_seconds = 0.03 # self.guard_milliseconds = 50 # POLL DONE 的保底時間間隔 @@ -390,12 +398,12 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base): return None if frame_type == self.FRAME_TYPE_TX_STATUS: - length = (frame[1] << 8) | frame[2] - logger.debug( - 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]}" - ) + # length = (frame[1] << 8) | frame[2] + # logger.debug( + # 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]}" + # ) return None logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}") @@ -404,7 +412,8 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base): # ---- DISC / POLL 封裝 ---- 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 訊息 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: return + + system_id, sent_length, remain_length = struct.unpack('>BHH', payload[4:9]) + remote_device.remain_bytes = remain_length + # 這段是有問題的 因為會有整數封包切割問題 以及載具端的 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} " @@ -461,7 +473,6 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base): # TODO 傳送速率 # TODO 累積速率預測 remote_device.received_len = 0 - remote_device.remain_bytes = remain_length remote_device.last_done_time = time.time() if ( @@ -582,12 +593,9 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base): def _should_run_discovery(self) -> bool: # 條件1. 目前沒有任何遠端ESP裝置被紀錄 或者 手動啟動 if (not self.esp32_address_mapping) or (self.pending_manual_discovery): - return True + return (time.time() - self.last_discovery_time) > 2 # 條件2. 每個固定週期 會做一次 - return ( - time.time() - self.last_discovery_time - >= self.discovery_interval_seconds - ) + return (time.time() - self.last_discovery_time) > self.discovery_interval_seconds # ---- 手動請求（thread-safe 對外 API）---- @@ -754,19 +762,20 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base): 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) + idle_timeout_sec = self.mavPack_interval_timeout / 1000.0 + poll_tick = min(0.02, idle_timeout_sec) 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 + # try: + await asyncio.wait_for( + self.poll_done_event.wait(), + timeout=poll_tick, + ) + + # except asyncio.TimeoutError: + # continue return True # poll 程序 @@ -1374,51 +1383,51 @@ if __name__ == '__main__': # UDP_REMOTE_PORT = 14571 # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.STRAIGHT) - # 測試項二 - 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 + # # 測試項二 + # 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(f"連結完成 : {linked_serial}. 等待兩秒") + # linked_serial = sm.get_serial_link() + # print(f"連結完成 : {linked_serial}. 等待兩秒") - # 等 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) + # # 等 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(2) - sm.shutdown() - print("結束運行") + # 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(30) - # 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()) + + time.sleep(120) + sm.remove_serial_link(serial_id) + sm.shutdown() ''' ================= 改版記錄 ============================