(modify) serialManager 架構調整 詳見改版記錄

README.md

@@ -1,7 +1,5 @@
 這是天雷系統的專案
 
-
-
 ===
 ## 功能簡介
 1. mavlink 多對多支援平台

src/GUI/communication.py

@@ -419,13 +419,8 @@ class WebSocketMavlinkReceiver(threading.Thread):
 
 class DroneMonitor(Node):
     # Subscribe to drone ROS2 topics
-    _instance = None  # Singleton pattern to prevent duplicate nodes
-    
     def __init__(self):
-        # Use a unique node name with timestamp to avoid conflicts on restart
-        import time
-        node_name = f'drone_monitor_{int(time.time() * 1000) % 100000}'
-        super().__init__(node_name)
+        super().__init__('drone_monitor')
         self.signals = DroneSignals()
         self.drone_topics = {}
         self.lock = Lock()
@@ -466,17 +461,6 @@ class DroneMonitor(Node):
         # ================================================================================
         self.serial_receivers = []
         
-        # ================================================================================
-        # 【新增】初始化 CommandLongClient（持久化，不會每次調用都創建/銷毀）
-        # ================================================================================
-        self.command_long_client = None
-        try:
-            self.command_long_client = CommandLongClient()
-        except Exception as e:
-            print(f"⚠️ 警告: 無法初始化 CommandLongClient: {e}")
-            self.command_long_client = None
-        # ================================================================================
-        
         # 主题检测定时器
         self.create_timer(1.0, self.scan_topics)
     
@@ -602,7 +586,7 @@ class DroneMonitor(Node):
     # ================================================================================
 
     async def set_mode(self, drone_id, mode_name):
-        """使用 CommandLongClient 切換無人機飛行模式（使用非阻塞的 async 方法）"""
+        """使用 CommandLongClient 切換無人機飛行模式"""
         try:
             # 解析 drone_id 提取 sysid
             parts = drone_id.split('_')
@@ -619,24 +603,30 @@ class DroneMonitor(Node):
             
             print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})")
             
-            if not self.command_long_client:
-                print(f"❌ [SET_MODE] CommandLongClient 未初始化")
-                return False
+            # 在線程池中調用 CommandLongClient
+            loop = asyncio.get_event_loop()
+            executor = ThreadPoolExecutor(max_workers=1)
             
-            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
-            result = await self.command_long_client.change_mode_async(
-                target_sysid=sysid,
-                custom_mode=float(custom_mode),
-                target_compid=0,
-                base_mode=1.0,
-                timeout_sec=2.0,
-            )
+            def _call_set_mode():
+                client = CommandLongClient() if CommandLongClient else None
+                if not client:
+                    return False
+                result = client.change_mode(
+                    target_sysid=sysid,
+                    custom_mode=float(custom_mode),
+                    target_compid=0,
+                    base_mode=1.0,
+                    timeout_sec=2.0,
+                )
+                client.destroy_node()
+                return result.success if result else False
             
-            if result and result.success:
+            result = await loop.run_in_executor(executor, _call_set_mode)
+            if result:
                 print(f"✅ [SET_MODE] 模式切換成功")
                 return True
             else:
-                print(f"❌ [SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})")
+                print(f"❌ [SET_MODE] 模式切換失敗")
                 return False
         except Exception as e:
             print(f"❌ [SET_MODE] 例外錯誤: {e}")
@@ -644,7 +634,7 @@ class DroneMonitor(Node):
             return False
 
     async def arm_drone(self, drone_id, arm):
-        """使用 CommandLongClient 執行 ARM/DISARM（使用非阻塞的 async 方法）"""
+        """使用 CommandLongClient 執行 ARM/DISARM"""
         try:
             # 解析 drone_id 提取 sysid
             parts = drone_id.split('_')
@@ -656,23 +646,29 @@ class DroneMonitor(Node):
             action_name = "解鎖" if arm else "上鎖"
             print(f"\n📢 [ARM] {drone_id} → {action_name}")
             
-            if not self.command_long_client:
-                print(f"❌ [ARM] CommandLongClient 未初始化")
-                return False
+            # 在線程池中調用 CommandLongClient
+            loop = asyncio.get_event_loop()
+            executor = ThreadPoolExecutor(max_workers=1)
             
-            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
-            result = await self.command_long_client.arm_disarm_async(
-                target_sysid=sysid,
-                arm=arm,
-                target_compid=0,
-                timeout_sec=2.0,
-            )
+            def _call_arm_disarm():
+                client = CommandLongClient() if CommandLongClient else None
+                if not client:
+                    return False
+                result = client.arm_disarm(
+                    target_sysid=sysid,
+                    arm=arm,
+                    target_compid=0,
+                    timeout_sec=2.0,
+                )
+                client.destroy_node()
+                return result.success if result else False
             
-            if result and result.success:
+            result = await loop.run_in_executor(executor, _call_arm_disarm)
+            if result:
                 print(f"✅ [ARM] {action_name}成功")
                 return True
             else:
-                print(f"❌ [ARM] {action_name}失敗 (message={result.message if result else 'None'})")
+                print(f"❌ [ARM] {action_name}失敗")
                 return False
         except Exception as e:
             print(f"❌ [ARM] 例外錯誤: {e}")
@@ -680,7 +676,7 @@ class DroneMonitor(Node):
             return False
 
     async def takeoff_drone(self, drone_id, altitude):
-        """使用 CommandLongClient 執行無人機起飛（使用非阻塞的 async 方法）"""
+        """使用 CommandLongClient 執行無人機起飛"""
         try:
             # 解析 drone_id 提取 sysid
             parts = drone_id.split('_')
@@ -691,23 +687,29 @@ class DroneMonitor(Node):
             
             print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)")
             
-            if not self.command_long_client:
-                print(f"❌ [TAKEOFF] CommandLongClient 未初始化")
-                return False
+            # 在線程池中調用 CommandLongClient
+            loop = asyncio.get_event_loop()
+            executor = ThreadPoolExecutor(max_workers=1)
             
-            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
-            result = await self.command_long_client.takeoff_async(
-                target_sysid=sysid,
-                altitude_m=float(altitude),
-                target_compid=0,
-                timeout_sec=2.0,
-            )
+            def _call_takeoff():
+                client = CommandLongClient() if CommandLongClient else None
+                if not client:
+                    return False
+                result = client.takeoff(
+                    target_sysid=sysid,
+                    altitude_m=float(altitude),
+                    target_compid=0,
+                    timeout_sec=2.0,
+                )
+                client.destroy_node()
+                return result.success if result else False
             
-            if result and result.success:
+            result = await loop.run_in_executor(executor, _call_takeoff)
+            if result:
                 print(f"✅ [TAKEOFF] 起飛成功")
                 return True
             else:
-                print(f"❌ [TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
+                print(f"❌ [TAKEOFF] 起飛失敗")
                 return False
         except Exception as e:
             print(f"❌ [TAKEOFF] 例外錯誤: {e}")

src/GUI/gui.py

@@ -33,7 +33,7 @@ from mission_group import (
 # ================================================================================
 
 class ControlStationUI(QMainWindow):
-    VERSION = '2.0.6'
+    VERSION = '2.0.3'
     
     def __init__(self):
         super().__init__()
@@ -52,10 +52,6 @@ class ControlStationUI(QMainWindow):
         self.executor = rclpy.executors.SingleThreadedExecutor()
         self.executor.add_node(self.monitor)
         
-        # 添加 CommandLongClient 到執行器（這樣它的回調才能被處理）
-        if self.monitor.command_long_client:
-            self.executor.add_node(self.monitor.command_long_client)
-        
         # 定时处理ROS事件
         self.timer = QTimer()
         self.timer.timeout.connect(self.spin_ros)
@@ -1720,15 +1716,6 @@ class ControlStationUI(QMainWindow):
                 receiver.stop()
             for receiver in self.monitor.ws_receivers:
                 receiver.stop()
-            # Clean up serial receivers
-            for receiver in self.monitor.serial_receivers:
-                receiver.stop()
-            # Clean up CommandLongClient
-            if self.monitor.command_long_client:
-                try:
-                    self.monitor.command_long_client.destroy_node()
-                except:
-                    pass
             self.monitor.destroy_node()
             self.executor.shutdown()
         except Exception as e:

src/fc_network_adapter/fc_network_adapter/serialManager.py

@@ -38,7 +38,7 @@ class SerialMode(Enum):
     NOT_USE = auto()       # 不使用
 
 
-# ====================== Frame Processor 基類與實現 =====================
+# ====================== Frame Processor Base and Implementation =====================
 
 class FrameProcessor(ABC):
     """協議處理器基類"""
@@ -47,10 +47,10 @@ class FrameProcessor(ABC):
         self.buffer = bytearray()
     
     @abstractmethod
-    def process_incoming(self, data: bytes):
+    def process_incoming(self, data: bytes) -> bytes:
         """
         處理接收到的數據
-        返回：已完整解析的 payload 列表
+        返回：已完整解析的 payload 列表 後續要丟到 UDP 去
         """
         pass
     
@@ -66,7 +66,7 @@ class FrameProcessor(ABC):
 class RawFrameProcessor(FrameProcessor):
     """原始數據直通處理器"""
     
-    def process_incoming(self, data: bytes):
+    def process_incoming(self, data: bytes) -> bytes:
         """直接返回原始數據，不進行緩衝"""
         return [data] if data else []
     
@@ -76,110 +76,106 @@ class RawFrameProcessor(FrameProcessor):
 
 
 class XBeeFrameProcessor(FrameProcessor):
-    """XBee API 協議處理器"""
-    
-    def process_incoming(self, data: bytes):
+    """
+    XBee API 協議處理器
+
+    職責：
+    - XBee API frame 的拆幀 / 組幀
+    - 0x90 (RX Packet)   -> 解出 payload 回傳
+    - 0x88 (AT Response) -> 轉交 at_handler 處理（若有注入）
+    - 0x8B (TX Status)   -> 目前忽略
+    - 其他 frame type    -> 記 warning 忽略
+
+    若未來要做變化型 XBee（例如 API2 escape mode、不同 addressing），
+    繼承此類並覆寫 _encapsulate / _decapsulate / _try_extract_frame 即可。
+    """
+
+    # XBee API frame type
+    FRAME_TYPE_TX_REQUEST  = 0x10
+    FRAME_TYPE_AT_RESPONSE = 0x88
+    FRAME_TYPE_TX_STATUS   = 0x8B
+    FRAME_TYPE_RX_PACKET   = 0x90
+
+    def __init__(self, at_handler: "ATCommandHandler" = None):
+        super().__init__()
+        self.at_handler = at_handler
+
+    # ---- 對外契約 ----
+    def process_incoming(self, data: bytes) -> bytes:
         """處理 XBee API 幀並提取 payload"""
         self.buffer.extend(data)
         payloads = []
-        
+
+        while True:
+            frame = self._try_extract_frame()
+            if frame is None:
+                break
+            payload = self._dispatch_frame(frame)
+            if payload:
+                payloads.append(payload)
+
+        return payloads
+
+    def process_outgoing(self, data: bytes) -> bytes:
+        """將數據封裝為 XBee API 傳輸幀"""
+        return self._encapsulate(data)
+
+    # ---- 內部：拆幀與分派 ----
+    def _try_extract_frame(self) -> bytes:
+        """
+        從 buffer 嘗試抓一個完整 frame
+        - 對齊幀頭 0x7E
+        - 長度不足時等待下次進 buffer
+        - 成功則從 buffer 切掉並回傳整個 frame bytes
+        """
         while len(self.buffer) >= 3:
-            # 尋找幀頭
             if self.buffer[0] != 0x7E:
                 self.buffer.pop(0)
                 continue
-            
-            # 讀取 payload 長度
+
             length = (self.buffer[1] << 8) | self.buffer[2]
             full_length = 3 + length + 1  # 起始符(1) + 長度(2) + payload + 校驗和(1)
-            
-            # 等待完整幀
+
             if len(self.buffer) < full_length:
-                break
-            
-            # 提取完整 frame 並從緩衝區移除
+                return None
+
             frame = bytes(self.buffer[:full_length])
             del self.buffer[:full_length]
-            
-            # 判斷 frame 類型並處理
-            frame_type = frame[3]
-            
-            if frame_type == 0x90:  # RX Packet
-                payload = XBeeFrameHandler.decapsulate_data(frame)
-                if payload:
-                    payloads.append(payload)
-            elif frame_type == 0x88:  # AT Response
-                # 可以在這裡處理 AT 指令回應
-                # response = XBeeFrameHandler.parse_at_command_response(frame)
-                # 目前忽略
-                pass
-            elif frame_type == 0x8B:  # Transmit Status
-                # 傳輸狀態，目前忽略
-                pass
-            else:
-                logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}")
-        
-        return payloads
-    
-    def process_outgoing(self, data: bytes) -> bytes:
-        """將數據封裝為 XBee API 傳輸幀"""
-        return XBeeFrameProcessor.encapsulate_data(data)
+            return frame
 
-
-# ====================== XBee Frame Handler =====================
-
-class XBeeFrameHandler:
-    """XBee API Frame 處理器"""
-    
-    @staticmethod
-    def parse_at_command_response(frame: bytes) -> dict:
-        """解析 AT Command Response (0x88)"""
-        if len(frame) < 8:
-            return None
-        
-        frame_type = frame[3]
-        if frame_type != 0x88:
-            return None
-        
-        frame_id = frame[4]
-        at_command = frame[5:7]
-        status = frame[7]
-        data = frame[8:] if len(frame) > 8 else b''
-        
-        return {
-            'frame_id': frame_id,
-            'command': at_command,
-            'status': status,
-            'data': data,
-            'is_ok': status == 0x00
-        }
-    
-    @staticmethod
-    def parse_receive_packet(frame: bytes) -> dict:
-        """解析 RX Packet (0x90) - 未來擴展用"""
-        # if len(frame) < 15 or frame[3] != 0x90:
-        #     return None
-        
-        # return {
-        #     'source_addr': frame[4:12],
-        #     'reserved': frame[12:14],
-        #     'options': frame[14],
-        #     'data': frame[15:-1]
-        # }
-        pass
         return None
 
+    def _dispatch_frame(self, frame: bytes) -> bytes:
+        """根據 frame type 分派；若是 RX payload 回傳 bytes，其餘回傳 None"""
+        frame_type = frame[3]
+
+        if frame_type == self.FRAME_TYPE_RX_PACKET:
+            return self._decapsulate(frame)
+
+        if frame_type == self.FRAME_TYPE_AT_RESPONSE:
+            if self.at_handler is not None:
+                self.at_handler.handle_frame(frame)
+            return None
+
+        if frame_type == self.FRAME_TYPE_TX_STATUS:
+            return None
+
+        logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}")
+        return None
+
+    # ---- 內部：編解碼 ----
     @staticmethod
-    def encapsulate_data(data: bytes, dest_addr64: bytes = b'\x00\x00\x00\x00\x00\x00\xFF\xFF', frame_id = 0x01) -> bytes:
+    def _encapsulate(
+        data: bytes,
+        dest_addr64: bytes = b'\x00\x00\x00\x00\x00\x00\xFF\xFF',
+        frame_id: int = 0x01,
+    ) -> bytes:
         """
-        將數據封裝為 XBee API 傳輸幀
-        
-        使用 XBee API 格式封裝數據:
-        - 傳輸請求幀 (0x10)
+        將 payload 包成 XBee TX Request (0x10)
         - 使用廣播地址
         - 添加適當的頭部和校驗和
         """
-        frame_type = 0x10
+        frame_type = XBeeFrameProcessor.FRAME_TYPE_TX_REQUEST
         dest_addr16 = b'\xFF\xFE'
         broadcast_radius = 0x00
         options = 0x00
@@ -191,18 +187,24 @@ class XBeeFrameHandler:
         return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
 
     @staticmethod
-    def decapsulate_data(data: bytes):
-        
-        # 獲取數據長度 (不包括校驗和)
-        length = (data[1] << 8) | data[2]
-
+    def _decapsulate(frame: bytes) -> bytes:
+        """從 RX Packet (0x90) 取出 payload"""
+        length = (frame[1] << 8) | frame[2]
         rf_data_start = 3 + 12
-        return data[rf_data_start:3 + length]
+        return frame[rf_data_start:3 + length]
 
 
+# ====================== Dongle Command Handler =====================
+
 class ATCommandHandler:
-    """AT 指令回應處理器"""
-    
+    """
+    XBee AT 指令回應處理器
+
+    職責：
+    - 解析 AT Response frame (0x88)
+    - 依 AT 指令分派到對應的 handler
+    """
+
     def __init__(self, serial_port: str):
         self.serial_port = serial_port
         self.handlers = {
@@ -211,65 +213,93 @@ class ATCommandHandler:
             b'SL': self._handle_serial_low,
             # 可擴展其他 AT 指令
         }
-    
-    def handle_response(self, response: dict):
-        """根據 AT 指令類型分派處理"""
-        if not response or not response['is_ok']:
-            if response:
-                logger.warning(f"[{self.serial_port}] AT {response['command'].decode()} 失敗，狀態碼: {response['status']}")
+
+    def handle_frame(self, frame: bytes) -> None:
+        """接收一整個 AT Response frame，內部完成 parse + dispatch"""
+        parsed = self._parse(frame)
+        if parsed is None:
             return
-        
-        command = response['command']
-        handler = self.handlers.get(command)
-        
+        self._dispatch(parsed)
+
+    @staticmethod
+    def _parse(frame: bytes) -> dict:
+        """解析 AT Command Response (0x88)；不符格式回傳 None"""
+        if len(frame) < 8:
+            return None
+
+        if frame[3] != 0x88:
+            return None
+
+        frame_id = frame[4]
+        at_command = frame[5:7]
+        status = frame[7]
+        data = frame[8:] if len(frame) > 8 else b''
+
+        return {
+            'frame_id': frame_id,
+            'command': at_command,
+            'status': status,
+            'data': data,
+            'is_ok': status == 0x00,
+        }
+
+    def _dispatch(self, response: dict) -> None:
+        """根據 AT 指令類型分派處理"""
+        if not response['is_ok']:
+            logger.warning(
+                f"[{self.serial_port}] AT {response['command'].decode()} "
+                f"失敗，狀態碼: {response['status']}"
+            )
+            return
+
+        handler = self.handlers.get(response['command'])
         if handler:
             handler(response['data'])
         else:
-            logger.debug(f"[{self.serial_port}] 未處理的 AT 指令: {command.decode()}")
-    
+            logger.debug(
+                f"[{self.serial_port}] 未處理的 AT 指令: "
+                f"{response['command'].decode()}"
+            )
+
     def _handle_rssi(self, data: bytes):
         """處理 DB (RSSI) 回應"""
         # 未來可實現 RSSI 處理邏輯
         pass
-    
+
     def _handle_serial_high(self, data: bytes):
         """處理 SH (Serial Number High)"""
         pass
-    
+
     def _handle_serial_low(self, data: bytes):
         """處理 SL (Serial Number Low)"""
         pass
 
 
-# ====================== Serial Handler =====================
-
+# ================ Serial UDP Socket Object ==============
 class SerialHandler(asyncio.Protocol):
     """asyncio.Protocol 用於處理 Serial 收發"""
-    
+
     def __init__(self, udp_handler, serial_port_str, serial_mode: SerialMode):
         self.udp_handler = udp_handler  # UDP 的傳輸物件
         self.serial_port_str = serial_port_str
         self.serial_mode = serial_mode
         self.transport = None  # Serial 自己的傳輸物件
-        
-        # 根據模式創建對應的 processor
-        self.processor = self._create_processor(serial_mode)
-        
-        # AT 指令處理器（僅 XBee 模式使用）
-        if serial_mode == SerialMode.XBEEAPI2AT:
-            self.at_handler = ATCommandHandler(serial_port_str)
-        else:
-            self.at_handler = None
 
-    def _create_processor(self, serial_mode: SerialMode) -> FrameProcessor:
-        """工廠方法：根據模式創建處理器"""
+        # 根據模式建立 processor（需要 AT handler 時一併在工廠內建好注入）
+        self.processor = self._create_processor(serial_mode, serial_port_str)
+
+    @staticmethod
+    def _create_processor(serial_mode: SerialMode, serial_port_str: str) -> FrameProcessor:
+        """工廠方法：根據模式建立對應的 processor，必要時一併注入 AT handler"""
         if serial_mode == SerialMode.STRAIGHT:
             return RawFrameProcessor()
-        elif serial_mode == SerialMode.XBEEAPI2AT:
-            return XBeeFrameProcessor()
-        else:
-            logger.warning(f"Unknown serial mode: {serial_mode}, using Raw")
-            return RawFrameProcessor()
+
+        if serial_mode == SerialMode.XBEEAPI2AT:
+            at_handler = ATCommandHandler(serial_port_str)
+            return XBeeFrameProcessor(at_handler=at_handler)
+
+        logger.warning(f"Unknown serial mode: {serial_mode}, using Raw")
+        return RawFrameProcessor()
 
     def connection_made(self, transport):
         """連接建立時的回調"""
@@ -291,8 +321,6 @@ class SerialHandler(asyncio.Protocol):
             )
 
 
-# ====================== UDP Handler =====================
-
 class UDPHandler(asyncio.DatagramProtocol):
     """asyncio.DatagramProtocol 用於處理 UDP 收發"""
     
@@ -600,7 +628,7 @@ if __name__ == '__main__':
 
     SERIAL_PORT = '/dev/ttyUSB0'  # 手動指定
     SERIAL_BAUDRATE = 115200
-    UDP_REMOTE_PORT = 14571
+    UDP_REMOTE_PORT = 14561
     sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
 
     # SERIAL_PORT = '/dev/ttyACM0'  # 手動指定
@@ -615,3 +643,19 @@ if __name__ == '__main__':
     sm.remove_serial_link(1)
     time.sleep(3)
     sm.shutdown()
+
+
+
+'''
+================= 改版記錄 ============================
+2026.4.20
+1. XBeeFrameHandler 結構移除
+2. XBeeFrameProcessor 新增 _encapsulate, _decapsulate 編碼解碼 xbee 封包的功能 (原來在 XBeeFrameHandler 中)
+3. XBeeFrameProcessor 新增 _try_extract_frame 處理被可能截斷的 UART 封包
+4. XBeeFrameProcessor 新增 _dispatch_frame 分配封包到 UDP 或者 Dongle Command Handler 
+5. ATCommandHandler 新增 _parse 去拆解 0x88 AT Command Response
+6. ATCommandHandler 新增 _dispatch 把拆解的結果 分配到 _handle_XXX
+7. ATCommandHandler 新增各項 _handle_XXX (未實作)
+
+'''
+

src/fc_network_apps/longCommand.py

@@ -4,7 +4,6 @@ from __future__ import annotations
 
 from dataclasses import dataclass
 from typing import Optional
-import asyncio
 
 import rclpy
 from rclpy.node import Node
@@ -186,118 +185,3 @@ class CommandLongClient(Node):
             param7=0.0,
             timeout_sec=timeout_sec,
         )
-
-    # ============================================================================
-    # 【新增】非阻塞 async 包裝方法（用於 GUI 的非阻塞調用）
-    # 這些方法在 ThreadPoolExecutor 中運行同步版本，以避免阻塞事件循環
-    # ============================================================================
-
-    async def change_mode_async(
-        self,
-        *,
-        target_sysid: int,
-        custom_mode: float,
-        target_compid: int = 0,
-        base_mode: float = 1.0,
-        confirmation: int = 0,
-        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-    ) -> CommandLongResult:
-        """非阻塞 async 版本的 change_mode（在 ThreadPoolExecutor 中運行）"""
-        from concurrent.futures import ThreadPoolExecutor
-        loop = asyncio.get_event_loop()
-        executor = ThreadPoolExecutor(max_workers=1)
-        return await loop.run_in_executor(
-            executor,
-            lambda: self.change_mode(
-                target_sysid=target_sysid,
-                custom_mode=custom_mode,
-                target_compid=target_compid,
-                base_mode=base_mode,
-                confirmation=confirmation,
-                timeout_sec=timeout_sec,
-            )
-        )
-
-    async def arm_disarm_async(
-        self,
-        *,
-        target_sysid: int,
-        arm: bool,
-        target_compid: int = 0,
-        confirmation: int = 0,
-        param2: float = 0.0,
-        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-    ) -> CommandLongResult:
-        """非阻塞 async 版本的 arm_disarm（在 ThreadPoolExecutor 中運行）"""
-        from concurrent.futures import ThreadPoolExecutor
-        loop = asyncio.get_event_loop()
-        executor = ThreadPoolExecutor(max_workers=1)
-        return await loop.run_in_executor(
-            executor,
-            lambda: self.arm_disarm(
-                target_sysid=target_sysid,
-                arm=arm,
-                target_compid=target_compid,
-                confirmation=confirmation,
-                param2=param2,
-                timeout_sec=timeout_sec,
-            )
-        )
-
-    async def takeoff_async(
-        self,
-        *,
-        target_sysid: int,
-        altitude_m: float,
-        target_compid: int = 0,
-        min_pitch_deg: float = 0.0,
-        yaw_deg: float = 0.0,
-        latitude: Optional[float] = None,
-        longitude: Optional[float] = None,
-        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-    ) -> CommandLongResult:
-        """非阻塞 async 版本的 takeoff（在 ThreadPoolExecutor 中運行）"""
-        from concurrent.futures import ThreadPoolExecutor
-        loop = asyncio.get_event_loop()
-        executor = ThreadPoolExecutor(max_workers=1)
-        return await loop.run_in_executor(
-            executor,
-            lambda: self.takeoff(
-                target_sysid=target_sysid,
-                altitude_m=altitude_m,
-                target_compid=target_compid,
-                min_pitch_deg=min_pitch_deg,
-                yaw_deg=yaw_deg,
-                latitude=latitude,
-                longitude=longitude,
-                timeout_sec=timeout_sec,
-            )
-        )
-
-    async def land_async(
-        self,
-        *,
-        target_sysid: int,
-        target_compid: int = 0,
-        yaw_deg: float = 0.0,
-        latitude: Optional[float] = None,
-        longitude: Optional[float] = None,
-        altitude_m: float = 0.0,
-        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-    ) -> CommandLongResult:
-        """非阻塞 async 版本的 land（在 ThreadPoolExecutor 中運行）"""
-        from concurrent.futures import ThreadPoolExecutor
-        loop = asyncio.get_event_loop()
-        executor = ThreadPoolExecutor(max_workers=1)
-        return await loop.run_in_executor(
-            executor,
-            lambda: self.land(
-                target_sysid=target_sysid,
-                target_compid=target_compid,
-                yaw_deg=yaw_deg,
-                latitude=latitude,
-                longitude=longitude,
-                altitude_m=altitude_m,
-                timeout_sec=timeout_sec,
-            )
-        )

src/unitdev02/unitdev02/lifecycle_node.py

@@ -1,45 +0,0 @@
-
-
-
-import rclpy
-from rclpy.node import Node
-from std_msgs.msg import String
-import time
-
-# import mavros_msgs.srv
-
-class TalkerNode(Node):
-    def __init__(self):
-        start_time = time.time()
-        super().__init__('talker_node')
-        end_time = time.time()
-        print(f"Node initialization took {end_time - start_time:.2f} seconds")
-
-        self.publisher_ = self.create_publisher(String, 'hahatest/_1', 10)
-        self.timer = self.create_timer(1.0, self.timer_callback)  # 每秒執行一次
-        self.get_logger().info('TalkerNode has been started.')
-
-    def timer_callback(self):
-        msg = String()
-        msg.data = 'Hello, ROS 2!'
-        self.publisher_.publish(msg)
-        self.get_logger().info(f'Published: "{msg.data}"')
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = TalkerNode()
-
-    print("Before sleep")
-    time.sleep(5)  # 等待 5 秒鐘
-    print("After sleep")
-    try:
-        start_time = time.time()
-        while time.time() - start_time < 10:  # 持續 10 秒鐘
-            rclpy.spin_once(node)
-            time.sleep(1)  # 每秒執行一次
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-if __name__ == '__main__':
-    main()

src/unitdev02/unitdev02/ping_test.py

@@ -0,0 +1,188 @@
+import time
+from pymavlink import mavutil
+
+# # 強制 source_system=255
+# master = mavutil.mavlink_connection('udpout:localhost:14561', source_system=255)
+
+# print("Starting Force-Ping Test...")
+
+# while True:
+#     # 1. 發送 Heartbeat (證明連線活著)
+#     master.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS, 
+#                              mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0)
+    
+#     # 2. 強制發送 PING (target 1, 1)
+#     # 我們加一個計數器，方便在 tcpdump 裡辨識
+#     ping_seq = int(time.time()) % 100
+#     master.mav.ping_send(int(time.time() * 1e6), ping_seq, 1, 1)
+    
+#     print(f"Sent Heartbeat and PING (seq {ping_seq})")
+    
+#     # 3. 非阻塞讀取，避免卡死
+#     msg = master.recv_match(blocking=False)
+#     if msg:
+#         print(f"--> Received something: {msg.get_type()}")
+        
+#     time.sleep(1) # 每秒噴一次
+
+# -------------------------------------------------------------------
+
+# # 改成 udpin，這會讓你的程式守在 14561 等模擬器送資料進來
+# # 這樣你就能收到模擬器的 Heartbeat，建立連線後再發 PING
+# master = mavutil.mavlink_connection('udpin:localhost:14561', source_system=255)
+
+# print("Waiting for simulator heartbeat...")
+# # 這一行很重要：先收到 Heartbeat，pymavlink 才會設定好 target 地址
+# master.wait_heartbeat()
+# print(f"Heartbeat from system {master.target_system} component {master.target_component}")
+
+# while True:
+#     # 既然收到了，現在發 PING 就會順著原路回去
+#     master.mav.ping_send(int(time.time() * 1e6), 1, master.target_system, master.target_component)
+#     print("PING sent!")
+    
+#     msg = master.recv_match(type='PING', blocking=True, timeout=2)
+#     if msg:
+#         print(f"Got PING response: {msg}")
+#     time.sleep(1)
+
+
+# -------------------------------------------------------------------
+
+# import time
+# from pymavlink import mavutil
+
+# # 核心修改：因為 MAVProxy 正在往 14561 噴資料，我們必須「接住」它
+# # 使用 udpin 會讓你的程式在 14561 監聽
+# master = mavutil.mavlink_connection('udpin:localhost:14561', source_system=254)
+
+# print("正在等待 MAVProxy 轉發的 Heartbeat...")
+
+# # 只要這行跑過，代表你跟 MAVProxy 握手成功了
+# master.wait_heartbeat()
+# print(f"成功連線到系統 {master.target_system}！")
+
+# while True:
+#     # 1. 務必發送 Heartbeat，讓 MAVProxy 知道要把回應送回這個 Port
+#     master.mav.heartbeat_send(
+#         mavutil.mavlink.MAV_TYPE_GCS, 
+#         mavutil.mavlink.MAV_AUTOPILOT_INVALID, 
+#         0, 0, 0
+#     )
+
+#     # 2. 修改 target_component 為 1 (飛控核心)
+#     # 注意：這裡刻意將 seq 設為動態，方便你在 watch PING 裡觀察
+#     p_seq = int(time.time()) % 255
+#     master.mav.ping_send(
+#         int(time.time() * 1e6), 
+#         p_seq, 
+#         1, 
+#         1  # <--- 改成 1 試試看！
+#     )
+#     print(f"Sent PING seq {p_seq} to 1/1")
+
+#     # 3. 讀取所有訊息，看看有沒有 PING 回來
+#     # 有時候回應的 target_system 可能是 255 (你的 ID)
+#     msg = master.recv_match(type='PING', blocking=True, timeout=1.0)
+#     if msg:
+#         print(f"🎉 成功！收到回應: {msg}")
+    
+#     time.sleep(1)
+
+
+
+# -------------------------------------------------------------------
+
+
+# import time
+# from pymavlink import mavutil
+
+# # 建議一樣用 udpin 監聽 MAVProxy 的輸出
+# master = mavutil.mavlink_connection('udpin:localhost:14561', source_system=255)
+
+# def get_time_ns():
+#     return int(time.time() * 1e9) # 轉為奈秒
+
+# print("開始測試 TIMESYNC 響應...")
+
+# while True:
+#     now_ns = get_time_ns()
+    
+#     # 發送 TIMESYNC 請求
+#     # tc1=0 代表這是請求，ts1=當前時間
+#     master.mav.timesync_send(0, now_ns)
+    
+#     # 等待回應
+#     msg = master.recv_match(type='TIMESYNC', blocking=True, timeout=1.0)
+    
+#     if msg and msg.tc1 != 0:
+#         # 收到回應，msg.tc1 應該等於我們剛才送出的 now_ns
+#         rtt_ms = (get_time_ns() - msg.tc1) / 1e6
+#         print(f"🕒 TIMESYNC RTT: {rtt_ms:.3f} ms")
+#         break # 測試一次就好
+#     else:
+#         print("等待 TIMESYNC 回應中...")
+        
+#     time.sleep(1)
+
+# -------------------------------------------------------------------
+
+import time
+import statistics
+from pymavlink import mavutil
+
+# 連接到 MAVProxy 轉發的端口
+master = mavutil.mavlink_connection('udpin:localhost:14561', source_system=255)
+
+# 測試設定
+TARGET_SYS = 2  # 你想測試的載具 ID
+SAMPLES = 20    # 取 20 次樣本來算平均和抖動
+rtt_history = []
+
+print(f"開始測試對 System {TARGET_SYS} 的通道品質...")
+
+def get_time_ns():
+    return int(time.time() * 1e9)
+
+try:
+    while len(rtt_history) < SAMPLES:
+        ts1_send = get_time_ns()
+        
+        # 發送 TIMESYNC (V1/V2 格式相容)
+        # 針對特定系統發送
+        master.mav.timesync_send(0, ts1_send)
+        
+        # 等待回應 (過濾目標系統的訊息)
+        msg = master.recv_match(type='TIMESYNC', blocking=True, timeout=1.0)
+        
+        if msg and msg.get_srcSystem() == TARGET_SYS and msg.tc1 != 0:
+            ts1_recv = get_time_ns()
+            rtt_ns = ts1_recv - msg.tc1
+            rtt_ms = rtt_ns / 1e6
+            rtt_history.append(rtt_ms)
+            print(f"Sample {len(rtt_history)}: RTT = {rtt_ms:.3f} ms")
+        else:
+            print("Request timeout or mismatched ID...")
+            
+        time.sleep(0.1) # 快速採樣
+
+    # 4. 計算統計數據
+    avg_latency = statistics.mean(rtt_history)
+    std_dev = statistics.stdev(rtt_history) # 這就是 Jitter 的一種表現
+    max_rtt = max(rtt_history)
+    min_rtt = min(rtt_history)
+
+    print("\n--- 物理通道品質分析報告 ---")
+    print(f"目標載具  : System {TARGET_SYS}")
+    print(f"平均延遲 (Avg) : {avg_latency:.3f} ms")
+    print(f"最小延遲 (Min) : {min_rtt:.3f} ms")
+    print(f"最大延遲 (Max) : {max_rtt:.3f} ms")
+    print(f"抖動幅度 (Jitter): {std_dev:.3f} ms (標準差)")
+    
+    if std_dev > 10:
+        print("警告：網路抖動過高，可能影響控制穩定性！")
+    else:
+        print("通道品質：良好")
+
+except KeyboardInterrupt:
+    print("測試終止。")

src/unitdev02/unitdev02/pymav_nullbuffer.py

@@ -0,0 +1,46 @@
+from pymavlink.dialects.v20 import common as mavlink
+
+class NullBuffer:
+    def write(self, data): pass
+    def seek(self, pos): pass
+    def tell(self): return 0
+
+class CapturingBuffer:
+    def __init__(self):
+        self.last_data = b''
+    def write(self, data):
+        self.last_data = bytes(data)
+    def seek(self, pos): pass
+    def tell(self): return 0
+
+# 1. 初始化（僅作為編碼器）
+# file 參數可以是任何擁有 write() 方法的物件，這裡用 BytesIO 模擬
+# 初始化方法一：使用 BytesIO
+# import io
+# out_buf = io.BytesIO()
+# mav = mavlink.MAVLink(out_buf, srcSystem=1, srcComponent=191)
+
+# 初始化方法二：使用自定義的 NullBuffer
+mav = mavlink.MAVLink(NullBuffer(), srcSystem=1, srcComponent=191)
+mav.seq = 254
+
+# 2. 建立心跳包並取得二進制數據
+# 連續產出「不同」的訊息物件
+for i in range(3):
+    msg = mav.heartbeat_encode(mavlink.MAV_TYPE_GCS, 0, 0, 0, 0)
+    data = msg.pack(mav)
+    mav.seq = (mav.seq + 1) & 0xFF
+    # MAVLink 2 的序列號在第 3 個 Byte (Index 2)
+    print(f"第 {i} 次發送, Seq: {data[4]}, Raw: {data.hex()}")
+
+
+
+print("分隔線")
+
+buf = CapturingBuffer()
+mav_buf2 = mavlink.MAVLink(buf, srcSystem=1, srcComponent=191)
+
+for i in range(3):
+    mav_buf2.heartbeat_send(mavlink.MAV_TYPE_GCS, 0, 0, 0, 0)
+    data = buf.last_data
+    print(f"第 {i} 次發送, Seq: {data[4]}, Raw: {data.hex()}")

src/unitdev02/unitdev02/serial_udp_ad.py

@@ -0,0 +1,71 @@
+import asyncio
+import serial_asyncio
+
+SERIAL_PORT = '/dev/ttyUSB0'   # 修改為你的 serial port
+SERIAL_BAUDRATE = 115200       # 修改為你的 baudrate
+UDP_REMOTE_IP = '192.168.1.100' # 修改為目標 IP
+UDP_REMOTE_PORT = 5005         # 修改為目標 port
+UDP_LOCAL_PORT = 5006          # 本地 UDP 監聽 port
+
+class SerialToUDP(asyncio.Protocol):
+    def __init__(self, udp_transport):
+        self.udp_transport = udp_transport
+
+    def connection_made(self, transport):
+        self.transport = transport
+
+    def data_received(self, data):
+        # Serial 收到資料，轉發到 UDP
+        self.udp_transport.sendto(data, (UDP_REMOTE_IP, UDP_REMOTE_PORT))
+
+    def write_to_serial(self, data):
+        self.transport.write(data)
+
+class UDPToSerial(asyncio.DatagramProtocol):
+    def __init__(self, serial_proto):
+        self.serial_proto = serial_proto
+
+    def datagram_received(self, data, addr):
+        # UDP 收到資料，轉發到 Serial
+        self.serial_proto.write_to_serial(data)
+
+async def main():
+    loop = asyncio.get_running_loop()
+
+    # 定義協議工廠函數
+    def create_empty_protocol():
+        return asyncio.DatagramProtocol()
+
+    # 建立 UDP1 傳輸
+    udp_transport, _ = await loop.create_datagram_endpoint(
+        create_empty_protocol,
+        local_addr=('0.0.0.0', UDP_LOCAL_PORT)
+    )
+
+    # 建立 Serial 傳輸
+    serial_proto = SerialToUDP(udp_transport)
+    def get_serial_protocol():
+        return serial_proto
+
+    _, serial_transport = await serial_asyncio.create_serial_connection(
+        loop, get_serial_protocol, SERIAL_PORT, baudrate=SERIAL_BAUDRATE
+    )
+
+    # 建立 UDP2 監聽
+    udp_proto = UDPToSerial(serial_proto)
+    def get_udp_protocol():
+        return udp_proto
+
+    udp_listen_transport, _ = await loop.create_datagram_endpoint(
+        get_udp_protocol,
+        local_addr=('0.0.0.0', UDP_LOCAL_PORT)
+    )
+
+    # 保持運行
+    try:
+        await asyncio.Future()
+    except asyncio.CancelledError:
+        pass
+
+if __name__ == '__main__':
+    asyncio.run(main())

src/unitdev02/unitdev02/sslChech.sh

@@ -1,27 +0,0 @@
-#!/bin/bash
-
-# 網站清單
-DOMAINS=("google.com" "smarter.nchu.edu.tw")
-
-echo "網站 SSL 憑證剩餘天數："
-echo "---------------------------"
-
-for domain in "${DOMAINS[@]}"; do
-    end_date=$(echo | openssl s_client -servername "$domain" -connect "$domain:443" 2>/dev/null |
-        openssl x509 -noout -enddate | cut -d= -f2)
-
-    end_timestamp=$(date -d "$end_date" +%s)
-    now_timestamp=$(date +%s)
-
-    remaining_days=$(( (end_timestamp - now_timestamp) / 86400 ))
-
-    if [ $remaining_days -lt 0 ]; then
-        status="已過期 ❌"
-    elif [ $remaining_days -lt 15 ]; then
-        status="即將到期 ⚠️"
-    else
-        status="正常 ✅"
-    fi
-
-    printf "%-20s 到期日：%-25s 剩餘天數：%3d 天 %s\n" "$domain" "$end_date" "$remaining_days" "$status"
-done

src/unitdev02/unitdev02/udp_unix_ad.py

@@ -0,0 +1,40 @@
+import socket
+import sys
+import select
+
+# 設定來源 IP 與 Port
+SRC_IP = '127.0.0.1'    # 監聽所有介面
+SRC_PORT = 16661      # 請自行修改
+
+# 建立 UDP 監聽 socket
+src_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+src_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+src_sock.bind((SRC_IP, SRC_PORT))
+print(f"Listening for UDP on {SRC_IP}:{SRC_PORT}")
+
+# 設定目標 Unix socket 路徑
+UNIX_SOCKET_PATH = '/tmp/unix_socket_mavlink.sock'  # 請自行修改
+
+# 建立 Unix socket 連線
+unix_sock = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
+try:
+    unix_sock.connect(UNIX_SOCKET_PATH)
+except Exception as e:
+    print(f"Failed to connect to unix socket: {e}")
+    sys.exit(1)
+
+while True:
+    # 使用 select 監聽兩個 socket
+    readable, _, _ = select.select([src_sock, unix_sock], [], [])
+    for sock in readable:
+        if sock is src_sock:
+            data, addr = src_sock.recvfrom(4096)
+            if data:
+                unix_sock.sendall(data)
+                # print(f"Received UDP data from {addr}: {data}") # debug
+                # break # debug
+        elif sock is unix_sock:
+            data = unix_sock.recv(4096)
+            if data:
+                # 回送到最近收到資料的 UDP client
+                src_sock.sendto(data, addr)