2.9.1: new mission_orch

- gui.py: _handle_group_mode_change 與 _on_engagement_apply_speed 由
  單一 async 函式內 for 迴圈逐台 await（序列化，最壞 N×timeout）改為
  asyncio.gather 並發，各自等各自 ACK；行為語意不變。
- communication.py: set_mode/arm_drone/set_speed 前 3 處過期註解
  「避免多機並行時的競態條件」更正為共用 client + target_sysid 路由
  （OOM 修復後已非 per-drone、無 per-drone 狀態）。

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

.gitignore

@@ -32,3 +32,8 @@ CLAUDE.md
 my_env_notes.md
 notes/
 **/eeprom.bin
+
+# MAVProxy / SITL 執行產生的 log 與參數 dump
+*.tlog
+*.tlog.raw
+mav.parm

README.md

@@ -67,6 +67,8 @@ cd ~/AirTrapMine/src/ # 這是範例!!!
 python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
 python -m fc_network_adapter.tests.demo_integration
 python -m someotherpkg.src.example_wholeMoving
+
+python fc_network_apps/vehicleDiagShow.py
 ```
 
 2. 
@@ -83,17 +85,16 @@ python gui.py
     建立、維護與飛控韌體的連接 
     構築 mavlink 封包
     處理無線模組的通訊格式 (XBee)
-    --同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)--
-2. fc_interfaces (重要)
-    自定義的 ROS2 介面檔 沒啥好說的 沒有這個核心會運作不了
+2. fc_interfaces (必要)
+    自定義的 ROS2 介面檔 沒有這個核心會運作不了
 3. fc_network_module (重要)
     非核心 但是支援載具的重要附屬功能 
     需要該功能時 作為一個 ros2 節點打開
     例如 : ntrip rtk 訊號轉接
 4. fc_network_apps
-    與 fc_network_adapter 銜接做高階功能包裝的應用小程式 
+    是 fc_network_adapter 高階包裝API
     利於開發GUI或其他應用 使用者的外層包裝
-    這裡的定位是 "核心功能的高階包裝" 可以完全不去用
+    可以不使用 或者當作一個範例程式來看
 5. someotherpkg
     如何使用 fc_network_apps 的範例檔案
 6. GUI 

serial_udp_update.py

@@ -1,193 +0,0 @@
-import asyncio
-import serial_asyncio
-import struct
-import os
-import sys
-import serial
-import signal
-import traceback
-from pymavlink import mavutil
-
-# === 設定區 ===
-SERIAL_PORT = 'COM15'  # 手動指定
-SERIAL_BAUDRATE = 57600
-UDP_REMOTE_IP = '127.0.0.1'
-UDP_REMOTE_PORT = 14550
-DEBUG_MODE = False
-TARGET_ADDR64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'  # 廣播
-
-# === 工具函數 ===
-def check_serial_port():
-    try:
-        ser = serial.Serial(SERIAL_PORT, SERIAL_BAUDRATE)
-        ser.close()
-        return True
-    except serial.SerialException as e:
-        print(f"錯誤：串口設備 {SERIAL_PORT} 被占用或無法訪問：{str(e)}")
-        return False
-    except Exception as e:
-        print(f"錯誤：檢查串口時發生未知錯誤：{str(e)}")
-        return False
-
-def build_api_tx_frame(data: bytes, dest_addr64: bytes, frame_id=0x01) -> bytes:
-    frame_type = 0x10
-    dest_addr16 = b'\xFF\xFE'
-    broadcast_radius = 0x00
-    options = 0x00
-
-    frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id)
-    frame += dest_addr64 + dest_addr16
-    frame += struct.pack(">BB", broadcast_radius, options) + data
-    checksum = 0xFF - (sum(frame) & 0xFF)
-    return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
-
-# === Serial Protocol 實作 ===
-class SerialToUDP(asyncio.Protocol):
-    def __init__(self, udp_protocol):
-        self.udp_protocol = udp_protocol
-        self.buffer = bytearray()
-
-    def connection_made(self, transport):
-        self.transport = transport
-        if hasattr(self.udp_protocol, 'set_serial_transport'):
-            self.udp_protocol.set_serial_transport(self)
-        print(f"Serial connection established on {SERIAL_PORT}")
-
-    def data_received(self, data):
-        self.buffer.extend(data)
-        while True:
-            if len(self.buffer) < 3:
-                return
-            if self.buffer[0] != 0x7E:
-                self.buffer.pop(0)
-                continue
-            length = (self.buffer[1] << 8) | self.buffer[2]
-            full_length = 3 + length + 1
-            if len(self.buffer) < full_length:
-                return
-            frame = self.buffer[:full_length]
-            del self.buffer[:full_length]
-            if hasattr(self.udp_protocol, 'send_udp'):
-                self.udp_protocol.send_udp(bytes(frame))
-
-    def write_to_serial(self, data):
-        try:
-            api_frame = build_api_tx_frame(data, TARGET_ADDR64)
-            pass
-            self.transport.write(api_frame)
-        except Exception as e:
-            print(f"[TX Error] 無法封裝或傳送資料: {e}")
-
-# === UDP Protocol 實作 ===
-class UDPHandler(asyncio.DatagramProtocol):
-    def __init__(self):
-        self.serial_transport = None
-        self.transport = None
-        self.mav_decoder = mavutil.mavlink.MAVLink(None)
-
-    def connection_made(self, transport):
-        self.transport = transport
-        print("UDP transport ready.")
-
-    def set_serial_transport(self, serial_transport):
-        self.serial_transport = serial_transport
-
-    def datagram_received(self, data, addr):
-        if self.serial_transport:
-            self.serial_transport.write_to_serial(data)
-
-    def send_udp(self, data):
-        decoded_data = self.decapsulate_data(data)
-        if decoded_data is None:
-            pass
-            return
-        self.decode_mavlink_data(decoded_data)
-        if self.transport:
-            self.transport.sendto(decoded_data, (UDP_REMOTE_IP, UDP_REMOTE_PORT))
-
-    def decapsulate_data(self, data):
-        try:
-            if not data or data[0] != 0x7E:
-                return None
-            length = (data[1] << 8) | data[2]
-            if len(data) < length + 4:
-                return None
-            frame_type = data[3]
-            if frame_type == 0x90:
-                rf_data_start = 3 + 12
-                return data[rf_data_start:3 + length]
-            else:
-                return None
-        except Exception as e:
-            print(f"[XBee 解封錯誤] {e}")
-            return None
-
-    def decode_mavlink_data(self, data):
-        try:
-            msg = self.mav_decoder.parse_char(data)
-            if msg:
-                if msg.get_type() == "HEARTBEAT":
-                    pass  # 不輸出任何訊息
-                else:
-                    pass
-        except Exception as e:
-            print(f"[MAVLink Decode Error] {e}")
-
-# === 主流程 ===
-async def main():
-    if not check_serial_port():
-        print("程式終止：串口檢查失敗")
-        return
-
-    loop = asyncio.get_running_loop()
-    if os.name != 'nt':  # Windows 不支援 add_signal_handler
-        for sig in (signal.SIGINT, signal.SIGTERM):
-            loop.add_signal_handler(sig, lambda: asyncio.create_task(shutdown(loop)))
-
-    udp_handler = UDPHandler()
-
-    try:
-        udp_transport, _ = await loop.create_datagram_endpoint(
-            lambda: udp_handler,
-            local_addr=('0.0.0.0', 0)
-        )
-    except Exception as e:
-        print(f"無法創建 UDP 端點：{str(e)}")
-        return
-
-    sock = udp_transport.get_extra_info('socket')
-    print(f"UDP listening on {sock.getsockname()}")
-
-    try:
-        serial_proto = SerialToUDP(udp_handler)
-        await serial_asyncio.create_serial_connection(
-            loop, lambda: serial_proto, SERIAL_PORT, baudrate=SERIAL_BAUDRATE
-        )
-    except Exception as e:
-        print(f"無法建立串口連接：{str(e)}")
-        traceback.print_exc()
-        udp_transport.close()
-        return
-
-    print("等待串口資料...")
-    try:
-        await asyncio.Future()
-    except asyncio.CancelledError:
-        pass
-
-async def shutdown(loop):
-    print("Shutting down...")
-    tasks = [t for t in asyncio.all_tasks() if t is not asyncio.current_task()]
-    for task in tasks:
-        task.cancel()
-    await asyncio.gather(*tasks, return_exceptions=True)
-    loop.stop()
-
-if __name__ == '__main__':
-    try:
-        asyncio.run(main())
-    except KeyboardInterrupt:
-        print("程式被使用者中斷")
-    except Exception as e:
-        print("程式執行錯誤：")
-        traceback.print_exc()

src/GUI/command_sender.py

@@ -106,7 +106,11 @@ class Ros2CommandSender(QObject):
     # (drone_id, sysid, success, message)
     send_result = pyqtSignal(str, int, bool, str)
 
-    DEFAULT_TIMEOUT_SEC = 2.0
+    # 閉環設計：goto setpoint 由 mission_executor 週期性重送、換點靠 GPS 到達判定，
+    # 所以這裡的 timeout 只影響「echo 驗證要等多久」，不影響指令是否送達飛控
+    # （adapter 是先送 setpoint 再等 echo）。故意設短，讓 adapter 序列化的 service
+    # 在 echo miss 時快速放棄、換下一台，避免多機時排隊塞爆 client poll。
+    DEFAULT_TIMEOUT_SEC = 0.5
 
     def __init__(self, monitor, timeout_sec: float = DEFAULT_TIMEOUT_SEC):
         super().__init__()

src/GUI/communication.py

@@ -11,6 +11,7 @@ import json
 import socket
 import sys
 import os
+import time
 import traceback
 try:
     import serial
@@ -29,6 +30,28 @@ def _log(level, message):
     print(f"[{level}] {message}")
 
 
+def build_xbee_json_command(system_id, command, position=None):
+    """Build the shared newline-free XBee command used by Serial and WebSocket."""
+    if isinstance(system_id, bool) or not isinstance(system_id, int):
+        raise ValueError("XBee system_id 必須為整數")
+    if not isinstance(command, str):
+        raise ValueError("XBee 指令必須為大寫字串")
+    if command not in {'START', 'STOP', 'GOTO'}:
+        raise ValueError(f"XBee 指令必須為大寫 START、STOP 或 GOTO: {command}")
+
+    payload_data = {
+        's': system_id,
+        'c': command,
+    }
+    if command == 'GOTO':
+        if not isinstance(position, (list, tuple)) or len(position) < 2:
+            raise ValueError("GOTO 缺少有效的經緯度")
+        payload_data['p'] = [float(position[0]), float(position[1])]
+
+    return json.dumps(
+        payload_data, ensure_ascii=False, separators=(',', ':'))
+
+
 # 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入）
 _src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
 if _src_path not in sys.path:
@@ -69,6 +92,18 @@ except ImportError as e:
     _log("ERROR", f"錯誤: {e}")
     GnssRaw = None
 
+try:
+    from fc_interfaces.msg import SystemDiagnosticsRaw
+except ImportError as e:
+    _log("WARN", f"SystemDiagnosticsRaw 尚不可用，略過 sys_diags 訂閱: {e}")
+    SystemDiagnosticsRaw = None
+
+try:
+    from fc_interfaces.msg import FcNetworkLog
+except ImportError as e:
+    _log("WARN", f"FcNetworkLog 尚不可用，略過 FC Network 紀錄訂閱: {e}")
+    FcNetworkLog = None
+
 class DroneSignals(QObject):
     update_signal = pyqtSignal(str, str, object)  # (msg_type, drone_id, data)
 
@@ -83,7 +118,9 @@ class JsonTelemetryProcessor:
         "position": {"lat": 24.0, "lon": 120.0},
         "heading": 90
     }
-    Serial JSON also accepts the compact UAV.py shape:
+    Serial and WebSocket JSON also accept compact UAV status shapes:
+    {"s": 1, "m": "GUIDED", "b": 85, "p": [24.0, 120.0], "y": 90.0}
+    or the full shape:
     {"s": 1, "m": "GUIDED", "a": 1, "b": 85, "h": 10.0,
      "v": 4.2, "p": [24.0, 120.0], "ypr": [90.0, 0.0, 0.0],
      "g": 3, "d": [0.8, 1.2]}
@@ -106,6 +143,20 @@ class JsonTelemetryProcessor:
             if not isinstance(data, dict):
                 return
 
+            # XBee command acknowledgement does not carry a UAV system id.
+            # Route it to the GUI log/status area instead of silently dropping it.
+            if all(key in data for key in ('result', 'command', 'message')):
+                self.signals.update_signal.emit(
+                    'xbee_ack',
+                    self.connection_name,
+                    {
+                        'result': data.get('result'),
+                        'command': data.get('command'),
+                        'message': data.get('message'),
+                    }
+                )
+                return
+
             system_id = data.get('system_id', data.get('sysid', data.get('s')))
             if system_id is None:
                 return
@@ -113,6 +164,13 @@ class JsonTelemetryProcessor:
             drone_id = f"s{self.socket_id}_{system_id}"
             self._emit_json_connection_type(drone_id)
 
+            # Compact XBee/UAV packets are complete snapshots. Emit every field
+            # group, using None for omitted values, so the GUI can show '-'
+            # instead of inventing zero-valued telemetry or retaining stale data.
+            if 's' in data and 'c' not in data:
+                self._process_compact_uav_telemetry(data, drone_id)
+                return
+
             mode = data.get('mode', data.get('mode_name', data.get('m')))
             state = {}
             if mode is not None:
@@ -157,23 +215,23 @@ class JsonTelemetryProcessor:
             pos = data.get('position')
             if isinstance(pos, dict):
                 gps_data = {
-                    'lat': pos.get('lat', pos.get('latitude', 0)),
-                    'lon': pos.get('lon', pos.get('longitude', 0)),
-                    'alt': pos.get('alt', pos.get('altitude', 0))
+                    'lat': pos.get('lat', pos.get('latitude')),
+                    'lon': pos.get('lon', pos.get('longitude')),
+                    'alt': pos.get('alt', pos.get('altitude'))
                 }
                 self.signals.update_signal.emit('gps', drone_id, gps_data)
             elif 'lat' in data or 'latitude' in data:
                 self.signals.update_signal.emit('gps', drone_id, {
-                    'lat': data.get('lat', data.get('latitude', 0)),
-                    'lon': data.get('lon', data.get('longitude', 0)),
-                    'alt': data.get('alt', data.get('altitude', 0))
+                    'lat': data.get('lat', data.get('latitude')),
+                    'lon': data.get('lon', data.get('longitude')),
+                    'alt': data.get('alt', data.get('altitude'))
                 })
             elif isinstance(data.get('p'), (list, tuple)) and len(data.get('p')) >= 2:
                 dop = data.get('d') if isinstance(data.get('d'), (list, tuple)) else []
                 gps_data = {
                     'lat': data['p'][0],
                     'lon': data['p'][1],
-                    'alt': data.get('h', 0)
+                    'alt': data.get('h')
                 }
                 if 'g' in data:
                     gps_data['fix_type'] = data.get('g')
@@ -289,6 +347,82 @@ class JsonTelemetryProcessor:
         except Exception as e:
             print(f"{self.source_type} JSON telemetry processing error: {e}")
 
+    def _process_compact_uav_telemetry(self, data, drone_id):
+        """Convert one compact XBee UAV snapshot into existing GUI events."""
+        snapshot = {'_snapshot': True}
+
+        self.signals.update_signal.emit('state', drone_id, {
+            **snapshot,
+            'mode': data.get('m'),
+            'armed': bool(data.get('a')) if data.get('a') is not None else None,
+        })
+        self.signals.update_signal.emit('battery', drone_id, {
+            **snapshot,
+            'percentage': data.get('b'),
+            'voltage': None,
+        })
+
+        position = data.get('p')
+        has_position = isinstance(position, (list, tuple)) and len(position) >= 2
+        dop = data.get('d')
+        has_dop = isinstance(dop, (list, tuple))
+        ypr = data.get('ypr')
+        has_ypr = isinstance(ypr, (list, tuple)) and len(ypr) >= 3
+        has_yaw = data.get('y') is not None
+        # In the legacy compact shape, a lone 'h' is heading. In the newer
+        # y/ypr shapes it is height, matching the original ground-station parser.
+        height = data.get('h') if has_ypr or has_yaw else None
+        yaw = ypr[0] if has_ypr else data.get('y', data.get('h'))
+        pitch = ypr[1] if has_ypr else None
+        roll = ypr[2] if has_ypr else None
+
+        self.signals.update_signal.emit('gps', drone_id, {
+            **snapshot,
+            'lat': position[0] if has_position else None,
+            'lon': position[1] if has_position else None,
+            'alt': height,
+            'fix_type': data.get('g'),
+            'eph': dop[0] if has_dop and len(dop) >= 1 else None,
+            'epv': dop[1] if has_dop and len(dop) >= 2 else None,
+            'satellites_visible': None,
+        })
+
+        self.signals.update_signal.emit('altitude', drone_id, {
+            **snapshot,
+            'altitude': height,
+        })
+        self.signals.update_signal.emit('local_pose', drone_id, {
+            **snapshot,
+            'x': None,
+            'y': None,
+            'z': height,
+        })
+
+        # Compact 'v' is scalar ground speed, not an XY velocity vector.
+        self.signals.update_signal.emit('velocity', drone_id, {
+            **snapshot,
+            'vx': None,
+            'vy': None,
+            'vz': None,
+        })
+
+        self.signals.update_signal.emit('attitude', drone_id, {
+            **snapshot,
+            'roll': roll,
+            'pitch': pitch,
+            'yaw': yaw,
+            'rates': None,
+        })
+        self.signals.update_signal.emit('hud', drone_id, {
+            **snapshot,
+            'heading': yaw,
+            'groundspeed': data.get('v'),
+            'airspeed': None,
+            'throttle': None,
+            'alt': height,
+            'climb': None,
+        })
+
 class UDPMavlinkReceiver(threading.Thread):
     """UDP MAVLink 接收器"""
     def __init__(self, ip, port, signals, connection_name, monitor=None):
@@ -436,6 +570,7 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
         self.source_type = 'Serial'
         self.running = False
         self.serial_conn = None
+        self.serial_write_lock = Lock()
         self.mav_parser = mavutil.mavlink.MAVLink(None)
         self.mav_parser.robust_parsing = True
         self.json_buffer = []
@@ -478,11 +613,24 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
         finally:
             if self.serial_conn:
                 try:
-                    self.serial_conn.close()
+                    with self.serial_write_lock:
+                        self.serial_conn.close()
                 except Exception:
                     pass
             self._release_socket_id()
 
+    def send_json_command(self, system_id, command, position=None):
+        """Send one newline-delimited XBee JSON command on this serial link."""
+        payload = build_xbee_json_command(system_id, command, position) + '\n'
+        with self.serial_write_lock:
+            if not self.running or self.serial_conn is None:
+                raise RuntimeError("Serial 連線尚未啟動")
+            if hasattr(self.serial_conn, 'is_open') and not self.serial_conn.is_open:
+                raise RuntimeError("Serial 連線已關閉")
+            self.serial_conn.write(payload.encode('utf-8'))
+            self.serial_conn.flush()
+        return payload.strip()
+
     def _process_mavlink_byte(self, byte):
         try:
             msg = self.mav_parser.parse_char(byte)
@@ -639,7 +787,8 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
         self.running = False
         if self.serial_conn:
             try:
-                self.serial_conn.close()
+                with self.serial_write_lock:
+                    self.serial_conn.close()
             except Exception:
                 pass
         self._release_socket_id()
@@ -663,12 +812,19 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
         self.running = False
         self.max_retries = 5
         self.base_delay = 1.0
+        self.loop = None
+        self.websocket = None
+        self.websocket_lock = Lock()
         
     def run(self):
         """執行 WebSocket 接收循環"""
         self.running = True
-        asyncio.set_event_loop(asyncio.new_event_loop())
-        asyncio.get_event_loop().run_until_complete(self.ws_client_loop())
+        self.loop = asyncio.new_event_loop()
+        asyncio.set_event_loop(self.loop)
+        try:
+            self.loop.run_until_complete(self.ws_client_loop())
+        finally:
+            self.loop.close()
     
     async def ws_client_loop(self):
         """WebSocket 連接的主循環"""
@@ -679,22 +835,28 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
         while self.running and retry_count < self.max_retries:
             try:
                 async with websockets.connect(self.url) as websocket:
+                    with self.websocket_lock:
+                        self.websocket = websocket
                     print(f"WebSocket {self.connection_name} connected to {self.url}")
                     retry_count = 0  # 重置重試計數
-                    
-                    async for message in websocket:
-                        if not self.running:
-                            break
-                            
-                        try:
-                            data = json.loads(message)
-                            if isinstance(data, dict):
-                                data['_connection_source'] = self.connection_name
-                            self.process_websocket_message(data)
-                        except json.JSONDecodeError as e:
-                            print(f"WebSocket {self.connection_name} JSON decode error: {e}")
-                        except Exception as e:
-                            print(f"WebSocket {self.connection_name} message processing error: {e}")
+                    try:
+                        async for message in websocket:
+                            if not self.running:
+                                break
+
+                            try:
+                                data = json.loads(message)
+                                if isinstance(data, dict):
+                                    data['_connection_source'] = self.connection_name
+                                self.process_websocket_message(data)
+                            except json.JSONDecodeError as e:
+                                print(f"WebSocket {self.connection_name} JSON decode error: {e}")
+                            except Exception as e:
+                                print(f"WebSocket {self.connection_name} message processing error: {e}")
+                    finally:
+                        with self.websocket_lock:
+                            if self.websocket is websocket:
+                                self.websocket = None
                             
             except websockets.exceptions.ConnectionClosedError:
                 print(f"WebSocket {self.connection_name} connection closed")
@@ -717,6 +879,37 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
         
         print(f"WebSocket client {self.connection_name} stopped")
         self._release_socket_id()
+
+    def send_json_command(self, system_id, command, position=None):
+        """Schedule an XBee JSON command on this receiver's WebSocket loop."""
+        payload = build_xbee_json_command(system_id, command, position)
+        with self.websocket_lock:
+            loop = self.loop
+            websocket = self.websocket
+
+        if not self.running or loop is None or not loop.is_running():
+            raise RuntimeError("WebSocket 連線尚未啟動")
+        if websocket is None:
+            raise RuntimeError("WebSocket 尚未連線")
+
+        async def _send_on_current_connection():
+            with self.websocket_lock:
+                if self.websocket is not websocket:
+                    raise RuntimeError("WebSocket 連線已變更")
+            await websocket.send(payload)
+
+        future = asyncio.run_coroutine_threadsafe(
+            _send_on_current_connection(), loop)
+
+        def _report_send_error(done_future):
+            try:
+                done_future.result()
+            except Exception as e:
+                print(
+                    f"WebSocket {self.connection_name} command send error: {e}")
+
+        future.add_done_callback(_report_send_error)
+        return payload
     
     def process_websocket_message(self, data):
         """處理 WebSocket 訊息"""
@@ -725,6 +918,11 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
     def stop(self):
         """停止接收器"""
         self.running = False
+        with self.websocket_lock:
+            loop = self.loop
+            websocket = self.websocket
+        if loop is not None and loop.is_running() and websocket is not None:
+            asyncio.run_coroutine_threadsafe(websocket.close(), loop)
         self._release_socket_id()
 
     def _release_socket_id(self):
@@ -738,7 +936,6 @@ class DroneMonitor(Node):
     
     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)
         self.signals = DroneSignals()
@@ -763,6 +960,9 @@ class DroneMonitor(Node):
         # 【新增】儲存 GPS 資料的字典
         # ================================================================================
         self.drone_gps = {}  # {drone_id: {'lat': ..., 'lon': ..., 'alt': ...}}
+        # GnssRaw.altitude 是 AMSL；交戰/航點指令的 alt 則是相對 Home 高度。
+        self.drone_relative_alt = {}
+        self.drone_relative_alt_updated_at = {}
         # ================================================================================
         
         # ================================================================================
@@ -782,24 +982,62 @@ class DroneMonitor(Node):
         self.serial_receivers = []
         
         # ================================================================================
-        # 【新增】初始化 CommandLongClient 字典（為每個 drone 維護獨立的 client）
+        # 共用 command / position client（各一個節點，靠 request 的 target_sysid 路由）
         # ================================================================================
-        # 改為為每個 drone 創建獨立的 client，避免多機並行時的競態條件
-        self.command_long_clients = {}  # {drone_id: CommandLongClient}
-        self.client_lock = Lock()  # 保護 clients 字典的訪問
-        self.client_counter = 0  # 用於生成唯一的 client 節點名稱
-        self.executor = None  # 將在 gui.py 中設置，用於添加新的 clients
-        # ================================================================================
-
-        # ================================================================================
-        # PositionTargetGlobalIntClient 字典（per-drone，用於 Offboard goto）
-        # ================================================================================
-        self.position_target_clients = {}  # {drone_id: PositionTargetGlobalIntClient}
-        self.pos_client_counter = 0
+        # 兩個 client 都只打到單一 service（send_command_long / pos_global_int），
+        # 沒有任何 per-drone 狀態，故不需要 per-drone 節點。改成「啟動時各預建一個、
+        # 執行期只重用」，避免：
+        #   (1) 切 mode / 首次 goto 時於執行期新建 ROS2 node = 新 DDS participant，
+        #       其 discovery handshake 撞到壞掉的外來 participant → FastRTPS bad_alloc → OOM。
+        #   (2) 從 asyncio/Qt 執行緒對「正在被 _ros_spin_thread spin 的 executor」
+        #       跨執行緒 add_node → wait set 損毀 → CPU 空轉 100%。
+        self.command_long_client = None      # CommandLongClient（共用）
+        self.position_target_client = None   # PositionTargetGlobalIntClient（共用）
+        self.client_lock = Lock()            # 保護共用 client 的建立
+        self.executor = None                 # 將在 gui.py 中設置，用於 add_node
         # ================================================================================
         
         # 主题检测定时器
         self.create_timer(1.0, self.scan_topics)
+
+        # FC Network 使用專用結構化紀錄 topic，不再解析 /rosout。
+        self.fc_network_log_sub = None
+        if FcNetworkLog is not None:
+            self.fc_network_log_sub = self.create_subscription(
+                FcNetworkLog,
+                '/fc_network/logs',
+                self.fc_network_log_callback,
+                200
+            )
+
+    def get_drone_gps_snapshot(self):
+        """回傳執行緒安全的定位快照。"""
+        with self.lock:
+            return {
+                drone_id: dict(position)
+                for drone_id, position in self.drone_gps.items()
+                if isinstance(position, dict)
+            }
+
+    def record_relative_gps(self, drone_id, gps_data, updated_at=None):
+        """記錄已知以 Home 為基準的 GPS/高度（UDP、Serial、WebSocket）。"""
+        now = time.monotonic() if updated_at is None else float(updated_at)
+        stored = dict(gps_data)
+        stored['_gps_updated_at'] = now
+        alt = stored.get('alt')
+        if isinstance(alt, (int, float)) and math.isfinite(float(alt)):
+            stored['alt'] = float(alt)
+            stored['_relative_alt_updated_at'] = now
+            stored['_altitude_reference'] = 'relative_home'
+        else:
+            stored['alt'] = None
+
+        with self.lock:
+            if stored.get('_altitude_reference') == 'relative_home':
+                self.drone_relative_alt[drone_id] = stored['alt']
+                self.drone_relative_alt_updated_at[drone_id] = now
+            self.drone_gps[drone_id] = stored
+        return stored
     
     def get_next_socket_id(self):
         """取得目前最小的未使用 socket_id（從 0 開始）。"""
@@ -854,57 +1092,69 @@ class DroneMonitor(Node):
             
             return self.socket_id_mapping[original_socket_id]
        
-    def get_or_create_client(self, drone_id):
-        """為每個 drone 獲取或創建獨立的 CommandLongClient，避免競態條件"""
-        with self.client_lock:
-            if drone_id not in self.command_long_clients:
-                try:
-                    # 生成唯一的 client 節點名稱
-                    self.client_counter += 1
-                    unique_name = f"cmd_long_client_{drone_id}_{self.client_counter}"
-                    client = CommandLongClient(node_name=unique_name)
-                    self.command_long_clients[drone_id] = client
-                    _log("INFO", f"已為 {drone_id} 建立 CommandLongClient (node={unique_name})")
-                    
-                    # 將新 client 添加到主執行器（這樣它的回調才能被處理）
-                    if self.executor:
-                        self.executor.add_node(client)
-                        _log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器")
-                    
-                except TypeError:
-                    # 舊版 CommandLongClient 不支持 node_name 參數，使用預設
-                    client = CommandLongClient()
-                    self.command_long_clients[drone_id] = client
-                    _log("INFO", f"已為 {drone_id} 建立 CommandLongClient (使用預設名稱)")
-                    
-                    if self.executor:
-                        self.executor.add_node(client)
-                        _log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器")
-                        
-                except Exception as e:
-                    _log("WARN", f"無法為 {drone_id} 建立 CommandLongClient: {e}")
-                    return None
-            return self.command_long_clients[drone_id]
+    def init_shared_command_clients(self):
+        """啟動時預建共用的 command / position client（各一個節點）。
 
-    def get_or_create_position_client(self, drone_id):
-        """為每個 drone 獲取或創建獨立的 PositionTargetGlobalIntClient。"""
+        必須在 _ros_spin_thread 啟動「之前」於主執行緒呼叫：此時 executor 已建立
+        但尚未被 spin，add_node 不會與 spin_once 跨執行緒競爭。之後執行期送指令
+        一律重用這兩個節點，不再於飛行中新建 DDS participant。"""
+        with self.client_lock:
+            if self.command_long_client is None and CommandLongClient is not None:
+                try:
+                    self.command_long_client = CommandLongClient(node_name="cmd_long_client_shared")
+                    if self.executor:
+                        self.executor.add_node(self.command_long_client)
+                    _log("INFO", "已預建共用 CommandLongClient (node=cmd_long_client_shared)")
+                except Exception as e:
+                    _log("WARN", f"預建 CommandLongClient 失敗: {e}")
+            if self.position_target_client is None and PositionTargetGlobalIntClient is not None:
+                try:
+                    self.position_target_client = PositionTargetGlobalIntClient(node_name="pos_target_client_shared")
+                    if self.executor:
+                        self.executor.add_node(self.position_target_client)
+                    _log("INFO", "已預建共用 PositionTargetGlobalIntClient (node=pos_target_client_shared)")
+                except Exception as e:
+                    _log("WARN", f"預建 PositionTargetGlobalIntClient 失敗: {e}")
+
+    def get_or_create_client(self, drone_id=None):
+        """回傳共用的 CommandLongClient（單一節點，靠 request 的 target_sysid 路由）。
+
+        正常情況已於啟動時由 init_shared_command_clients() 預建；此處僅保留一次性
+        lazy fallback（例如 init 尚未被呼叫），不會在執行期重複新建 participant。
+        drone_id 參數保留以相容既有呼叫端，實際不影響路由（路由在 request 內）。"""
+        if self.command_long_client is not None:
+            return self.command_long_client
+        with self.client_lock:
+            if self.command_long_client is None and CommandLongClient is not None:
+                try:
+                    self.command_long_client = CommandLongClient(node_name="cmd_long_client_shared")
+                    if self.executor:
+                        self.executor.add_node(self.command_long_client)
+                    _log("INFO", "已建立共用 CommandLongClient (lazy fallback)")
+                except Exception as e:
+                    _log("WARN", f"無法建立共用 CommandLongClient: {e}")
+                    return None
+            return self.command_long_client
+
+    def get_or_create_position_client(self, drone_id=None):
+        """回傳共用的 PositionTargetGlobalIntClient（單一節點，靠 target_sysid 路由）。
+
+        同 get_or_create_client：正常已於啟動預建，此處僅一次性 lazy fallback。"""
+        if self.position_target_client is not None:
+            return self.position_target_client
         if PositionTargetGlobalIntClient is None:
             return None
         with self.client_lock:
-            if drone_id not in self.position_target_clients:
+            if self.position_target_client is None:
                 try:
-                    self.pos_client_counter += 1
-                    unique_name = f"pos_target_client_{drone_id}_{self.pos_client_counter}"
-                    client = PositionTargetGlobalIntClient(node_name=unique_name)
-                    self.position_target_clients[drone_id] = client
-                    _log("INFO", f"已為 {drone_id} 建立 PositionTargetGlobalIntClient (node={unique_name})")
+                    self.position_target_client = PositionTargetGlobalIntClient(node_name="pos_target_client_shared")
                     if self.executor:
-                        self.executor.add_node(client)
-                        _log("INFO", f"已將 {drone_id} 的 PositionTargetGlobalIntClient 加入主執行器")
+                        self.executor.add_node(self.position_target_client)
+                    _log("INFO", "已建立共用 PositionTargetGlobalIntClient (lazy fallback)")
                 except Exception as e:
-                    _log("WARN", f"無法為 {drone_id} 建立 PositionTargetGlobalIntClient: {e}")
+                    _log("WARN", f"無法建立共用 PositionTargetGlobalIntClient: {e}")
                     return None
-            return self.position_target_clients[drone_id]
+            return self.position_target_client
 
     def scan_topics(self):
         topics = self.get_topic_names_and_types()
@@ -969,6 +1219,32 @@ class DroneMonitor(Node):
                         setattr(self, subs_attr, subs)
                     except Exception:
                         pass
+                if isinstance(subs, dict) and 'sys_diags' not in subs and SystemDiagnosticsRaw is not None:
+                    base_topic = f'/fc_network/vehicle/{sys_id}'
+                    try:
+                        sys_diags_sub = self.create_subscription(
+                            SystemDiagnosticsRaw,
+                            f'{base_topic}/sys_diags',
+                            lambda msg, sid=sys_id: self.sys_diags_callback(sid, msg),
+                            10
+                        )
+                        subs['sys_diags'] = sys_diags_sub
+                        setattr(self, subs_attr, subs)
+                    except Exception:
+                        pass
+                if isinstance(subs, dict) and 'status_text' not in subs:
+                    base_topic = f'/fc_network/vehicle/{sys_id}'
+                    try:
+                        status_text_sub = self.create_subscription(
+                            String,
+                            f'{base_topic}/status_text',
+                            lambda msg, sid=sys_id: self.status_text_callback(sid, msg),
+                            10
+                        )
+                        subs['status_text'] = status_text_sub
+                        setattr(self, subs_attr, subs)
+                    except Exception:
+                        pass
     
     def setup_drone(self, sys_id):
         # sys_id 格式： sys11, sys12, ...
@@ -1040,6 +1316,21 @@ class DroneMonitor(Node):
                 lambda msg, sid=sys_id: self.attitude_callback(sid, msg),
                 10
             )
+
+        if SystemDiagnosticsRaw is not None:
+            subs['sys_diags'] = self.create_subscription(
+                SystemDiagnosticsRaw,
+                f'{base_topic}/sys_diags',
+                lambda msg, sid=sys_id: self.sys_diags_callback(sid, msg),
+                10
+            )
+
+        subs['status_text'] = self.create_subscription(
+            String,
+            f'{base_topic}/status_text',
+            lambda msg, sid=sys_id: self.status_text_callback(sid, msg),
+            10
+        )
         
         setattr(self, f'drone_{sys_id}_subs', subs)
 
@@ -1089,7 +1380,8 @@ class DroneMonitor(Node):
             
             _log("INFO", f"[SET_MODE] {drone_id} -> {mode_name} (custom_mode={custom_mode})")
             
-            # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
+            # 取得共用 command_long client（OOM 修復後已非 per-drone，靠 request 的
+            # target_sysid 路由；drone_id 僅用於一次性 lazy fallback，無 per-drone 狀態）
             client = self.get_or_create_client(drone_id)
             if not client:
                 _log("ERROR", "[SET_MODE] CommandLongClient 無法初始化")
@@ -1115,6 +1407,50 @@ class DroneMonitor(Node):
             traceback.print_exc()
             return False
 
+    async def set_speed(self, drone_id, speed_mps, speed_type=1):
+        """對指定無人機下 DO_CHANGE_SPEED(178) 設定/限制最大速度（非阻塞 async）。
+
+        speed_type: 0=Airspeed, 1=Ground Speed（Copter 用 1）, 2=Climb, 3=Descent。
+        speed_mps: 目標速度 (m/s)。DO_CHANGE_SPEED 常數定義在 scope 外的 longCommand.py，
+        故此處用共用 client 的泛用方法 _send_command_long_async 直接發 command=178，
+        不修改 fc_network_apps（維持 GUI-only scope）。"""
+        try:
+            parts = drone_id.split('_')
+            if len(parts) < 2:
+                _log("ERROR", f"[SET_SPEED] 無效的 drone_id 格式: {drone_id}")
+                return False
+            sysid = int(parts[-1])
+
+            client = self.get_or_create_client(drone_id)
+            if not client:
+                _log("ERROR", "[SET_SPEED] CommandLongClient 無法初始化")
+                return False
+
+            _log("INFO", f"[SET_SPEED] {drone_id} -> {speed_mps} m/s (type={speed_type})")
+            result = await client._send_command_long_async(
+                target_sysid=sysid,
+                target_compid=0,
+                command=178,          # MAV_CMD_DO_CHANGE_SPEED
+                confirmation=0,
+                param1=float(speed_type),
+                param2=float(speed_mps),
+                param3=-1.0,          # throttle 不變
+                param4=0.0,           # 0=absolute
+                param5=0.0,
+                param6=0.0,
+                param7=0.0,
+                timeout_sec=5.0,
+            )
+            if result and result.success:
+                _log("INFO", f"[SET_SPEED] {drone_id} 速度設定成功")
+                return True
+            _log("ERROR", f"[SET_SPEED] 速度設定失敗 (message={result.message if result else 'None'})")
+            return False
+        except Exception as e:
+            _log("ERROR", f"[SET_SPEED] 例外錯誤: {e}")
+            traceback.print_exc()
+            return False
+
     async def arm_drone(self, drone_id, arm):
         """使用 CommandLongClient 執行 ARM/DISARM（使用非阻塞的 async 方法）"""
         try:
@@ -1128,7 +1464,8 @@ class DroneMonitor(Node):
             action_name = "解鎖" if arm else "上鎖"
             _log("INFO", f"[ARM] {drone_id} -> {action_name}")
             
-            # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
+            # 取得共用 command_long client（OOM 修復後已非 per-drone，靠 request 的
+            # target_sysid 路由；drone_id 僅用於一次性 lazy fallback，無 per-drone 狀態）
             client = self.get_or_create_client(drone_id)
             if not client:
                 _log("ERROR", "[ARM] CommandLongClient 無法初始化")
@@ -1165,7 +1502,8 @@ class DroneMonitor(Node):
             
             _log("INFO", f"[TAKEOFF] {drone_id} -> 起飛 (高度={altitude}m)")
             
-            # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
+            # 取得共用 command_long client（OOM 修復後已非 per-drone，靠 request 的
+            # target_sysid 路由；drone_id 僅用於一次性 lazy fallback，無 per-drone 狀態）
             client = self.get_or_create_client(drone_id)
             if not client:
                 _log("ERROR", "[TAKEOFF] CommandLongClient 無法初始化")
@@ -1299,6 +1637,67 @@ class DroneMonitor(Node):
             'voltage': msg.voltage
         }
 
+    def sys_diags_callback(self, sys_id, msg):
+        """轉送 /fc_network/vehicle/sysN/sys_diags 到 GUI 紀錄區。"""
+        stamp = getattr(msg, 'stamp', None)
+        data = {
+            'stamp': {
+                'sec': getattr(stamp, 'sec', 0),
+                'nanosec': getattr(stamp, 'nanosec', 0),
+            },
+            'sensors_install_mask': int(msg.sensors_install_mask),
+            'sensors_enabled_mask': int(msg.sensors_enabled_mask),
+            'sensors_health_mask': int(msg.sensors_health_mask),
+            'mcu_load': int(msg.mcu_load),
+            'mcu_load_percent': float(msg.mcu_load) / 10.0,
+            'bus_error_rate': int(msg.bus_error_rate),
+            'bus_error_rate_percent': float(msg.bus_error_rate) / 10.0,
+            'bus_error_count': int(msg.bus_error_count),
+            'errors_count1': int(msg.errors_count1),
+            'errors_count2': int(msg.errors_count2),
+            'errors_count3': int(msg.errors_count3),
+            'errors_count4': int(msg.errors_count4),
+        }
+        self.signals.update_signal.emit('sys_diags', sys_id, data)
+
+    def status_text_callback(self, sys_id, msg):
+        """轉送飛控 STATUSTEXT（飛行檢查、警告與錯誤）。"""
+        raw = msg.data.strip()
+        if not raw:
+            return
+
+        match = re.match(r'^\[([\d.]+)\]\s*\[(-?\d+)\]\s*(.*)$', raw)
+        if match:
+            vehicle_timestamp = float(match.group(1))
+            severity = int(match.group(2))
+            text = match.group(3)
+        else:
+            vehicle_timestamp = None
+            severity = -1
+            text = raw
+
+        self.signals.update_signal.emit('status_text', sys_id, {
+            'severity': severity,
+            'text': text,
+            'vehicle_timestamp': vehicle_timestamp,
+            'raw': raw,
+        })
+
+    def fc_network_log_callback(self, msg):
+        """接收 /fc_network/logs 的結構化紀錄。"""
+        stamp = getattr(msg, 'stamp', None)
+        self.signals.update_signal.emit('fc_network_log', msg.source or 'fc_network', {
+            'stamp': {
+                'sec': getattr(stamp, 'sec', 0),
+                'nanosec': getattr(stamp, 'nanosec', 0),
+            },
+            'level': int(msg.level),
+            'source': msg.source,
+            'event_code': msg.event_code,
+            'sysid': int(msg.sysid),
+            'message': msg.message,
+        })
+
     def state_callback(self, drone_id, msg):
         mode = msg.mode
         if mode in self.filtered_modes:
@@ -1373,10 +1772,15 @@ class DroneMonitor(Node):
         if actual_drone_id is None:
             return
 
+        # GnssRaw.altitude 是 GLOBAL_POSITION_INT.alt（AMSL）。交戰航點使用
+        # GLOBAL_RELATIVE_ALT_INT，因此 alt 僅能使用 position_ned 的相對高度。
+        now = time.monotonic()
         gps_data = {
             'lat': msg.latitude,
             'lon': msg.longitude,
-            'alt': msg.altitude
+            'alt': None,
+            'amsl_alt': msg.altitude,
+            '_gps_updated_at': now,
         }
 
         if hasattr(msg, 'fix_type'):
@@ -1388,12 +1792,20 @@ class DroneMonitor(Node):
         if hasattr(msg, 'epv'):
             gps_data['epv'] = msg.epv
         
-        self.latest_data[(actual_drone_id, 'gps')] = gps_data
-        
         # ================================================================================
         # 【新增】儲存 GPS 資料到 drone_gps 字典
         # ================================================================================
-        self.drone_gps[actual_drone_id] = gps_data.copy()
+        with self.lock:
+            relative_alt = self.drone_relative_alt.get(actual_drone_id)
+            relative_alt_updated_at = self.drone_relative_alt_updated_at.get(
+                actual_drone_id
+            )
+            if relative_alt_updated_at is not None:
+                gps_data['alt'] = relative_alt
+                gps_data['_relative_alt_updated_at'] = relative_alt_updated_at
+                gps_data['_altitude_reference'] = 'relative_home'
+            self.drone_gps[actual_drone_id] = gps_data.copy()
+        self.latest_data[(actual_drone_id, 'gps')] = gps_data
         # ================================================================================
 
     def local_vel_callback(self, drone_id, msg):
@@ -1453,6 +1865,18 @@ class DroneMonitor(Node):
             vx = msg.twist.twist.linear.y
             vy = msg.twist.twist.linear.x
             vz = -msg.twist.twist.linear.z
+
+            now = time.monotonic()
+            with self.lock:
+                self.drone_relative_alt[actual_drone_id] = z
+                self.drone_relative_alt_updated_at[actual_drone_id] = now
+                current_gps = self.drone_gps.get(actual_drone_id)
+                if isinstance(current_gps, dict):
+                    current_gps = dict(current_gps)
+                    current_gps['alt'] = z
+                    current_gps['_relative_alt_updated_at'] = now
+                    current_gps['_altitude_reference'] = 'relative_home'
+                    self.drone_gps[actual_drone_id] = current_gps
             
             # 儲存高度信息
             self.latest_data[(actual_drone_id, 'altitude')] = {

src/GUI/drone_panel.py

@@ -222,11 +222,11 @@ class DronePanel(QWidget):
         status_title = QLabel("狀態：")
         _set_scaled_stylesheet(status_title, "color: #888; min-width: 50px;")
         
-        self.mode_label = QLabel("--")
+        self.mode_label = QLabel("-")
         self.mode_label.setObjectName(f"{self.drone_id}_mode")
         _set_scaled_stylesheet(self.mode_label, "color: #DDD;")
         
-        self.armed_label = QLabel("--")
+        self.armed_label = QLabel("-")
         self.armed_label.setObjectName(f"{self.drone_id}_armed")
         _set_scaled_stylesheet(self.armed_label, "color: #DDD;")
         
@@ -284,7 +284,7 @@ class DronePanel(QWidget):
         battery_title = QLabel("電池：")
         _set_scaled_stylesheet(battery_title, "color: #888; min-width: 50px;")
 
-        self.battery_pct_label = QLabel("--")
+        self.battery_pct_label = QLabel("-")
         self.battery_pct_label.setObjectName(f"{self.drone_id}_battery_pct")
         _set_scaled_stylesheet(self.battery_pct_label, "color: #DDD;")
 
@@ -294,7 +294,7 @@ class DronePanel(QWidget):
         _set_scaled_stylesheet(separator1, "color: #DDD;")
 
         # 顯示電壓
-        self.battery_vol_label = QLabel("--")
+        self.battery_vol_label = QLabel("-")
         self.battery_vol_label.setObjectName(f"{self.drone_id}_battery_vol")
         _set_scaled_stylesheet(self.battery_vol_label, "color: #DDD;")
 
@@ -304,7 +304,7 @@ class DronePanel(QWidget):
         _set_scaled_stylesheet(separator2, "color: #DDD;")
 
         # 顯示電池節數 (S count)
-        self.battery_cells_label = QLabel("--")
+        self.battery_cells_label = QLabel("-")
         self.battery_cells_label.setObjectName(f"{self.drone_id}_battery_cells")
         _set_scaled_stylesheet(self.battery_cells_label, "color: #DDD;")
 
@@ -327,14 +327,14 @@ class DronePanel(QWidget):
         altitude_title = QLabel("高度：")
         _set_scaled_stylesheet(altitude_title, "color: #888; min-width: 50px;")
         
-        self.altitude_label = QLabel("--")
+        self.altitude_label = QLabel("-")
         self.altitude_label.setObjectName(f"{self.drone_id}_altitude")
         _set_scaled_stylesheet(self.altitude_label, "color: #DDD;")
         
         speed_title = QLabel("速度：")
         _set_scaled_stylesheet(speed_title, "color: #888; margin-left: 10px;")
         
-        self.speed_label = QLabel("--")
+        self.speed_label = QLabel("-")
         self.speed_label.setObjectName(f"{self.drone_id}_speed")
         _set_scaled_stylesheet(self.speed_label, "color: #DDD;")
         
@@ -355,14 +355,14 @@ class DronePanel(QWidget):
         fix_title = QLabel("定位：")
         _set_scaled_stylesheet(fix_title, "color: #888; min-width: 50px;")
 
-        self.fix_type_label = QLabel("--")
+        self.fix_type_label = QLabel("-")
         self.fix_type_label.setObjectName(f"{self.drone_id}_fix_type_label")
         _set_scaled_stylesheet(self.fix_type_label, "color: #DDD;")
 
         error_title = QLabel("誤差(H/V)：")
         _set_scaled_stylesheet(error_title, "color: #888; margin-left: 8px;")
 
-        self.gnss_error_label = QLabel("--")
+        self.gnss_error_label = QLabel("-")
         self.gnss_error_label.setObjectName(f"{self.drone_id}_gnss_error")
         _set_scaled_stylesheet(self.gnss_error_label, "color: #DDD;")
 

src/GUI/map_layout.py

@@ -21,6 +21,8 @@ class DroneMap:
         self.font_scale = 1.0
         self.socket_colors = {}
         self.drone_group_colors = {}
+        self.mission_following_enabled = False
+        self.mission_following_drone_id = None
         
         # 創建橋接對象
         self.bridge = MapBridge()
@@ -199,16 +201,18 @@ class DroneMap:
                     bridge = channel.objects.bridge;
                 });
 
-                var map = L.map('map').setView([0, 0], 19); 
+                var map = L.map('map', {maxZoom: 20}).setView([0, 0], 19);
                 
                 // 創建不同的地圖圖層
                 var streetLayer = L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', {
-                    maxZoom: 19,
+                    maxNativeZoom: 19,
+                    maxZoom: 20,
                     attribution: '© OpenStreetMap contributors'
                 });
                 
                 var satelliteLayer = L.tileLayer('https://server.arcgisonline.com/ArcGIS/rest/services/World_Imagery/MapServer/tile/{z}/{y}/{x}', {
-                    maxZoom: 19,
+                    maxNativeZoom: 19,
+                    maxZoom: 20,
                     attribution: 'Tiles © Esri'
                 });
                 
@@ -217,7 +221,6 @@ class DroneMap:
                 currentLayer.addTo(map);
                 var isSatellite = true;
 
-
                 // 地圖點擊事件
                 map.on('click', function(e) {
                     if (selectionMode === 'polygon') {
@@ -349,6 +352,7 @@ class DroneMap:
                 var socketColors = {};
                 var droneGroupColors = {};
                 var focusedId = null;
+                var missionFollowingEnabled = false;
                 var initialized = false;
                 var trajectoryGroup = L.layerGroup().addTo(map);
 
@@ -364,6 +368,8 @@ class DroneMap:
 
                 // 多群組任務規劃 — 每個 group 各自的 layer group
                 var groupMissionLayers = {};  // group_id → L.layerGroup
+                // Grid Sweep 框選範圍也依群組保存，停止時可單獨清除。
+                var gridSweepAreaLayers = {};  // group_id → L.layerGroup
                 
                 // 選擇工具變量
                 var selectionMode = null;  // 'drones', 'rect', 'polygon', 'route', null
@@ -377,6 +383,8 @@ class DroneMap:
                 var clickStartPos = null;
                 var lastManualMapMoveAt = 0;
                 var autoCenteringMap = false;
+                const autoFollowPanThresholdRatio = 0.25;
+                var lastAutoFollowPanAt = 0;
 
                 function markManualMapMove() {
                     if (!autoCenteringMap) {
@@ -385,6 +393,42 @@ class DroneMap:
                 }
 
                 map.on('dragstart drag dragend zoomstart zoomend', markManualMapMove);
+
+                function isOutsideAutoFollowWindow(latlng) {
+                    if (!latlng) {
+                        return false;
+                    }
+
+                    var size = map.getSize();
+                    if (!size || size.x <= 0 || size.y <= 0) {
+                        return false;
+                    }
+
+                    var centerPoint = map.latLngToContainerPoint(map.getCenter());
+                    var targetPoint = map.latLngToContainerPoint(latlng);
+                    var dx = Math.abs(targetPoint.x - centerPoint.x);
+                    var dy = Math.abs(targetPoint.y - centerPoint.y);
+
+                    return dx >= size.x * autoFollowPanThresholdRatio ||
+                           dy >= size.y * autoFollowPanThresholdRatio;
+                }
+
+                function autoFollowPanTo(latlng) {
+                    if (!isOutsideAutoFollowWindow(latlng)) {
+                        return;
+                    }
+                    if (Date.now() - lastManualMapMoveAt < 1000) {
+                        return;
+                    }
+                    if (Date.now() - lastAutoFollowPanAt < 250) {
+                        return;
+                    }
+
+                    lastAutoFollowPanAt = Date.now();
+                    autoCenteringMap = true;
+                    map.panTo(latlng, {animate: true});
+                    setTimeout(() => { autoCenteringMap = false; }, 300);
+                }
                 
                 // 路徑標記變量 (跟隨模式用)
                 var routePoints = [];
@@ -539,23 +583,6 @@ class DroneMap:
                             [bounds.getSouth(), bounds.getEast()],
                             [bounds.getSouth(), bounds.getWest()]
                         ];
-                        
-                        L.rectangle(bounds, {
-                            color: '#FF6B6B',
-                            fillColor: '#FF6B6B',
-                            fillOpacity: 0.2,
-                            weight: 2
-                        }).addTo(selectionLayer);
-                        
-                        rectPoints.forEach(point => {
-                            L.circleMarker(point, {
-                                radius: 5,
-                                color: '#FF6B6B',
-                                fillColor: '#FF6B6B',
-                                fillOpacity: 0.8
-                            }).addTo(selectionLayer);
-                        });
-                        
                         if (bridge) {
                             var pointsStr = JSON.stringify(rectPoints);
                             bridge.rectangleSelected(pointsStr);
@@ -729,22 +756,36 @@ class DroneMap:
 
                 function focusOn(id) {
                     if (!markers[id]) return;
-                    focusedId = id;
+                    // 非任務狀態只移動一次；任務執行中才記住焦點持續跟隨。
+                    focusedId = missionFollowingEnabled ? id : null;
                     var latlng = markers[id].getLatLng();
                     autoCenteringMap = true;
                     map.flyTo(latlng, map.getZoom());
                     setTimeout(() => { autoCenteringMap = false; }, 500);
                 }
 
-                setInterval(() => {
+                function setMissionFollowingEnabled(enabled, preferredId) {
+                    missionFollowingEnabled = Boolean(enabled);
+                    if (!missionFollowingEnabled) {
+                        // 非任務狀態不跟隨 UAV 或目標點，保留使用者手動地圖視角。
+                        focusedId = null;
+                        return;
+                    }
+
+                    // 任務已在執行時保留原有焦點；尚無焦點時跟隨指定 UAV。
                     if (focusedId && markers[focusedId]) {
-                        if (Date.now() - lastManualMapMoveAt < 1000) {
-                            return;
-                        }
+                        return;
+                    }
+                    if (preferredId && markers[preferredId]) {
+                        focusOn(preferredId);
+                        initialized = true;
+                    }
+                }
+
+                setInterval(() => {
+                    if (missionFollowingEnabled && focusedId && markers[focusedId]) {
                         var latlng = markers[focusedId].getLatLng();
-                        autoCenteringMap = true;
-                        map.panTo(latlng);
-                        setTimeout(() => { autoCenteringMap = false; }, 300);
+                        autoFollowPanTo(latlng);
                     }
                 }, 1000);
 
@@ -760,6 +801,9 @@ class DroneMap:
                             .setLatLng([lat, lon])
                             .setRotationAngle(heading);
                         idLabels[id].setLatLng([lat, lon]);
+                        if (missionFollowingEnabled && id === focusedId) {
+                            autoFollowPanTo(markers[id].getLatLng());
+                        }
                     } else {
                         initTrajectory(id);
                         addTrajectoryPoint(id, lat, lon);
@@ -777,7 +821,6 @@ class DroneMap:
                             if (bridge) {
                                 bridge.emitDroneClicked(id);
                             }
-                            focusOn(id);
                         })
                         .addTo(map);
 
@@ -792,14 +835,23 @@ class DroneMap:
                             if (bridge) {
                                 bridge.emitDroneClicked(id);
                             }
-                            focusOn(id);
                         })
                         .addTo(map);
 
-                        if (!initialized || id < focusedId) {
+                        if (!initialized) {
+                            // 第一次收到 UAV 位置時只做一次初始置中，
+                            // 非任務狀態不設 focusedId，所以後續不會跟隨。
+                            if (missionFollowingEnabled) {
+                                focusedId = id;
+                            }
+                            autoCenteringMap = true;
+                            map.setView([lat, lon], 19);
+                            setTimeout(() => { autoCenteringMap = false; }, 300);
+                            initialized = true;
+                        } else if (missionFollowingEnabled &&
+                                   (!focusedId || id < focusedId)) {
                             focusedId = id;
                             map.setView([lat, lon], 19);
-                            initialized = true;
                         }
                     }
                 }
@@ -907,6 +959,41 @@ class DroneMap:
                     }
                 }
 
+                function drawGridSweepAreaForGroup(groupId, color, rectPoints) {
+                    clearGridSweepAreaForGroup(groupId);
+                    var layerGroup = L.layerGroup().addTo(map);
+                    gridSweepAreaLayers[groupId] = layerGroup;
+                    var bounds = L.latLngBounds(rectPoints);
+
+                    L.rectangle(bounds, {
+                        color: color,
+                        fillColor: color,
+                        fillOpacity: 0.2,
+                        weight: 2
+                    }).addTo(layerGroup);
+                    rectPoints.forEach(point => {
+                        L.circleMarker(point, {
+                            radius: 5,
+                            color: color,
+                            fillColor: color,
+                            fillOpacity: 0.8
+                        }).addTo(layerGroup);
+                    });
+                }
+
+                function clearGridSweepAreaForGroup(groupId, restartSelection) {
+                    if (gridSweepAreaLayers[groupId]) {
+                        map.removeLayer(gridSweepAreaLayers[groupId]);
+                        delete gridSweepAreaLayers[groupId];
+                    }
+                    if (restartSelection) {
+                        clearSelectionMode();
+                        selectionMode = 'rect';
+                        map.dragging.disable();
+                        console.log('Grid Sweep: 已清除範圍並重新進入框選模式');
+                    }
+                }
+
                 function clearAllMissionPlans() {
                     for (var gid in groupMissionLayers) {
                         map.removeLayer(groupMissionLayers[gid]);
@@ -939,6 +1026,9 @@ class DroneMap:
             self.set_font_scale(self.font_scale)
             self.set_socket_colors(self.socket_colors)
             self.set_drone_group_colors(self.drone_group_colors)
+            self.set_mission_following(
+                self.mission_following_enabled,
+                self.mission_following_drone_id)
         else:
             _log("ERROR", "地圖載入失敗")
     
@@ -985,6 +1075,17 @@ class DroneMap:
         if self.map_loaded:
             self.map_view.page().runJavaScript(f"focusOn('{drone_id}');")
 
+    def set_mission_following(self, enabled, preferred_drone_id=None):
+        """只在任務執行中啟用 UAV 自動跟隨。"""
+        self.mission_following_enabled = bool(enabled)
+        self.mission_following_drone_id = preferred_drone_id
+        if self.map_loaded:
+            preferred_json = json.dumps(preferred_drone_id)
+            self.map_view.page().runJavaScript(
+                "setMissionFollowingEnabled("
+                f"{str(self.mission_following_enabled).lower()}, "
+                f"{preferred_json});")
+
     def set_font_scale(self, scale):
         """設定地圖 HTML 控制項的字體倍率。"""
         self.font_scale = scale
@@ -1029,6 +1130,23 @@ class DroneMap:
             self.map_view.page().runJavaScript(f"clearMissionPlanForGroup('{group_id}');")
             _log("INFO", f"地圖已清除 Group {group_id} 任務規劃")
 
+    def draw_grid_sweep_area_for_group(self, group_id, color, rect_corners):
+        """繪製指定群組的 Grid Sweep 框選範圍。"""
+        if self.map_loaded:
+            group_json = json.dumps(group_id)
+            color_json = json.dumps(color)
+            corners_json = json.dumps(rect_corners)
+            self.map_view.page().runJavaScript(
+                f"drawGridSweepAreaForGroup({group_json}, {color_json}, {corners_json});")
+
+    def clear_grid_sweep_area_for_group(self, group_id, restart_selection=False):
+        """清除指定群組的 Grid Sweep 範圍，可選擇重新進入框選模式。"""
+        if self.map_loaded:
+            group_json = json.dumps(group_id)
+            restart_js = str(bool(restart_selection)).lower()
+            self.map_view.page().runJavaScript(
+                f"clearGridSweepAreaForGroup({group_json}, {restart_js});")
+
     def set_mission_mode(self, mode):
         """從 Python 端切換地圖的任務模式（觸發框選/路徑標記等）"""
         if self.map_loaded:
@@ -1113,7 +1231,7 @@ class MapBridge(QObject):
     def emitGpsSignal(self, lat, lon):
         """供 JavaScript 調用的方法"""
         self.gps_signal.emit(lat, lon)
-    
+
     @pyqtSlot(str)
     def emitDroneClicked(self, drone_id):
         """供 JavaScript 調用的方法 - 當無人機被點擊時"""

src/GUI/mission_executor.py

@@ -3,14 +3,19 @@
 任務執行模組
 管理多架無人機的 GUIDED 模式飛行控制迴圈
 
-設計:
+設計 (閉環 / closed-loop):
     - 每架無人機持有一個航點序列，逐點推進
     - 各自到達就各自切換到下一個航點
-    - 事件驅動發送：航點切換時送一次，收到 send_result 才決定重送/前進
-    - 失敗策略 (b)：單機重試 MAX_RETRY 次仍失敗 → 該機 fallback LOITER，其他機繼續
+    - 閉環發送：以固定週期「持續重送」當前目標 setpoint（GUIDED 位置目標冪等），
+      掉包會在下個週期自癒；換點完全靠 GPS 到達判定驅動，不依賴通訊 ACK
+    - send_result（echo 驗證）僅作觀測/UI 顯示，不影響飛行控制：
+      驗證失敗不會 fallback LOITER，指令其實已送達飛控，交給 GPS 續飛
     - Rendezvous barrier：在指定 wp index 等所有活著的機到齊才一起推進，有 timeout 保護
     - 用 QTimer 驅動到達判定，在 Qt 主線程執行
     - 暫停 = 停止到達判定 + 停止新指令
+
+背景：adapter 的 pos_global_int service 被序列化 + 阻塞等 msg87 echo，實飛慢速鏈路下
+多機會只有一台在 client timeout 內拿到回應。閉環設計繞過此瓶頸（見專案記憶）。
 """
 import asyncio
 import math
@@ -42,7 +47,7 @@ class DroneTask:
     """單架無人機的任務資料"""
     __slots__ = (
         'drone_id', 'sysid', 'waypoints', 'wp_index', 'done',
-        'sent_current_wp', 'fail_count', 'status', 'waiting_since',
+        'last_sent_at', 'fail_count', 'status', 'waiting_since',
         'entered_radius_at', 'last_log_at',
     )
 
@@ -52,7 +57,8 @@ class DroneTask:
         self.waypoints = waypoints
         self.wp_index = 0
         self.done = len(waypoints) == 0
-        self.sent_current_wp = False
+        # 上次送出當前目標的 monotonic 時間；0.0 = 尚未送過（下個 tick 立即送）
+        self.last_sent_at = 0.0
         self.fail_count = 0
         self.status = TaskStatus.NORMAL
         self.waiting_since = 0.0  # monotonic time 進入 WAITING_AT_BARRIER 的瞬間
@@ -85,7 +91,7 @@ class MissionExecutor(QObject):
         }
     """
 
-    MAX_RETRY = 3
+    MAX_RETRY = 3  # 已停用：閉環下 echo 驗證失敗不再累計切 LOITER（保留常數以防外部引用）
 
     drone_waypoint_reached = pyqtSignal(str, int, int)   # (drone_id, wp_index, total)
     task_status_changed = pyqtSignal(str, str, str)       # (drone_id, status, message)
@@ -95,12 +101,20 @@ class MissionExecutor(QObject):
                  arrival_radius=4.0, tick_rate_hz=2.0,
                  barrier_timeout_sec=20.0,
                  hover_stable_sec=2.0,
-                 progress_log_interval_sec=3.0):
+                 progress_log_interval_sec=3.0,
+                 resend_interval_sec=2.5,
+                 complete_stops=True):
         super().__init__()
         self.sender = sender
+        # complete_stops=False：全員到達也不停（hover-hold 持續重送），供
+        # orchestrator 的瞬時隊形追蹤使用（靠 update_plan 持續餵新 slot）。
+        self.complete_stops = complete_stops
         self.drone_gps = drone_gps
-        self.monitor = monitor  # 用於失敗 fallback 到 LOITER
+        self.monitor = monitor  # 保留：僅供外部手動 fallback 使用，閉環下不自動切 LOITER
         self.arrival_radius = arrival_radius
+        # 閉環重送週期：每隔這麼久重送一次當前目標 setpoint（掉包自癒）。
+        # 注意：需 > N架 × client_timeout，否則多機序列化會塞不完（見 command_sender）。
+        self.resend_interval_sec = resend_interval_sec
         self.barrier_timeout_sec = barrier_timeout_sec
         # hover-stable 判定：進入 radius 後須穩定停留 hover_stable_sec 秒才算到達，
         # 容忍 GPS 抖動跨越邊界（用 radius * 1.5 作 hysteresis）
@@ -152,10 +166,51 @@ class MissionExecutor(QObject):
             f"共 {total_wps} 個航點, "
             f"到達半徑={self.arrival_radius}m (hover-stable {self.hover_stable_sec}s), "
             f"tick 週期={self._interval_ms}ms, "
+            f"重送週期={self.resend_interval_sec}s (閉環，GPS 驅動換點), "
             f"barrier timeout={self.barrier_timeout_sec}s, "
             f"{rv_info}",
         )
 
+    def update_plan(self, planned_waypoints):
+        """
+        熱替換航點（moving-target 用）。保留 RUNNING 狀態與 timer 不中斷，
+        只換各機的目標座標並重置到達/送出狀態，讓下個 tick 立即朝新目標。
+
+        - 未在執行中 → 等同 start()
+        - 沿用 start() 的 planned_waypoints 格式
+        - drone 集合可變動：新機補 task、消失的機移除；FALLBACK_LOITER 的機不強制復原
+        """
+        if self.state != MissionState.RUNNING:
+            self.start(planned_waypoints)
+            return
+
+        drone_ids = planned_waypoints['drone_ids']
+        waypoints_list = planned_waypoints['waypoints']
+        self.rendezvous_indices = set(
+            planned_waypoints.get('rendezvous_indices', []) or []
+        )
+
+        for i, drone_id in enumerate(drone_ids):
+            wps = waypoints_list[i]
+            task = self.tasks.get(drone_id)
+            if task is None:
+                sysid = int(drone_id.split('_')[1])
+                self.tasks[drone_id] = DroneTask(drone_id, sysid, wps)
+                continue
+            task.waypoints = wps
+            task.wp_index = 0
+            task.done = (len(wps) == 0)
+            task.last_sent_at = 0.0       # 下個 tick 立即重送新目標
+            task.entered_radius_at = 0.0
+            if task.status != TaskStatus.FALLBACK_LOITER:
+                task.status = TaskStatus.NORMAL
+
+        # 移除不在新計畫中的機
+        keep = set(drone_ids)
+        for drone_id in list(self.tasks.keys()):
+            if drone_id not in keep:
+                del self.tasks[drone_id]
+
     def pause(self):
         if self.state == MissionState.RUNNING:
             self._timer.stop()
@@ -249,24 +304,28 @@ class MissionExecutor(QObject):
         # ---- Phase 2: barrier 釋放檢查 ----
         self._check_barriers(now)
 
-        # ---- Phase 3: 發送未送過的目標 ----
+        # ---- Phase 3: 週期性重送當前目標（閉環）----
+        # 每 resend_interval_sec 重送一次目前 wp 的 setpoint，不靠 send_result 決定前進。
+        # GUIDED 位置目標冪等，重送安全；掉包會在下個週期自癒。first send: last_sent_at=0 → 立即送。
         for task in self.tasks.values():
             if task.done or task.status in (
                 TaskStatus.FALLBACK_LOITER, TaskStatus.WAITING_AT_BARRIER
             ):
                 continue
-            if task.sent_current_wp:
-                continue
             target = task.current_target
             if target is None:
                 continue
+            if now - task.last_sent_at < self.resend_interval_sec:
+                continue
             tgt_lat, tgt_lon, tgt_alt = target
-            task.sent_current_wp = True
+            task.last_sent_at = now
             self.sender.send_position_global(
                 task.drone_id, task.sysid, tgt_lat, tgt_lon, tgt_alt
             )
 
         # ---- Phase 4: 完成檢查 ----
+        if not self.complete_stops:
+            return  # hover-hold 模式：不自動結束，持續重送（Phase 3 已處理）
         all_done = all(
             t.done or t.status == TaskStatus.FALLBACK_LOITER
             for t in self.tasks.values()
@@ -293,12 +352,14 @@ class MissionExecutor(QObject):
         )
 
     def _advance_waypoint(self, task, arrived_distance):
-        """把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。"""
+        """把 task 推進一個航點，重置發送計時。不處理 barrier 邏輯。"""
         task.wp_index += 1
-        task.sent_current_wp = False
+        task.last_sent_at = 0.0  # 新 wp 下個 tick 立即送
         task.fail_count = 0
         task.entered_radius_at = 0.0
         task.last_log_at = 0.0
+        if task.status == TaskStatus.RETRYING:
+            task.status = TaskStatus.NORMAL  # 清掉上個 wp 殘留的「送出未驗證」狀態
         if task.wp_index >= task.total_waypoints:
             task.done = True
             self.drone_waypoint_reached.emit(
@@ -369,42 +430,46 @@ class MissionExecutor(QObject):
     # ------------------------------------------------------------------ 結果回呼
 
     def _on_send_result(self, drone_id, sysid, success, message):
-        """Ros2CommandSender.send_result 的 slot"""
+        """
+        Ros2CommandSender.send_result 的 slot（閉環：僅觀測，不影響飛行控制）。
+
+        指令在 adapter 端等 echo 前就已送達飛控，echo 驗證失敗不代表沒送到；
+        因此這裡不再累計失敗切 LOITER，只更新 UI 狀態。真正的前進由 GPS 到達判定驅動，
+        重送由 Phase 3 的週期性重送負責。
+        """
         task = self.tasks.get(drone_id)
         if task is None or task.done:
             return
-        # 若已在 barrier 等待，舊指令的遲到回應不要再觸發重試/fallback
+        # 若已在 barrier 等待，舊指令的遲到回應不要再干擾狀態
         if task.status == TaskStatus.WAITING_AT_BARRIER:
             return
 
         if success:
             if task.fail_count > 0 or task.status == TaskStatus.RETRYING:
                 task.status = TaskStatus.NORMAL
-                self.task_status_changed.emit(drone_id, task.status.value, "recovered")
+                self.task_status_changed.emit(drone_id, task.status.value, "send verified")
             task.fail_count = 0
             return
 
+        # echo 驗證失敗：僅記錄 + UI 提示，不改變飛行（交給 GPS 續飛、下個週期重送）
         task.fail_count += 1
-        _log("WARN", f"{drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}")
-
-        if task.fail_count < self.MAX_RETRY:
-            task.status = TaskStatus.RETRYING
-            task.sent_current_wp = False  # 下個 tick 會重送
-            self.task_status_changed.emit(
-                drone_id, task.status.value,
-                f"retry {task.fail_count}/{self.MAX_RETRY}: {message}"
-            )
-        else:
-            task.status = TaskStatus.FALLBACK_LOITER
-            self.task_status_changed.emit(
-                drone_id, task.status.value,
-                f"fallback LOITER after {self.MAX_RETRY} fails: {message}"
-            )
-            _log("ERROR", f"{drone_id} 連續失敗 {self.MAX_RETRY} 次，切換至 LOITER")
-            self._fallback_to_loiter(drone_id)
+        task.status = TaskStatus.RETRYING  # 純觀測用途，Phase 1/3 不會因此停飛
+        _log(
+            "WARN",
+            f"{drone_id} 送出未驗證 x{task.fail_count}（指令已送達飛控，由 GPS 續飛）: {message}",
+        )
+        self.task_status_changed.emit(
+            drone_id, task.status.value,
+            f"send unverified x{task.fail_count} (GPS 續飛): {message}"
+        )
 
     def _fallback_to_loiter(self, drone_id):
-        """用 monitor.set_mode 切 LOITER。set_mode 是 coroutine，透過 event loop 派送。"""
+        """
+        用 monitor.set_mode 切 LOITER。set_mode 是 coroutine，透過 event loop 派送。
+
+        閉環改版後不再由 _on_send_result 自動呼叫（送指令失敗不代表沒送到）；
+        保留此方法供外部（例如 GUI 手動安全按鈕）需要時使用。
+        """
         if self.monitor is None:
             _log("WARN", f"無 monitor，無法將 {drone_id} 切換至 LOITER")
             return

src/GUI/mission_group.py

@@ -24,10 +24,10 @@ GROUP_COLORS = [
 
 DEFAULT_MISSION_PARAM_VALUES = {
     'spacing': '5.0',
-    'base_altitude': '10.0',
+    'base_altitude': '30.0',
     'altitude_diff': '2.0',
     'radius': '10.0',
-    'altitude': '10.0',
+    'altitude': '30.0',
     'start_angle': '0',
     'lateral_offset': '3.0',
     'longitudinal_spacing': '5.0',
@@ -57,6 +57,7 @@ class MissionGroup:
         self.planned_waypoints = None        # 規劃結果 dict
         self.executor = None                 # MissionExecutor 實例（延遲建立）
         self.center_origin = None            # 規劃原點
+        self.mission_target = None           # (lat, lon)，最近點擊的地圖目標供 XBee GOTO 使用
         self.leader_drone_id = None          # LEADER_FOLLOWER 專用：指定的領隊無人機 ID
 
     @property
@@ -163,6 +164,7 @@ class GroupPanel(QWidget):
     arm_requested = pyqtSignal(str)                    # group_id
     takeoff_requested = pyqtSignal(str, float)         # group_id, altitude
     reboot_requested = pyqtSignal(str)                 # group_id
+    xbee_command_requested = pyqtSignal(str, str)      # group_id, START/STOP/GOTO
     box_select_requested = pyqtSignal(str)             # group_id — 框選直接分配
     select_all_requested = pyqtSignal(str)             # group_id — 全選直接分配
     clear_group_requested = pyqtSignal(str)            # group_id — 清除分組
@@ -247,8 +249,8 @@ class GroupPanel(QWidget):
         takeoff_row.setSpacing(3)
         self.alt_input = QComboBox()
         self.alt_input.setEditable(True)
-        self.alt_input.addItems(["5", "10", "15", "20"])
-        self.alt_input.setCurrentText("10")
+        self.alt_input.addItems(["5", "10", "15", "20", "25", "30"])
+        self.alt_input.setCurrentText("30")
         self.alt_input.setStyleSheet(COMBO)
         alt_lbl = QLabel("m")
         alt_lbl.setStyleSheet(LBL)
@@ -394,6 +396,43 @@ class GroupPanel(QWidget):
         reboot_col.addWidget(reboot_btn)
         reboot_col.addStretch()
 
+        # ============================
+        # XBee JSON 指令（GOTO 使用最近點擊的地圖目標點）
+        # ============================
+        xbee_col = QVBoxLayout()
+        xbee_col.setSpacing(3)
+
+        xbee_title = QLabel("XBee/WS 指令")
+        xbee_title.setStyleSheet(TITLE)
+        xbee_col.addWidget(xbee_title)
+
+        xbee_buttons = QVBoxLayout()
+        xbee_buttons.setSpacing(3)
+        start_xbee_btn = QPushButton("START")
+        start_xbee_btn.setStyleSheet(
+            BTN.format(bg='#2E7D32', fg='white', hover='#388E3C'))
+        start_xbee_btn.clicked.connect(
+            lambda: self.xbee_command_requested.emit(
+                self.group.group_id, 'START'))
+        stop_xbee_btn = QPushButton("STOP")
+        stop_xbee_btn.setStyleSheet(
+            BTN.format(bg='#C62828', fg='white', hover='#D32F2F'))
+        stop_xbee_btn.clicked.connect(
+            lambda: self.xbee_command_requested.emit(
+                self.group.group_id, 'STOP'))
+        goto_xbee_btn = QPushButton("GOTO")
+        goto_xbee_btn.setToolTip("前往目前 Group 最近一次點擊的地圖目標點")
+        goto_xbee_btn.setStyleSheet(
+            BTN.format(bg='#1565C0', fg='white', hover='#1976D2'))
+        goto_xbee_btn.clicked.connect(
+            lambda: self.xbee_command_requested.emit(
+                self.group.group_id, 'GOTO'))
+        xbee_buttons.addWidget(start_xbee_btn)
+        xbee_buttons.addWidget(stop_xbee_btn)
+        xbee_buttons.addWidget(goto_xbee_btn)
+        xbee_col.addLayout(xbee_buttons)
+        xbee_col.addStretch()
+
         # ============================
         # 第四欄：任務參數
         # ============================
@@ -500,7 +539,7 @@ class GroupPanel(QWidget):
         # 當任務類型切換時更新參數顯示
         self.type_combo.currentTextChanged.connect(self._update_param_visibility)
 
-        # ── 組裝欄位：控制指令 > 任務規劃 > 任務參數 > 選取與分組 > 飛控操作 ──
+        # ── 組裝欄位：控制指令 > 任務規劃 > 任務參數 > 選取與分組 > 飛控操作 > XBee ──
         # 使用伸展因子 0 讓列根據內容自動調整寬度，而不是均等分配
         cols.addLayout(left, 0)
         cols.addWidget(self._make_sep())
@@ -511,6 +550,8 @@ class GroupPanel(QWidget):
         cols.addLayout(right, 0)
         cols.addWidget(self._make_sep())
         cols.addLayout(reboot_col, 0)
+        cols.addWidget(self._make_sep())
+        cols.addLayout(xbee_col, 0)
         cols.addStretch()  # 填充剩餘空間，使欄位置左
 
         layout.addLayout(cols)
@@ -619,6 +660,7 @@ class GroupPanel(QWidget):
 
     def update_mission_info(self, center_lat, center_lon, target_lat, target_lon):
         """更新中心點 / 目標點顯示"""
+        self.group.mission_target = (float(target_lat), float(target_lon))
         info_style = f"color: {self.group.color}; font-size: 11px; font-weight: bold;"
         self.center_label.setText(f"中心: {center_lat:.6f}°, {center_lon:.6f}°")
         self.center_label.setStyleSheet(info_style)
@@ -627,6 +669,7 @@ class GroupPanel(QWidget):
 
     def clear_mission_info(self):
         """清除中心點 / 目標點顯示"""
+        self.group.mission_target = None
         self.center_label.setText("中心: --")
         self.center_label.setStyleSheet("color: #AAA; font-size: 11px;")
         self.target_label.setText("目標: --")
@@ -636,5 +679,5 @@ class GroupPanel(QWidget):
         try:
             alt = float(self.alt_input.currentText())
         except ValueError:
-            alt = 10.0
+            alt = 30.0
         self.takeoff_requested.emit(self.group.group_id, alt)

src/GUI/mission_orchestrator1.py

@@ -0,0 +1,710 @@
+#!/usr/bin/env python3
+"""
+任務編排器 (MissionOrchestrator) —— ABC 對抗任務的階段狀態機。
+
+角色：坐在既有 MissionExecutor 之上的「任務大腦」。偵察飛行由操作員用既有的
+群組執行按鈕控制（A 組 GRID_SWEEP、B 組待機）；orchestrator 只做兩件事：
+  1. 被動監看「A↔敵機」距離，達 R_detect 持續 dwell → latch，提示操作員。
+  2. 操作員按「重編隊」→ 接管 A+B 四機，用「瞬時隊形」追移動敵機、足夠近時展開包圍。
+
+追蹤採「瞬時隊形」而非路徑跟隨：每 tick 給每台一個目標點（此刻該站的隊形位置），
+敵機移動就更新。避免了路徑跟隨在移動目標下 index 歸零/往後外推造成的往返振盪。
+
+階段：IDLE → WATCH（監看偵測）→ DETECTED（latch，等確認）
+      → PURSUE（4 機瞬時 leader-follow 朝敵，B 在後方 slot 自然追上會合）
+      → ENCIRCLE（形心距敵 ≤ R_engage 展開包圍，圓心追敵；逃出 R_engage_exit 退回）
+
+設計定案見 memory: design_mission_orchestrator。修改限定 src/GUI/（scope_gui_only）。
+"""
+import math
+import time
+from enum import Enum
+
+from PyQt6.QtWidgets import (
+    QWidget, QVBoxLayout, QHBoxLayout, QLabel, QComboBox, QFrame, QPushButton,
+    QDoubleSpinBox
+)
+from PyQt6.QtCore import Qt, QObject, pyqtSignal
+
+
+def _log(level, message):
+    print(f"[{level}] {message}", flush=True)
+
+
+def _haversine(lat1, lon1, lat2, lon2):
+    """兩經緯度間的水平地面距離 (m)。"""
+    R = 6371000.0
+    p1 = math.radians(lat1)
+    p2 = math.radians(lat2)
+    dphi = math.radians(lat2 - lat1)
+    dlam = math.radians(lon2 - lon1)
+    a = (math.sin(dphi / 2) ** 2
+         + math.cos(p1) * math.cos(p2) * math.sin(dlam / 2) ** 2)
+    return 2 * R * math.asin(min(1.0, math.sqrt(a)))
+
+
+def _ll_to_m(lat, lon, ref_lat, ref_lon):
+    """經緯度 → 以 ref 為原點的本地平面公尺 (x=東, y=北)。等距近似，小範圍夠用。"""
+    x = math.radians(lon - ref_lon) * 6371000.0 * math.cos(math.radians(ref_lat))
+    y = math.radians(lat - ref_lat) * 6371000.0
+    return x, y
+
+
+def _m_to_ll(x, y, ref_lat, ref_lon):
+    """本地平面公尺 → 經緯度。"""
+    lat = ref_lat + math.degrees(y / 6371000.0)
+    lon = ref_lon + math.degrees(x / (6371000.0 * math.cos(math.radians(ref_lat))))
+    return lat, lon
+
+
+# 預設參數
+DEFAULT_R_DETECT = 40.0        # 偵測範圍 (m)
+DEFAULT_DETECT_DWELL = 1.0     # 偵測 dwell (s)
+
+DEFAULT_R_ENGAGE = 25.0        # 接戰半徑 (m)：形心↔敵 ≤ 此值 → 切包圍
+DEFAULT_R_ENGAGE_EXIT = 35.0   # 包圍退出半徑 (m)：> 此值 → 退回前進（hysteresis）
+
+DEFAULT_REPLAN_MOVE_M = 2.0    # 敵機位移 ≥ 此值即重算（moving target）
+DEFAULT_REPLAN_CAP_SEC = 1.0   # 重算週期上限 (s)
+
+DEFAULT_CIRCLE_RADIUS = 15.0   # 包圍半徑 (m)
+DEFAULT_LON_SPACING = 8.0      # 隊形縱向間距 (m)
+DEFAULT_LAT_OFFSET = 5.0       # 隊形橫向錯開 (m)   
+
+DEFAULT_FORMATION_ALT = 30.0   # 隊形基準高度 (m，相對 home)
+DEFAULT_ALT_LAYER_GAP = 10.0   # A/B 上下層垂直間隔 (m)：A=基準+gap/2、B=基準-gap/2
+DEFAULT_MIN_ALT_FLOOR = 10.0    # 相對 home 的最低安全高度 (m)：B 下層不得低於此值
+
+DEFAULT_LEAD_TIME_CAP = 4.0    # 前置攔截 lead_time 上限 (s)
+DEFAULT_FRIENDLY_SPEED = 5.0   # 友機假設水平速度 (m/s)：估 lead_time = gap/此值
+DEFAULT_ENEMY_VEL_WINDOW = 1.0 # 敵速估計的 GPS 差分視窗 (s)
+
+_NONE_LABEL = "（未選擇）"
+
+
+class MissionPhase(Enum):
+    IDLE = "idle"          # 未設定敵機/A 組
+    WATCH = "watch"        # 監看偵測（偵察飛行由操作員的群組任務負責）
+    DETECTED = "detected"  # 偵測 latch，等操作員確認重編隊
+    PURSUE = "pursue"      # 4 機瞬時 leader-follow 朝敵機
+    ENCIRCLE = "encircle"  # 4 機包圍敵機
+
+
+class MissionOrchestrator(QObject):
+    """
+    階段狀態機。透過注入的 callback 與 GUI 解耦（便於 headless 測試）：
+      make_executor()  -> MissionExecutor（complete_stops=False，hover-hold）
+      gps_provider()   -> {drone_id: {'lat','lon','alt'}}
+      stop_groups_fn() -> 停掉操作員的 A/B 群組任務（重編隊時避免雙重送點）
+    """
+
+    phase_changed = pyqtSignal(str)      # MissionPhase.value
+    detection_prompt = pyqtSignal(str)   # 偵測提示（含最近友機/距離）
+    status_message = pyqtSignal(str)
+
+    def __init__(self, make_executor, gps_provider, stop_groups_fn=None,
+                 r_detect=DEFAULT_R_DETECT,
+                 detect_dwell_sec=DEFAULT_DETECT_DWELL,
+                 r_engage=DEFAULT_R_ENGAGE,
+                 r_engage_exit=DEFAULT_R_ENGAGE_EXIT,
+                 replan_move_m=DEFAULT_REPLAN_MOVE_M,
+                 replan_cap_sec=DEFAULT_REPLAN_CAP_SEC,
+                 circle_radius=DEFAULT_CIRCLE_RADIUS,
+                 lon_spacing=DEFAULT_LON_SPACING,
+                 lat_offset=DEFAULT_LAT_OFFSET,
+                 formation_alt=DEFAULT_FORMATION_ALT,
+                 alt_layer_gap=DEFAULT_ALT_LAYER_GAP,
+                 min_alt_floor=DEFAULT_MIN_ALT_FLOOR,
+                 lead_time_cap_sec=DEFAULT_LEAD_TIME_CAP,
+                 friendly_speed=DEFAULT_FRIENDLY_SPEED,
+                 enemy_vel_window_sec=DEFAULT_ENEMY_VEL_WINDOW,
+                 parent=None):
+        super().__init__(parent)
+        self._make_executor = make_executor
+        self._gps_provider = gps_provider
+        self._stop_groups_fn = stop_groups_fn
+        self.r_detect = float(r_detect)
+        self.detect_dwell_sec = float(detect_dwell_sec)
+        self.r_engage = float(r_engage)
+        self.r_engage_exit = float(r_engage_exit)
+        self.replan_move_m = float(replan_move_m)
+        self.replan_cap_sec = float(replan_cap_sec)
+        self.circle_radius = float(circle_radius)
+        self.lon_spacing = float(lon_spacing)
+        self.lat_offset = float(lat_offset)
+        self.formation_alt = float(formation_alt)
+        self.alt_layer_gap = float(alt_layer_gap)
+        self.min_alt_floor = float(min_alt_floor)
+        self.lead_time_cap_sec = float(lead_time_cap_sec)
+        self.friendly_speed = float(friendly_speed)
+        self.enemy_vel_window_sec = float(enemy_vel_window_sec)
+
+        self.phase = MissionPhase.IDLE
+        self.a_drone_ids = []   # 偵察組
+        self.b_drone_ids = []   # 待機組
+        self.enemy_id = None
+
+        self._detect_since = 0.0
+        self._merged_ids = []           # PURSUE/ENCIRCLE 的 4 機（領隊優先，固定順序）
+        self._merged_ex = None          # 合併 executor（hover-hold）
+        self._last_replan_at = 0.0
+        self._last_plan_enemy = None    # 上次重算用的敵機 (lat, lon)
+        self._last_heading = (0.0, 1.0)  # 上次隊形朝向（敵機太近時沿用，避免抖動）
+        self._enemy_hist = []           # [(t, lat, lon)] 敵機軌跡（估速用）
+        self._enemy_vel = (0.0, 0.0)    # 估得的敵速 (east, north) m/s
+
+    # ------------------------------------------------------------- 設定（可連續呼叫）
+    def configure(self, a_drone_ids, b_drone_ids, enemy_id):
+        """設定 A/B 組與敵機。監看/待機階段可隨時更新；進入 PURSUE 後鎖定不受影響。"""
+        if self.phase in (MissionPhase.PURSUE, MissionPhase.ENCIRCLE):
+            return False
+        self.a_drone_ids = list(a_drone_ids)
+        self.b_drone_ids = list(b_drone_ids)
+        new_enemy = enemy_id or None
+        if new_enemy != self.enemy_id:
+            self._detect_since = 0.0  # 換敵機重置 dwell
+        self.enemy_id = new_enemy
+        return True
+
+    def _configured(self):
+        return bool(self.enemy_id) and bool(self.a_drone_ids) \
+            and self.enemy_id not in set(self.a_drone_ids) | set(self.b_drone_ids) \
+            and not (set(self.a_drone_ids) & set(self.b_drone_ids))
+
+    # ------------------------------------------------------------- 重編隊 / 停止
+    def confirm_detection(self):
+        """DETECTED → PURSUE：操作員手動確認。停掉群組掃描，接管 4 機瞬時追蹤。"""
+        if self.phase != MissionPhase.DETECTED:
+            self.status_message.emit("目前不在待確認狀態")
+            return
+        gps = self._gps_provider()
+        epos = gps.get(self.enemy_id)
+        if not epos:
+            self.status_message.emit("敵機定位遺失，無法重編隊")
+            return
+        merged = self._merged_ids_leader_first(gps, epos)
+        if self._positions_for(merged, gps) is None:
+            self.status_message.emit("有無人機尚未定位，無法重編隊")
+            return
+
+        # 先停掉操作員的 A/B 群組任務，避免與合併 executor 雙重送點塞爆 adapter
+        if self._stop_groups_fn:
+            try:
+                self._stop_groups_fn()
+            except Exception as e:
+                _log("WARN", f"[orchestrator] 停止群組任務失敗: {e}")
+
+        self._merged_ids = merged
+        self._last_heading = (0.0, 1.0)
+        self._enemy_hist = []
+        self._enemy_vel = (0.0, 0.0)
+        self._update_enemy_velocity(epos)
+        slots = self._pursue_slots(gps, epos)
+        self._merged_ex = self._make_executor()
+        self._merged_ex.start(self._slots_to_plan(slots))
+        self._mark_replanned(epos)
+        self._set_phase(MissionPhase.PURSUE)
+        self.status_message.emit("重編隊確認：4 機前進中（B 追上會合）")
+
+    def stop(self):
+        """全體停止，回 IDLE。"""
+        if self._merged_ex is not None:
+            try:
+                self._merged_ex.stop()
+            except Exception:
+                pass
+            self._merged_ex = None
+        self._merged_ids = []
+        self._detect_since = 0.0
+        self._enemy_hist = []
+        self._enemy_vel = (0.0, 0.0)
+        self._set_phase(MissionPhase.IDLE)
+        self.status_message.emit("任務已停止")
+
+    # ------------------------------------------------------------- tick
+    def tick(self):
+        """GUI 週期呼叫。"""
+        # 自動進出監看（偵察飛行由操作員群組任務負責，這裡只監看偵測）
+        if self.phase == MissionPhase.IDLE and self._configured():
+            self._set_phase(MissionPhase.WATCH)
+        elif self.phase == MissionPhase.WATCH and not self._configured():
+            self._detect_since = 0.0
+            self._set_phase(MissionPhase.IDLE)
+
+        if self.phase == MissionPhase.WATCH:
+            self._tick_watch()
+        elif self.phase == MissionPhase.PURSUE:
+            self._tick_pursue()
+        elif self.phase == MissionPhase.ENCIRCLE:
+            self._tick_encircle()
+
+    def _tick_watch(self):
+        gps = self._gps_provider()
+        epos = gps.get(self.enemy_id)
+        if not epos:
+            self._detect_since = 0.0
+            return
+        nearest_id, nearest_d = None, None
+        for did in self.a_drone_ids:
+            pos = gps.get(did)
+            if not pos:
+                continue
+            d = _haversine(epos['lat'], epos['lon'], pos['lat'], pos['lon'])
+            if nearest_d is None or d < nearest_d:
+                nearest_id, nearest_d = did, d
+
+        now = time.monotonic()
+        if nearest_d is not None and nearest_d <= self.r_detect:
+            if self._detect_since == 0.0:
+                self._detect_since = now
+            elif now - self._detect_since >= self.detect_dwell_sec:
+                self._set_phase(MissionPhase.DETECTED)  # latch，等操作員手動確認
+                self.detection_prompt.emit(
+                    f"偵測到敵機（{nearest_id} 距 {nearest_d:.1f} m）— 請確認重編隊")
+        else:
+            self._detect_since = 0.0
+
+    def _tick_pursue(self):
+        gps = self._gps_provider()
+        epos = gps.get(self.enemy_id)
+        if not epos:
+            return  # 敵機定位遺失（S4 加 stale-timeout hold）
+        self._update_enemy_velocity(epos)
+        d = self._centroid_dist(gps, epos)
+        if d is None:
+            return
+        if d <= self.r_engage:
+            self._apply_slots(self._circle_slots(gps, epos), epos)
+            self._set_phase(MissionPhase.ENCIRCLE)
+            self.status_message.emit("進入包圍")
+            return
+        if self._should_replan(time.monotonic(), epos):
+            self._apply_slots(self._pursue_slots(gps, epos), epos)
+
+    def _tick_encircle(self):
+        gps = self._gps_provider()
+        epos = gps.get(self.enemy_id)
+        if not epos:
+            return
+        self._update_enemy_velocity(epos)
+        d = self._centroid_dist(gps, epos)
+        if d is None:
+            return
+        if d > self.r_engage_exit:
+            self._apply_slots(self._pursue_slots(gps, epos), epos)
+            self._set_phase(MissionPhase.PURSUE)
+            self.status_message.emit("敵機逃離，退回前進")
+            return
+        if self._should_replan(time.monotonic(), epos):
+            self._apply_slots(self._circle_slots(gps, epos), epos)
+
+    # ------------------------------------------------------------- 高度分層
+    def _alt_for(self, did):
+        """S3 高度分層：A 組（含領隊）走上層、B 組走下層（不低於地面下限）。
+        同高度繞圈/追擊會交叉撞機，分層後 A/B 垂直錯開自然避開。"""
+        if did in set(self.b_drone_ids):
+            return max(self.min_alt_floor,
+                       self.formation_alt - self.alt_layer_gap / 2.0)
+        return self.formation_alt + self.alt_layer_gap / 2.0
+
+    # ------------------------------------------------------------- 前置攔截（估敵速）
+    def _update_enemy_velocity(self, epos):
+        """用敵機 GPS 差分估水平速度 (east, north) m/s，視窗 enemy_vel_window_sec。"""
+        now = time.monotonic()
+        self._enemy_hist.append((now, epos['lat'], epos['lon']))
+        while (len(self._enemy_hist) >= 2
+               and now - self._enemy_hist[0][0] > self.enemy_vel_window_sec):
+            self._enemy_hist.pop(0)
+        if len(self._enemy_hist) >= 2:
+            t0, la0, lo0 = self._enemy_hist[0]
+            t1, la1, lo1 = self._enemy_hist[-1]
+            dt = t1 - t0
+            if dt > 1e-3:
+                dx, dy = _ll_to_m(la1, lo1, la0, lo0)  # sample0→sample1 位移 (m)
+                self._enemy_vel = (dx / dt, dy / dt)
+
+    def enemy_speed(self):
+        """估得的敵機水平速率 (m/s)。"""
+        return math.hypot(*self._enemy_vel)
+
+    def _aim_point(self, gps, epos):
+        """前置攔截點：敵位 + 敵速×lead_time。lead_time = gap/friendly_speed（封頂
+        lead_time_cap_sec）。敵速尚未估到時退化為瞄當下位置（等同原純追尾）。"""
+        evx, evy = self._enemy_vel
+        lp = gps.get(self._merged_ids[0]) if self._merged_ids else None
+        gap = (_haversine(lp['lat'], lp['lon'], epos['lat'], epos['lon'])
+               if lp else 0.0)
+        lead_t = min(self.lead_time_cap_sec, gap / max(self.friendly_speed, 0.1))
+        return _m_to_ll(evx * lead_t, evy * lead_t, epos['lat'], epos['lon'])
+
+    # ------------------------------------------------------------- 瞬時隊形
+    def _pursue_slots(self, gps, epos):
+        """朝『前置攔截點』的瞬時 leader-follow：領隊朝攔截點，僚機沿隊軸後方 rank×間距、左右錯開。"""
+        ids = self._merged_ids
+        tlat, tlon = self._aim_point(gps, epos)  # 前置攔截點取代敵機當下位置
+        lp = gps.get(ids[0])
+        if lp is None:
+            return None
+        lx, ly = _ll_to_m(lp['lat'], lp['lon'], tlat, tlon)  # 領隊相對攔截點
+        hx, hy = -lx, -ly                                    # 領隊 → 攔截點
+        n = math.hypot(hx, hy)
+        if n < 1.0:
+            h = self._last_heading  # 太近，沿用上次朝向避免抖動
+        else:
+            h = (hx / n, hy / n)
+        self._last_heading = h
+        px, py = -h[1], h[0]  # 左垂直
+        slots = {}
+        for rank, d in enumerate(ids):
+            lat_sign = -1 if rank % 2 == 0 else 1
+            # 領隊(rank0)目標≈攔截點；僚機沿 -h 後退 rank×間距，並左右錯開
+            sx = -h[0] * (rank * self.lon_spacing) + px * (lat_sign * self.lat_offset)
+            sy = -h[1] * (rank * self.lon_spacing) + py * (lat_sign * self.lat_offset)
+            lat, lon = _m_to_ll(sx, sy, tlat, tlon)
+            slots[d] = (lat, lon, self._alt_for(d))
+        return slots
+
+    def _circle_slots(self, gps, epos):
+        """繞敵機的瞬時包圍：N 機均分在半徑 circle_radius 的圓周上。"""
+        ids = self._merged_ids
+        elat, elon = epos['lat'], epos['lon']
+        n = len(ids)
+        slots = {}
+        for i, d in enumerate(ids):
+            theta = 2.0 * math.pi * i / max(1, n)
+            x = self.circle_radius * math.cos(theta)
+            y = self.circle_radius * math.sin(theta)
+            lat, lon = _m_to_ll(x, y, elat, elon)
+            slots[d] = (lat, lon, self._alt_for(d))
+        return slots
+
+    def _apply_slots(self, slots, epos):
+        """把瞬時 slot 熱替換給合併 executor 並更新重算基準。"""
+        if slots is None or self._merged_ex is None:
+            return
+        self._merged_ex.update_plan(self._slots_to_plan(slots))
+        self._mark_replanned(epos)
+
+    def _slots_to_plan(self, slots):
+        return {
+            'drone_ids': list(self._merged_ids),
+            'waypoints': [[slots[d]] for d in self._merged_ids],
+            'rendezvous_indices': [],
+        }
+
+    # ------------------------------------------------------------- 內部
+    def _centroid_dist(self, gps, epos):
+        m_pos = self._positions_for(self._merged_ids, gps)
+        if m_pos is None:
+            return None
+        cen_lat = sum(p[0] for p in m_pos) / len(m_pos)
+        cen_lon = sum(p[1] for p in m_pos) / len(m_pos)
+        return _haversine(cen_lat, cen_lon, epos['lat'], epos['lon'])
+
+    def _positions_for(self, drone_ids, gps):
+        out = []
+        for did in drone_ids:
+            p = gps.get(did)
+            if not p:
+                return None
+            out.append((p['lat'], p['lon'], p.get('alt', 0.0)))
+        return out
+
+    def _merged_ids_leader_first(self, gps, epos):
+        """A+B 合併；A 中離敵最近者當領隊(rank0)，其餘 A 維持原序，再接 B。"""
+        def dist(did):
+            p = gps.get(did)
+            return float('inf') if not p else _haversine(
+                epos['lat'], epos['lon'], p['lat'], p['lon'])
+        if not self.a_drone_ids:
+            return list(self.b_drone_ids)
+        leader = min(self.a_drone_ids, key=dist)
+        rest_a = [d for d in self.a_drone_ids if d != leader]
+        return [leader] + rest_a + list(self.b_drone_ids)
+
+    def _mark_replanned(self, epos):
+        self._last_replan_at = time.monotonic()
+        self._last_plan_enemy = (epos['lat'], epos['lon'])
+
+    def _should_replan(self, now, epos):
+        if self._last_plan_enemy is None:
+            return True
+        moved = _haversine(self._last_plan_enemy[0], self._last_plan_enemy[1],
+                           epos['lat'], epos['lon'])
+        return (moved >= self.replan_move_m
+                or (now - self._last_replan_at) >= self.replan_cap_sec)
+
+    def _set_phase(self, phase):
+        if phase == self.phase:
+            return
+        self.phase = phase
+        _log("INFO", f"[orchestrator] phase → {phase.value}")
+        self.phase_changed.emit(phase.value)
+
+
+class EngagementPanel(QWidget):
+    """
+    交戰控制面板。
+
+    - 敵機下拉 + 即時「敵機↔最近友機」距離
+    - A/B 組指派下拉
+    - 階段顯示、重編隊 / 停止 按鈕
+
+    偵察飛行用既有的群組執行按鈕控制，這裡不含「開始」。面板只發訊號，邏輯在 GUI/orchestrator。
+    """
+
+    enemy_changed = pyqtSignal(str)        # drone_id（未選擇=空字串）
+    confirm_requested = pyqtSignal()       # 重編隊
+    stop_requested = pyqtSignal()
+    groups_changed = pyqtSignal(str, str)  # (a_group_id, b_group_id)
+    apply_speed_requested = pyqtSignal(float, float)  # (friendly_speed, enemy_ratio)
+
+    def __init__(self, parent=None, r_detect=DEFAULT_R_DETECT,
+                 friendly_speed=DEFAULT_FRIENDLY_SPEED):
+        super().__init__(parent)
+        self.r_detect = float(r_detect)
+        self._init_friendly_speed = float(friendly_speed)
+        self._enemy_id = ""
+        self._suppress_enemy = False
+        self._suppress_group = False
+        self._group_sig = None  # 目前下拉內容的簽章，變了才重建（避免 5Hz 狂刷）
+        self._build_ui()
+
+    # ------------------------------------------------------------------ UI
+    def _build_ui(self):
+        root = QVBoxLayout(self)
+        root.setContentsMargins(12, 12, 12, 12)
+        root.setSpacing(10)
+        root.setAlignment(Qt.AlignmentFlag.AlignTop)
+
+        title = QLabel("交戰設定")
+        title.setStyleSheet(
+            "color: #DDD; font-size: 14px; font-weight: bold; padding: 2px;")
+        root.addWidget(title)
+
+        combo_css = (
+            "QComboBox { background-color: #333; color: #EEE; border: 1px solid #555;"
+            " border-radius: 3px; padding: 3px 6px; font-size: 12px; min-width: 120px; }")
+
+        enemy_row = QHBoxLayout()
+        enemy_row.setSpacing(8)
+        el = QLabel("敵機 sysid：")
+        el.setStyleSheet("color: #CCC; font-size: 12px;")
+        self.enemy_combo = QComboBox()
+        self.enemy_combo.setStyleSheet(combo_css)
+        self.enemy_combo.addItem(_NONE_LABEL)
+        self.enemy_combo.currentTextChanged.connect(self._on_enemy_changed)
+        enemy_row.addWidget(el)
+        enemy_row.addWidget(self.enemy_combo)
+        enemy_row.addStretch()
+        root.addLayout(enemy_row)
+
+        a_row = QHBoxLayout()
+        a_row.setSpacing(8)
+        al = QLabel("偵察組 A：")
+        al.setStyleSheet("color: #CCC; font-size: 12px;")
+        self.a_group_combo = QComboBox()
+        self.a_group_combo.setStyleSheet(combo_css)
+        self.a_group_combo.addItem(_NONE_LABEL, None)
+        self.a_group_combo.currentTextChanged.connect(self._on_group_changed)
+        a_row.addWidget(al)
+        a_row.addWidget(self.a_group_combo)
+        a_row.addStretch()
+        root.addLayout(a_row)
+
+        b_row = QHBoxLayout()
+        b_row.setSpacing(8)
+        bl = QLabel("待機組 B：")
+        bl.setStyleSheet("color: #CCC; font-size: 12px;")
+        self.b_group_combo = QComboBox()
+        self.b_group_combo.setStyleSheet(combo_css)
+        self.b_group_combo.addItem(_NONE_LABEL, None)
+        self.b_group_combo.currentTextChanged.connect(self._on_group_changed)
+        b_row.addWidget(bl)
+        b_row.addWidget(self.b_group_combo)
+        b_row.addStretch()
+        root.addLayout(b_row)
+
+        # 速度比例限速：友方 V、敵方 r×V，按鈕一次套用（DO_CHANGE_SPEED）
+        spin_css = (
+            "QDoubleSpinBox { background-color: #333; color: #EEE;"
+            " border: 1px solid #555; border-radius: 3px; padding: 2px 4px;"
+            " font-size: 12px; min-width: 70px; }")
+        speed_row = QHBoxLayout()
+        speed_row.setSpacing(8)
+        sl = QLabel("友機速度：")
+        sl.setStyleSheet("color: #CCC; font-size: 12px;")
+        self.friendly_speed_spin = QDoubleSpinBox()
+        self.friendly_speed_spin.setStyleSheet(spin_css)
+        self.friendly_speed_spin.setRange(0.5, 20.0)
+        self.friendly_speed_spin.setSingleStep(0.5)
+        self.friendly_speed_spin.setDecimals(1)
+        self.friendly_speed_spin.setSuffix(" m/s")
+        self.friendly_speed_spin.setValue(self._init_friendly_speed)
+        rl = QLabel("敵/友比例：")
+        rl.setStyleSheet("color: #CCC; font-size: 12px;")
+        self.enemy_ratio_spin = QDoubleSpinBox()
+        self.enemy_ratio_spin.setStyleSheet(spin_css)
+        self.enemy_ratio_spin.setRange(0.1, 1.0)
+        self.enemy_ratio_spin.setSingleStep(0.05)
+        self.enemy_ratio_spin.setDecimals(2)
+        self.enemy_ratio_spin.setValue(0.6)
+        speed_row.addWidget(sl)
+        speed_row.addWidget(self.friendly_speed_spin)
+        speed_row.addWidget(rl)
+        speed_row.addWidget(self.enemy_ratio_spin)
+        speed_row.addStretch()
+        root.addLayout(speed_row)
+
+        apply_speed_row = QHBoxLayout()
+        self.apply_speed_btn = QPushButton("套用速度")
+        self.apply_speed_btn.setStyleSheet(
+            "QPushButton { background-color: #00796B; color: white; border: none;"
+            " padding: 6px 14px; border-radius: 4px; font-weight: bold; }"
+            " QPushButton:hover { background-color: #00897B; }")
+        self.apply_speed_btn.clicked.connect(self._on_apply_speed)
+        self.enemy_speed_hint = QLabel("")
+        self.enemy_speed_hint.setStyleSheet("color: #777; font-size: 11px;")
+        self._refresh_enemy_speed_hint()
+        self.friendly_speed_spin.valueChanged.connect(self._refresh_enemy_speed_hint)
+        self.enemy_ratio_spin.valueChanged.connect(self._refresh_enemy_speed_hint)
+        apply_speed_row.addWidget(self.apply_speed_btn)
+        apply_speed_row.addWidget(self.enemy_speed_hint)
+        apply_speed_row.addStretch()
+        root.addLayout(apply_speed_row)
+
+        sep = QFrame()
+        sep.setFrameShape(QFrame.Shape.HLine)
+        sep.setStyleSheet("color: #444;")
+        root.addWidget(sep)
+
+        self.distance_label = QLabel("敵機 ↔ 最近友機：—")
+        self.distance_label.setStyleSheet(
+            "color: #AAA; font-size: 13px; padding: 4px 2px;")
+        root.addWidget(self.distance_label)
+
+        hint = QLabel(f"（偵測範圍 R_detect = {self.r_detect:.0f} m）")
+        hint.setStyleSheet("color: #777; font-size: 11px;")
+        root.addWidget(hint)
+
+        self.phase_label = QLabel("階段：待機")
+        self.phase_label.setStyleSheet(
+            "color: #64B5F6; font-size: 13px; font-weight: bold; padding: 4px 2px;")
+        root.addWidget(self.phase_label)
+
+        btn_css = ("QPushButton {{ background-color: {bg}; color: {fg}; border: none;"
+                   " padding: 8px 16px; border-radius: 4px; font-weight: bold; }}"
+                   " QPushButton:hover {{ background-color: {hover}; }}"
+                   " QPushButton:disabled {{ background-color: #444; color: #888; }}")
+        btn_row = QHBoxLayout()
+        btn_row.setSpacing(8)
+        self.confirm_btn = QPushButton("重編隊")
+        self.confirm_btn.setStyleSheet(btn_css.format(bg='#F57C00', fg='white', hover='#FB8C00'))
+        self.confirm_btn.clicked.connect(lambda: self.confirm_requested.emit())
+        self.confirm_btn.setEnabled(False)
+        self.stop_btn = QPushButton("停止")
+        self.stop_btn.setStyleSheet(btn_css.format(bg='#555', fg='#DDD', hover='#666'))
+        self.stop_btn.clicked.connect(lambda: self.stop_requested.emit())
+        btn_row.addWidget(self.confirm_btn)
+        btn_row.addWidget(self.stop_btn)
+        btn_row.addStretch()
+        root.addLayout(btn_row)
+
+    # -------------------------------------------------------------- 對外 API
+    def current_enemy(self):
+        return self._enemy_id or None
+
+    def refresh_drone_list(self, drone_ids):
+        items = [_NONE_LABEL] + sorted(drone_ids)
+        current = self._enemy_id if self._enemy_id in drone_ids else ""
+        self._suppress_enemy = True
+        self.enemy_combo.clear()
+        self.enemy_combo.addItems(items)
+        idx = self.enemy_combo.findText(current) if current else 0
+        self.enemy_combo.setCurrentIndex(idx if idx >= 0 else 0)
+        self._suppress_enemy = False
+        if current != self._enemy_id:
+            self._enemy_id = current
+            self.enemy_changed.emit(self._enemy_id)
+
+    def refresh_group_list(self, group_labels):
+        """group_labels: list[(group_id, display_text)]。只有內容變動才重建，避免 5Hz 狂刷。"""
+        sig = tuple(group_labels)
+        if sig == self._group_sig:
+            return
+        self._group_sig = sig
+        self._suppress_group = True
+        for combo in (self.a_group_combo, self.b_group_combo):
+            prev = combo.currentData()
+            combo.clear()
+            combo.addItem(_NONE_LABEL, None)
+            for gid, text in group_labels:
+                combo.addItem(text, gid)
+            if prev is not None:
+                i = combo.findData(prev)
+                combo.setCurrentIndex(i if i >= 0 else 0)
+        self._suppress_group = False
+
+    def selected_group_ids(self):
+        return self.a_group_combo.currentData(), self.b_group_combo.currentData()
+
+    def set_phase_text(self, text):
+        self.phase_label.setText(f"階段：{text}")
+
+    def set_confirm_enabled(self, enabled):
+        self.confirm_btn.setEnabled(enabled)
+
+    def tick(self, drone_positions):
+        """{drone_id: (lat, lon)} 更新距離讀數。"""
+        enemy = self._enemy_id
+        base = "color: #AAA; font-size: 13px; padding: 4px 2px;"
+        if not enemy:
+            self.distance_label.setText("敵機 ↔ 最近友機：（尚未選擇敵機）")
+            self.distance_label.setStyleSheet(base)
+            return
+        epos = drone_positions.get(enemy)
+        if not epos:
+            self.distance_label.setText(f"敵機 {enemy} ↔ 最近友機：（等待敵機定位）")
+            self.distance_label.setStyleSheet(base)
+            return
+        nearest_id, nearest_d = None, None
+        for did, pos in drone_positions.items():
+            if did == enemy or not pos:
+                continue
+            d = _haversine(epos[0], epos[1], pos[0], pos[1])
+            if nearest_d is None or d < nearest_d:
+                nearest_id, nearest_d = did, d
+        if nearest_id is None:
+            self.distance_label.setText(f"敵機 {enemy} ↔ 最近友機：（無友機定位）")
+            self.distance_label.setStyleSheet(base)
+            return
+        color = "#E57373" if nearest_d <= self.r_detect else "#81C784"
+        self.distance_label.setText(
+            f"敵機 {enemy} ↔ 最近友機 {nearest_id}：{nearest_d:.1f} m")
+        self.distance_label.setStyleSheet(
+            f"color: {color}; font-size: 13px; font-weight: bold; padding: 4px 2px;")
+
+    # ----------------------------------------------------------------- 內部
+    def _on_enemy_changed(self, text):
+        if self._suppress_enemy:
+            return
+        self._enemy_id = "" if text == _NONE_LABEL else text
+        _log("INFO", f"[交戰] 敵機選擇：{self._enemy_id or '（未選擇）'}")
+        self.enemy_changed.emit(self._enemy_id)
+
+    def _on_group_changed(self, _text):
+        if self._suppress_group:
+            return
+        a_id, b_id = self.selected_group_ids()
+        self.groups_changed.emit(a_id or "", b_id or "")
+
+    def _refresh_enemy_speed_hint(self, *_):
+        v = self.friendly_speed_spin.value()
+        r = self.enemy_ratio_spin.value()
+        self.enemy_speed_hint.setText(f"→ 友機 {v:.1f} m/s、敵機 {v * r:.1f} m/s")
+
+    def _on_apply_speed(self):
+        v = self.friendly_speed_spin.value()
+        r = self.enemy_ratio_spin.value()
+        _log("INFO", f"[交戰] 套用速度：友機 {v:.1f} m/s、敵機 {v * r:.1f} m/s (比例 {r:.2f})")
+        self.apply_speed_requested.emit(v, r)

src/GUI/mission_planner.py

@@ -58,7 +58,7 @@ class FormationPlanner:
     """隊形規劃器"""
 
     def __init__(self, spacing: float = 5.0,
-                 base_altitude: float = 10.0,
+                 base_altitude: float = 30.0,
                  altitude_diff: float = 2.0):
         self.spacing = spacing
         self.base_altitude = base_altitude
@@ -179,7 +179,7 @@ class FormationPlanner:
         params = params or {}
         N = len(drone_positions)
         radius = params.get('radius', 10.0)
-        altitude = params.get('altitude', 10.0)
+        altitude = params.get('altitude', 30.0)
         start_angle = params.get('start_angle', 0.0)
 
         center_x, center_y, center_z = target_point
@@ -547,4 +547,4 @@ if __name__ == "__main__":
     )
     print(f"\nGrid Sweep:")
     for i, wp_list in enumerate(wps2):
-        print(f"  Drone {i}: {len(wp_list)} waypoints")
+        print(f"  Drone {i}: {len(wp_list)} waypoints")

src/GUI/overview_table.py

@@ -6,34 +6,38 @@ class OverviewTable(QTableWidget):
     """總覽表格，顯示所有無人機的狀態資訊"""
     
     # 默認的資訊類型和映射
-    DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "FixType", "衛星數", "EPH", "EPV",
+    # 「任務狀態」放最上面：閉環跑點時一眼看每台的 WP 進度 + 通訊(送出)狀態。
+    # 此列不來自遙測 QLabel，由 MissionExecutor 訊號經 update_table(field='mission') 餵入。
+    DEFAULT_INFO_TYPES = ["任務狀態",
+                          "模式", "ARM", "電壓", "經度", "緯度", "FixType", "衛星數", "EPH", "EPV",
                           "高度", "XY位置", "XY速度", "地速", "航向", "空速", "油門", "海拔高度",
                           "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
-    
+
     DEFAULT_INFO_TYPE_MAP = {
-        "mode": 0,
-        "armed": 1,
-        "battery": 2,
-        "longitude": 3,
-        "latitude": 4,
-        "fix_type": 5,
-        "satellites_visible": 6,
-        "eph": 7,
-        "epv": 8,
-        "altitude": 9,
-        "local": 10,
-        "velocity": 11,
-        "groundspeed": 12,
-        "heading": 13,
-        "airspeed": 14,
-        "throttle": 15,
-        "hud_alt": 16,
-        "climb": 17,
-        "roll": 18,
-        "pitch": 19,
-        "yaw": 20,
-        "loss_rate": 21,
-        "ping": 22
+        "mission": 0,
+        "mode": 1,
+        "armed": 2,
+        "battery": 3,
+        "longitude": 4,
+        "latitude": 5,
+        "fix_type": 6,
+        "satellites_visible": 7,
+        "eph": 8,
+        "epv": 9,
+        "altitude": 10,
+        "local": 11,
+        "velocity": 12,
+        "groundspeed": 13,
+        "heading": 14,
+        "airspeed": 15,
+        "throttle": 16,
+        "hud_alt": 17,
+        "climb": 18,
+        "roll": 19,
+        "pitch": 20,
+        "yaw": 21,
+        "loss_rate": 22,
+        "ping": 23
     }
     
     def __init__(self, info_types=None, info_type_map=None, parent=None):
@@ -43,6 +47,8 @@ class OverviewTable(QTableWidget):
         self.info_types = info_types if info_types is not None else self.DEFAULT_INFO_TYPES
         self.info_type_map = info_type_map if info_type_map is not None else self.DEFAULT_INFO_TYPE_MAP
         self.drones = {}  # 存儲無人機面板的引用
+        # 任務狀態不是遙測，不在 panel QLabel 內；獨立保存，避免 refresh_all 用遙測把它蓋掉
+        self.mission_status = {}  # drone_id -> 任務狀態文字
         
         # 初始化表格
         self.setColumnCount(1)
@@ -75,7 +81,11 @@ class OverviewTable(QTableWidget):
         if drone_id and field and value:
             if drone_id not in self.drones:
                 return
-                
+
+            # 任務狀態另存一份，讓 refresh_all（用遙測重繪）不會把它清掉
+            if field == 'mission':
+                self.mission_status[drone_id] = value
+
             col = 1 + list(self.drones.keys()).index(drone_id)
             row = self.info_type_map.get(field, -1)
 
@@ -104,8 +114,12 @@ class OverviewTable(QTableWidget):
         for col, did in enumerate(self.drones, start=1):
             panel = self.drones[did]
             for field, row in self.info_type_map.items():
-                lbl = panel.findChild(QLabel, f"{did}_{field}")
-                val = lbl.text() if lbl else "--"
+                if field == 'mission':
+                    # 任務狀態非遙測，取自獨立保存，避免被 "-" 蓋掉
+                    val = self.mission_status.get(did, "-")
+                else:
+                    lbl = panel.findChild(QLabel, f"{did}_{field}")
+                    val = lbl.text() if lbl else "-"
                 val_item = QTableWidgetItem(val)
                 val_item.setTextAlignment(Qt.AlignmentFlag.AlignCenter)
                 self.setItem(row, col, val_item)

src/GUI/websocket_client.py

@@ -0,0 +1,155 @@
+import asyncio
+import websockets
+import json
+import sys
+
+WS_URL = "ws://127.0.0.1:8765"
+SYSTEM_ID = 2
+GOTO_POSITION = [24.1234567, 120.1234567]
+
+
+class WebSocketClient:
+    def __init__(self, ws_url=WS_URL, system_id=SYSTEM_ID):
+        self.ws_url = ws_url
+        if isinstance(system_id, bool) or not isinstance(system_id, int):
+            raise ValueError("system_id 必須為整數")
+        self.system_id = system_id
+        
+    def process_websocket_message(self, data):
+        """解析 server 的 Essential／Full UAV 狀態或命令 ACK。"""
+        if isinstance(data, str):
+            try:
+                data = json.loads(data)
+            except json.JSONDecodeError as e:
+                print(f"JSON 格式錯誤: {e}")
+                return
+
+        if not isinstance(data, dict):
+            print(f"未知訊息格式: {data}")
+            return
+
+        if all(key in data for key in ("result", "command", "message")):
+            print(
+                f"ACK | Command:{data.get('command', '-')} | "
+                f"Result:{data.get('result', '-')} | "
+                f"Message:{data.get('message', '-')}"
+            )
+            return
+
+        if "s" not in data:
+            print(f"未知 JSON 訊息: {data}")
+            return
+
+        position = data.get("p")
+        if isinstance(position, (list, tuple)) and len(position) >= 2:
+            position_text = f"{position[0]}, {position[1]}"
+        else:
+            position_text = "-"
+
+        ypr = data.get("ypr")
+        if isinstance(ypr, (list, tuple)) and len(ypr) >= 3:
+            yaw, pitch, roll = ypr[0], ypr[1], ypr[2]
+        else:
+            yaw, pitch, roll = data.get("y"), None, None
+
+        dop = data.get("d")
+        if isinstance(dop, (list, tuple)) and len(dop) >= 2:
+            dop_text = f"{dop[0]}/{dop[1]}"
+        else:
+            dop_text = "-"
+
+        armed = data.get("a")
+        if armed is None:
+            armed_text = "-"
+        else:
+            armed_text = "ARMED" if bool(armed) else "DISARMED"
+
+        def value_or_dash(value, suffix=""):
+            return f"{value}{suffix}" if value is not None else "-"
+
+        print(
+            f"UAV | ID:{data.get('s', '-')} | "
+            f"Mode:{value_or_dash(data.get('m'))} | "
+            f"Arm:{armed_text} | "
+            f"Battery:{value_or_dash(data.get('b'), '%')} | "
+            f"Position:{position_text} | "
+            f"Height:{value_or_dash(data.get('h'), 'm')} | "
+            f"Velocity:{value_or_dash(data.get('v'), 'm/s')} | "
+            f"Yaw:{value_or_dash(yaw, 'deg')} | "
+            f"Pitch:{value_or_dash(pitch, 'deg')} | "
+            f"Roll:{value_or_dash(roll, 'deg')} | "
+            f"GPS:{value_or_dash(data.get('g'))} | DOP(H/V):{dop_text}"
+        )
+
+    async def send_commands_loop(self, websocket):
+        """從 stdin 讀取按鍵 (輸入後按 Enter) 並發送對應的 JSON 指令"""
+        loop = asyncio.get_event_loop()
+        print("輸入 1 = START, 2 = STOP, 3 = GOTO (然後按 Enter)")
+        while True:
+            # readline 會包含換行
+            try:
+                line = await loop.run_in_executor(None, sys.stdin.readline)
+            except Exception:
+                return
+            if not line:
+                # EOF
+                return
+            cmd = line.strip()
+            if cmd == '1':
+                msg = {"s": self.system_id, "c": "START"}
+            elif cmd == '2':
+                msg = {"s": self.system_id, "c": "STOP"}
+            elif cmd == '3':
+                msg = {
+                    "s": self.system_id,
+                    "c": "GOTO",
+                    "p": list(GOTO_POSITION),
+                }
+            else:
+                print(f"未知指令: {cmd}")
+                continue
+
+            try:
+                await websocket.send(json.dumps(msg))
+                print(f"已發送: {msg}")
+            except Exception as e:
+                print(f"發送失敗: {e}")
+                return
+
+    async def ws_client_loop(self):
+        retry_count = 0
+        max_retries = 5
+        base_delay = 1.0
+        while retry_count < max_retries:
+            try:
+                async with websockets.connect(self.ws_url) as websocket:
+                    print(f"WebSocket connected: {self.ws_url}")
+                    retry_count = 0
+                    # 啟動後台任務來接收 stdin 指令並發送
+                    send_task = asyncio.create_task(self.send_commands_loop(websocket))
+                    try:
+                        async for message in websocket:
+                            # message 已經是 string
+                            self.process_websocket_message(message)
+                    finally:
+                        send_task.cancel()
+                        await asyncio.gather(send_task, return_exceptions=True)
+                            
+            except websockets.exceptions.ConnectionClosedError:
+                print("WebSocket connection closed")
+                break
+            except Exception as e:
+                retry_count += 1
+                delay = base_delay * (2 ** min(retry_count, 4))
+                print(f"WebSocket connection error: {e}, retrying in {delay}s (attempt {retry_count}/{max_retries})")
+                await asyncio.sleep(delay)
+        
+        print("WebSocket client stopped after maximum retries")
+
+    async def start(self):
+        """啟動 WebSocket 客戶端"""
+        await self.ws_client_loop()
+
+if __name__ == "__main__":
+    client = WebSocketClient()
+    asyncio.run(client.start())

src/GUI/websocket_server.py

@@ -0,0 +1,179 @@
+import asyncio
+import websockets
+import json
+from urllib.parse import urlparse
+
+# 可切換為 "essential" 或 "full"；預設廣播完整狀態。
+STATUS_FORMAT = "full"
+SYSTEM_ID = 2
+
+# WebSocket 伺服器位址；修改此參數即可更換監聽 IP/port。
+WS_URL = "ws://0.0.0.0:8765"
+
+
+def parse_ws_url(ws_url):
+    """將 ws://host:port 解析為 websockets.serve 所需參數。"""
+    parsed = urlparse(ws_url)
+    if parsed.scheme != "ws":
+        raise ValueError("WS_URL 必須使用 ws:// 格式")
+    if not parsed.hostname:
+        raise ValueError("WS_URL 缺少主機位址")
+    try:
+        port = parsed.port
+    except ValueError as e:
+        raise ValueError(f"WS_URL port 格式錯誤: {e}") from e
+    if port is None:
+        raise ValueError("WS_URL 缺少 port")
+    return parsed.hostname, port
+
+# 啟動伺服器
+async def main(ws_url=WS_URL):
+    host, port = parse_ws_url(ws_url)
+    async with websockets.serve(client_handler, host, port):
+        print(f"WebSocket server started at {ws_url}")
+        await asyncio.Future()  # run forever
+
+# 模擬資料
+def get_uav_status(status_format=None):
+    status_format = status_format or STATUS_FORMAT
+    essential_status = {
+        "s": SYSTEM_ID,                 # system ID
+        "m": "GUIDED",                  # mode
+        "b": 78,                        # battery (%)
+        "p": [24.1199660, 120.677230],  # [lat, lon] (deg)
+        "y": 178.5,                     # yaw (deg)
+    }
+
+    full_status = {
+        "s": SYSTEM_ID,                 # system ID
+        "m": "GUIDED",                  # mode
+        "a": 1,                         # 1=armed, 0=disarmed
+        "b": 78,                        # battery (%)
+        "h": 12.3,                      # height (m)
+        "v": 4.2,                       # velocity (m/s)
+        "p": [24.1199660, 120.677230],  # [lat, lon] (deg)
+        "ypr": [178.5, -0.1, 0.0],      # yaw, pitch, roll (deg)
+        "g": 3,                         # GPS fix type
+        "d": [0.8, 1.2],                # HDOP, VDOP
+    }
+
+    if status_format == "essential":
+        return essential_status
+    if status_format == "full":
+        return full_status
+    raise ValueError(
+        f"不支援的 STATUS_FORMAT: {status_format}，請使用 essential 或 full")
+
+# 處理 client 傳來的訊息
+async def handle_client_messages(websocket):
+    try:
+        async for message in websocket:
+            try:
+                data = json.loads(message)
+                print(f"Received from client: {data}")
+                system_id = data.get("s")
+                cmd = data.get("c")
+                # XBee 命令格式:
+                # {"s":2,"c":"START"}
+                # {"s":2,"c":"STOP"}
+                # {"s":2,"c":"GOTO","p":[lat,lon]}
+                if system_id is None:
+                    ack = {
+                        "result": "ERROR",
+                        "command": cmd,
+                        "message": "缺少 system_id 欄位 s",
+                    }
+                    await websocket.send(json.dumps(ack, ensure_ascii=False))
+                    print(f"Sent ack: {ack}")
+                elif (isinstance(system_id, bool)
+                      or not isinstance(system_id, int)
+                      or system_id != SYSTEM_ID):
+                    ack = {
+                        "result": "ERROR",
+                        "command": cmd,
+                        "message": (
+                            f"指令目標 system_id={system_id} 不符，"
+                            f"本 UAV system_id={SYSTEM_ID}"
+                        ),
+                    }
+                    await websocket.send(json.dumps(ack, ensure_ascii=False))
+                    print(f"Sent ack: {ack}")
+                elif isinstance(cmd, str) and cmd in {"START", "STOP", "GOTO"}:
+                    if cmd == "START":
+                        print(f"收到 START 指令: system_id={system_id}")
+                        ack = {
+                            "result": "SUCCESS",
+                            "command": cmd,
+                            "message": "已接收 START 指令",
+                        }
+                    elif cmd == "STOP":
+                        print(f"收到 STOP 指令: system_id={system_id}")
+                        ack = {
+                            "result": "SUCCESS",
+                            "command": cmd,
+                            "message": "已接收 STOP 指令",
+                        }
+                    elif cmd == "GOTO":
+                        position = data.get("p")
+                        if isinstance(position, (list, tuple)) and len(position) >= 2:
+                            lat, lon = position[0], position[1]
+                            print(
+                                f"收到 GOTO 指令: system_id={system_id}, "
+                                f"lat={lat}, lon={lon}"
+                            )
+                            ack = {
+                                "result": "SUCCESS",
+                                "command": cmd,
+                                "message": "已接收 GOTO 指令",
+                            }
+                        else:
+                            print("GOTO 指令缺少位置 p=[lat,lon]")
+                            ack = {
+                                "result": "ERROR",
+                                "command": cmd,
+                                "message": "GOTO 指令缺少位置 p=[lat,lon]",
+                            }
+
+                    await websocket.send(json.dumps(ack, ensure_ascii=False))
+                    print(f"Sent ack: {ack}")
+                else:
+                    ack = {
+                        "result": "ERROR",
+                        "command": cmd,
+                        "message": "指令 c 必須為大寫 START、STOP 或 GOTO",
+                    }
+                    await websocket.send(json.dumps(ack, ensure_ascii=False))
+                    print(f"Sent ack: {ack}")
+            except json.JSONDecodeError:
+                print("❌ JSON 格式錯誤")
+    except websockets.ConnectionClosed:
+        print("❌ Client connection closed while receiving")
+
+# 持續廣播 UAV 資訊
+async def broadcast_uav_status(websocket):
+    try:
+        while True:
+            msg = get_uav_status()
+            await websocket.send(json.dumps(msg))
+            await asyncio.sleep(1)  # 每秒發送一次
+    except websockets.ConnectionClosed:
+        print("❌ Client connection closed while sending")
+
+# 每個 client 的處理流程
+async def client_handler(websocket, path=None):
+    print(f"🔗 Client connected: {websocket.remote_address}")
+    receive_task = asyncio.create_task(handle_client_messages(websocket))
+    send_task = asyncio.create_task(broadcast_uav_status(websocket))
+
+    done, pending = await asyncio.wait(
+        [receive_task, send_task],
+        return_when=asyncio.FIRST_COMPLETED,
+    )
+
+    for task in pending:
+        task.cancel()
+
+    print(f"🔌 Client disconnected: {websocket.remote_address}")
+
+if __name__ == "__main__":
+    asyncio.run(main(WS_URL))

src/GUI/xbee_gcs.py

@@ -0,0 +1,127 @@
+import serial
+import json
+import threading
+import time
+from pynput import keyboard
+
+SYSTEM_ID = 2
+
+try:
+    ser = serial.Serial('/dev/ttyUSB1', 115200, timeout=0.1) 
+    print("XBee 地面站啟動")
+    print("[1] START  [2] STOP  [3] GOTO  [Esc] 退出程式")
+except Exception as e:
+    print(f"無法開啟序列埠: {e}")
+    exit()
+
+GPS_TYPE_LABELS = {
+    0: "no GPS",
+    1: "no fix",
+    2: "2D",
+    3: "3D",
+    4: "DGPS",
+    5: "RTK float",
+    6: "RTK fixed",
+}
+
+def format_uav_data(uav_data):
+    system_id = uav_data.get("s", "?")
+    mode = uav_data.get("m", "?")
+    battery = uav_data.get("b", "?")
+    position = uav_data.get("p", "?")
+
+    parts = [
+        f"[UAV] ID:{system_id}",
+        f"Mode:{mode}",
+        f"Bat:{battery}%",
+        f"Pos:{position}",
+    ]
+
+    if "a" in uav_data:
+        armed_text = "ARMED" if int(uav_data.get("a", 0)) == 1 else "DISARMED"
+        parts.append(f"Arm:{armed_text}")
+
+    ypr = uav_data.get("ypr")
+    if isinstance(ypr, list) and len(ypr) >= 3:
+        parts.append(f"Height:{uav_data.get('h', '?')}m")
+        parts.append(f"Yaw:{ypr[0]}deg")
+        parts.append(f"Pitch:{ypr[1]}deg")
+        parts.append(f"Roll:{ypr[2]}deg")
+    elif "y" in uav_data:
+        parts.append(f"Height:{uav_data.get('h', '?')}m")
+        parts.append(f"Yaw:{uav_data.get('y')}deg")
+    elif "h" in uav_data:
+        parts.append(f"Heading:{uav_data.get('h')}")
+
+    if "v" in uav_data:
+        parts.append(f"Vel:{uav_data.get('v')}m/s")
+
+    if "g" in uav_data:
+        gps_type = uav_data.get("g")
+        parts.append(f"GPS:{gps_type}({GPS_TYPE_LABELS.get(gps_type, 'unknown')})")
+
+    dop = uav_data.get("d")
+    if isinstance(dop, list) and len(dop) >= 2:
+        parts.append(f"DOP(H/V):{dop[0]}/{dop[1]}")
+
+    return " | ".join(parts)
+
+def receive_thread():
+    while True:
+        try:
+            if ser.in_waiting > 0:
+                line = ser.readline().decode('utf-8', errors='ignore').strip()
+                if line:
+                    try:
+                        data = json.loads(line)
+                        # 判斷是確認訊息（ack）還是 UAV 狀態
+                        if "result" in data and "command" in data and "message" in data:
+                            # 這是確認訊息
+                            result = data.get("result")
+                            cmd = data.get("command")
+                            msg = data.get("message")
+                            print(f"\n[確認] 命令:{cmd} 狀態:{result} 訊息:{msg}", flush=True)
+                        else:
+                            # 這是 UAV 狀態訊息
+                            print(format_uav_data(data), flush=True)
+                    except json.JSONDecodeError:
+                        pass 
+        except Exception as e:
+            print(f"\n[接收錯誤] {e}")
+        time.sleep(0.01)
+
+t = threading.Thread(target=receive_thread, daemon=True)
+t.start()
+
+def send_command(command, position=None):
+    try:
+        if command not in {"START", "STOP", "GOTO"}:
+            raise ValueError("指令必須為大寫 START、STOP 或 GOTO")
+        cmd = {
+            "s": SYSTEM_ID,
+            "c": command
+        }
+        if position is not None:
+            cmd["p"] = position
+
+        payload = json.dumps(cmd, separators=(',', ':')) + "\n"
+        ser.write(payload.encode('utf-8'))
+        print(f"\n>> [指令已發送] {payload.strip()}")
+    except Exception as e:
+        print(f"\n發送失敗: {e}")
+
+def on_press(key):
+    try:
+        if key.char == "1":
+            send_command("START")
+        elif key.char == "2":
+            send_command("STOP")
+        elif key.char == "3":
+            send_command("GOTO", [24.1199660, 120.677230])
+    except AttributeError:
+        if key == keyboard.Key.esc:
+            print("\n程式結束")
+            return False
+
+with keyboard.Listener(on_press=on_press) as listener:
+    listener.join()

src/GUI/xbee_uav.py

@@ -0,0 +1,141 @@
+import serial
+import json
+import math
+import time
+
+PORT = '/dev/ttyUSB0'  # 替換為實際的序列埠名稱
+BAUD_RATE = 115200
+SYSTEM_ID = 2
+
+# 可切換為 "essential" 或 "full"；預設發送完整狀態。
+STATUS_FORMAT = "full"
+
+try:
+    ser = serial.Serial(PORT, BAUD_RATE, timeout=0.1)
+    print("XBee 無人機啟動...")
+except Exception as e:
+    print(f"無法開啟序列埠: {e}")
+    exit()
+
+def get_uav_status(status_format=None):
+    status_format = status_format or STATUS_FORMAT
+    essential_status = {
+        "s": SYSTEM_ID,                 # system ID
+        "m": "GUIDED",                  # mode
+        "b": 78,                        # battery (%)
+        "p": [24.1199660, 120.677230],  # [lat, lon] (deg)
+        "y": 178.5,                     # yaw (deg)
+    }
+    full_status = {
+        "s": SYSTEM_ID,                 # system ID
+        "m": "GUIDED",                  # mode
+        "a": 1,                         # 1=armed, 0=disarmed
+        "b": 78,                        # battery (%)
+        "h": 12.3,                      # height (m)
+        "v": 4.2,                       # velocity (m/s)
+        "p": [24.1199660, 120.677230],  # position: [lat, lon] (deg)
+        "ypr": [178.5, -0.1, 0.0],      # yaw, pitch, roll (deg)
+        "g": 3,                         # GPS type {0: no GPS, 1: no fix, 2: 2D, 3: 3D, 4: DGPS, 5: RTK float, 6: RTK fixed}
+        "d": [0.8, 1.2],                # DOP: [hdop, vdop]
+    }
+    if status_format == "essential":
+        return essential_status
+    if status_format == "full":
+        return full_status
+    raise ValueError(
+        f"不支援的 STATUS_FORMAT: {status_format}，請使用 essential 或 full")
+
+def handle_command(cmd_obj):
+    # 統一指令格式: {"s": <int>, "c": "START|STOP|GOTO"}
+    target_s = cmd_obj.get("s")
+    command = cmd_obj.get("c")
+    ack = None
+
+    if (isinstance(target_s, bool)
+            or not isinstance(target_s, int)
+            or target_s != SYSTEM_ID):
+        ack = {
+            "result": "ERROR",
+            "command": command,
+            "message": (
+                f"指令目標 system_id={target_s} 不符，"
+                f"本 UAV system_id={SYSTEM_ID}"
+            ),
+        }
+    elif not isinstance(command, str) or command not in {"START", "STOP", "GOTO"}:
+        print(f"\n[接收指令] 未知或格式錯誤的指令: {command}")
+        ack = {
+            "result": "ERROR",
+            "command": command,
+            "message": "指令 c 必須為大寫 START、STOP 或 GOTO",
+        }
+    elif command == "START":
+        print("\n[接收指令] 開始任務")
+        ack = {"result": "SUCCESS", "command": "START", "message": "成功"}
+    elif command == "STOP":
+        print("\n[接收指令] 停止任務")
+        ack = {"result": "SUCCESS", "command": "STOP", "message": "成功"}
+    elif command == "GOTO":
+        # 優先使用 XBee 格式 p=[lat, lon]，並兼容舊版 lat/lon 欄位。
+        position = cmd_obj.get("p")
+        if isinstance(position, (list, tuple)) and len(position) >= 2:
+            lat, lon = position[0], position[1]
+        else:
+            lat, lon = cmd_obj.get("lat"), cmd_obj.get("lon")
+
+        try:
+            if isinstance(lat, bool) or isinstance(lon, bool):
+                raise ValueError
+            lat = float(lat)
+            lon = float(lon)
+            if not math.isfinite(lat) or not math.isfinite(lon):
+                raise ValueError
+            if not -90.0 <= lat <= 90.0 or not -180.0 <= lon <= 180.0:
+                raise ValueError
+        except (TypeError, ValueError):
+            print("\n[接收指令] GOTO 目標無效，需要有效的 p=[lat,lon]")
+            ack = {
+                "result": "ERROR",
+                "command": "GOTO",
+                "message": "GOTO 指令缺少或包含無效的 p=[lat,lon]",
+            }
+        else:
+            target = [lat, lon]
+            print(f"\n[接收指令] 導航任務 目標位置: {target}")
+            ack = {
+                "result": "SUCCESS",
+                "command": "GOTO",
+                "p": target,
+                "message": "成功",
+            }
+    # 發送回覆（ack）到 GCS via serial
+    if ack is not None:
+        try:
+            payload = json.dumps(ack, ensure_ascii=False) + "\n"
+            ser.write(payload.encode('utf-8'))
+            print(f"已發送回覆: {payload.strip()}")
+        except Exception as e:
+            print(f"發送回覆失敗: {e}")
+
+try:
+    while True:
+        data = get_uav_status()
+        json_payload = json.dumps(data, separators=(',', ':')) + "\n"
+        ser.write(json_payload.encode('utf-8'))
+        print(f"發送狀態: {json_payload.strip()}")
+        for _ in range(10):
+            if ser.in_waiting > 0:
+                try:
+                    cmd_line = ser.readline().decode('utf-8').strip()
+                    if cmd_line:
+                        cmd_obj = json.loads(cmd_line)
+                        handle_command(cmd_obj)
+
+                except json.JSONDecodeError:
+                    pass 
+            time.sleep(0.1)
+
+except KeyboardInterrupt:
+    print("發送停止")
+finally:
+    ser.close()

src/fc_interfaces/CMakeLists.txt

@@ -14,6 +14,8 @@ find_package(builtin_interfaces REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/AttitudeRaw.msg"
   "msg/GnssRaw.msg"
+  "msg/SystemDiagnosticsRaw.msg"
+  "msg/FcNetworkLog.msg"
   "msg/ServiceAckResult.msg"
   "srv/MavPing.srv"
   "srv/MavCommandLong.srv"

src/fc_interfaces/msg/FcNetworkLog.msg

@@ -0,0 +1,12 @@
+uint8 DEBUG=10
+uint8 INFO=20
+uint8 WARN=30
+uint8 ERROR=40
+uint8 FATAL=50
+
+builtin_interfaces/Time stamp
+uint8 level
+string source
+string event_code
+int32 sysid
+string message

src/fc_interfaces/msg/SystemDiagnosticsRaw.msg

@@ -0,0 +1,11 @@
+builtin_interfaces/Time stamp
+uint32 sensors_install_mask
+uint32 sensors_enabled_mask
+uint32 sensors_health_mask
+uint16 mcu_load
+uint16 bus_error_rate
+uint16 bus_error_count
+uint16 errors_count1
+uint16 errors_count2
+uint16 errors_count3
+uint16 errors_count4

src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py

@@ -29,7 +29,7 @@ from .utils import acquireSerial, acquirePort
 from .utils.acquirePort import find_available_port
 
 logger = setup_logger(os.path.basename(__file__))
-PROJECT_VER = "v1.10"
+PROJECT_VER = "v1.20"
 
 class PanelState:
     def __init__(self):
@@ -435,6 +435,10 @@ class ControlPanel:
                         state.serial_info_temp["CommunicationType"] = "XBee(API-AT)"
                         menu_stack.pop()
                         idx_stack.pop()
+                    elif selected.action == "SET_SERIAL_COMM_XBEE_ESP":
+                        state.serial_info_temp["CommunicationType"] = "XBee(API-API) espv1"
+                        menu_stack.pop()
+                        idx_stack.pop()
                     elif selected.action == "SET_SERIAL_COMM_TELEMETRY":
                         state.serial_info_temp["CommunicationType"] = "TELEMETRY"
                         menu_stack.pop()
@@ -812,6 +816,7 @@ class ControlPanel:
                 port_menu = MenuNode(f"{port}", children=[
                     MenuNode("Set Comm Type", "設定通訊形態", "SET_SERIAL_COMM", children=[
                         MenuNode("XBee(API-AT)", "XBee 模式", "SET_SERIAL_COMM_XBEE"),
+                        MenuNode("XBee(API-API)", "XBee 模式(with ESP)", "SET_SERIAL_COMM_XBEE_ESP"),
                         MenuNode("Telemetry", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"),
                     ]),
                     MenuNode("Set Baud", "設定 Baud", "TEXT_BAUD_SERIAL"),
@@ -1106,7 +1111,7 @@ class ControlPanel:
                 0: "HB", 1: "S_STAT", 2: "S_TIME", 24: "GPS_RAW", 27: "RAW_IMU",
                 30: "ATT", 32: "LOC_POS", 33: "GLB_POS", 62: "NAV_CTL",
                 74: "VFR_HUD", 147: "BATT_ST", 136: "TERRAIN", 241: "VIBRAT",
-                125: "POW_STA",
+                125: "POW_STA", 253: "STAT_TXT",
             }
 
             # ardupilot mega 
@@ -1198,6 +1203,8 @@ class Orchestrator:
             'vfr_hud': 1.0,
             'mode': 0.0,
             'summary': 1.0,
+            'sys_diags': 1.0,
+            'status_text': 1.0,
         }
 
     def engageWholeSystem(self):
@@ -1593,6 +1600,7 @@ class Orchestrator:
         # 定義通訊類型映射表
         COMM_TYPE_MAP = {
             "XBee(API-AT)": sm.SerialMode.XBEEAPI2AT,
+            "XBee(API-AT)": sm.SerialMode.XBEEAPI_espv1,
             "TELEMETRY": sm.SerialMode.STRAIGHT,
             # 新增區
         }
@@ -1636,25 +1644,7 @@ class Orchestrator:
 
 def main():
 
-    # =========== 各項模組的版本先驗 ===========
-    # 除非你有在做這幾項模組的改版 不然動到這邊的版本號 代表執行環境有很大的問題!!!!!!
-    version_check = True
-    if mo.MODULE_VER != "1.50":
-        print("Module Version Error! : mavlinkObkect")
-        version_check = False
-    if mros.MODULE_VER != "2.10":
-        print("Module Version Error! : mavlinkROS2Nodes")
-        version_check = False
-    if mvv.MODULE_VER != "1.00":
-        print("Module Version Error! : mavlinkVehicleView")
-        version_check = False
-    if sm.MODULE_VER != "0.80":
-        print("Module Version Error! : serialManager")
-        version_check = False
-    if version_check == False:
-        print("Environment Obstacle! Check YOUR Execution System Path First!!")
-        return
-    # ========================================
+    logger.info(f"Each Module Running Version at mavlinkObkect:{mo.MODULE_VER}, mavlinkROS2Nodes:{mros.MODULE_VER}, mavlinkVehicleView:{mvv.MODULE_VER}, serialManager:{sm.MODULE_VER}")
 
     stop_evt = threading.Event()
     def signal_handler(signum, frame):

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -47,11 +47,11 @@ from pymavlink.dialects.v20 import ardupilotmega as mav_ardupilot
 
 # 自定義的 import
 from .mavlinkVehicleView import (
-    vehicle_registry,
-    VehicleView,
+    vehicle_registry, # 儲存全部物件的地方
+    VehicleView, # 代表每台載具的最外層
     VehicleComponent,
     ComponentType,
-    ConnectionType
+    StatusTextEntry,
 )
 from .utils import RingBuffer, setup_logger
 
@@ -108,13 +108,15 @@ class mavlink_bridge:
     def _init_message_handlers(self):
         """初始化訊息處理器映射表，提高處理效率"""
         self.message_handlers = {
-            0: self._handle_heartbeat,        # HEARTBEAT
-            24: self._handle_gps_raw_int,     # GPS_RAW_INT
-            30: self._handle_attitude,        # ATTITUDE
-            32: self._handle_local_position,  # LOCAL_POSITION_NED
-            33: self._handle_global_position, # GLOBAL_POSITION_INT
-            74: self._handle_vfr_hud,         # VFR_HUD
-            147: self._handle_battery_status, # BATTERY_STATUS
+            0: self._handle_heartbeat,          # HEARTBEAT
+            1: self._handle_vehicle_sys_status, # SYS_STATUS
+            24: self._handle_gps_raw_int,       # GPS_RAW_INT
+            30: self._handle_attitude,          # ATTITUDE
+            32: self._handle_local_position,    # LOCAL_POSITION_NED
+            33: self._handle_global_position,   # GLOBAL_POSITION_INT
+            74: self._handle_vfr_hud,           # VFR_HUD
+            147: self._handle_battery_status,   # BATTERY_STATUS
+            253: self._handle_status_text,      # STATUSTEXT
         }
 
     def start(self):
@@ -231,6 +233,29 @@ class mavlink_bridge:
             'vx': msg.vx, 'vy': msg.vy, 'vz': msg.vz
         }
 
+    def _handle_vehicle_sys_status(self, vehicle, component, msg, timestamp):
+        """處理 SYS_STATUS 訊息 (msg_id: 1)"""
+        diag = component.status.sys_diag
+        diag.sensors_install_mask = msg.onboard_control_sensors_present
+        diag.sensors_enabled_mask = msg.onboard_control_sensors_enabled
+        diag.sensors_health_mask = msg.onboard_control_sensors_health
+        diag.mcuLoad = msg.load
+        diag.busErrorRate = msg.drop_rate_comm
+        diag.busErrorCount = msg.errors_comm
+        diag.errors_count1 = msg.errors_count1
+        diag.errors_count2 = msg.errors_count2
+        diag.errors_count3 = msg.errors_count3
+        diag.errors_count4 = msg.errors_count4
+        diag.timestamp = timestamp
+
+    def _handle_status_text(self, vehicle, component, msg, timestamp):
+        """處理 STATUSTEXT 訊息 (msg_id: 253)"""
+        text = msg.text.rstrip('\x00') if msg.text else ''
+        if not text:
+            return
+        component.status.status_text_queue.append(
+            StatusTextEntry(text=text, severity=msg.severity, timestamp=timestamp)
+        )
 
     def _handle_global_position(self, vehicle, component, msg, timestamp):
         """處理 GLOBAL_POSITION_INT 訊息 (msg_id: 33)"""
@@ -395,9 +420,9 @@ class mavlink_object:
             self.MAVLink = mav_common.MAVLink(self.mavlinkPipeline, srcSystem=254, srcComponent=191)
         
         # 記錄訊息過濾類型 (可選)
-        # 0 HEARTBEAT, 24 GPS_RAW_INT, 30 ATTITUDE, 
-        # 32 LOCAL_POSITION_NED, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS
-        self.bridge_msg_types = set([0, 24, 30, 32, 33, 74, 147])  
+        # 0 HEARTBEAT, 1 SYS_STATUS, 24 GPS_RAW_INT, 30 ATTITUDE, 
+        # 32 LOCAL_POSITION_NED, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS, 253 STATUSTEXT
+        self.bridge_msg_types = set([0, 1, 24, 30, 32, 33, 74, 147, 253])  
         self.return_msg_types = set([])
 
         # 轉發到別的 mavlink object 作為目標端口 的列表
@@ -834,5 +859,8 @@ if __name__ == '__main__':
 1. async_io_manager.managed_objects 與 mavlink_object.mavlinkObjects 功能重複整合 保留 mavlink_object.mavlinkObjects
 2. async_io_manager 的 _stop_event 無效變數移除
 
+2026年 06月 10日
+1. 增加 SYS_STATUS 與 STATUSTEXT 訊息的處理機制
+
 '''
 

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -1,6 +1,6 @@
 """
 MAVLink ROS2 Nodes
-主要包含三個獨立的 ROS2 Node :
+主要包含四個獨立的 ROS2 Node :
 1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics
     從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
 2. MavlinkCommandService - 提供 MAVLink 指令 service 介面
@@ -8,6 +8,7 @@ MAVLink ROS2 Nodes
     並不會包含額外的功能
 3. RtcmRelay - 訂閱 RTCM topic 並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
     過期丟棄、去重、節流、分片
+4. FcNetworkLogPublisher - 將 FC Network logging queue 發布到 /fc_network/logs
 
 與一個節點管理器
 - fc_ros_manager
@@ -18,6 +19,7 @@ import os
 import time
 import math
 import hashlib
+import queue
 import threading
 from typing import Dict, Optional
 
@@ -42,10 +44,10 @@ from fc_interfaces.msg import ServiceAckResult
 # 自定義 imports
 from . import mavlinkVehicleView as mvv
 from . import mavlinkObject as mo
-from .utils import setup_logger
+from .utils import get_ros_log_queue, setup_logger
 
 logger = setup_logger(os.path.basename(__file__))
-MODULE_VER = "2.10"
+MODULE_VER = "2.50"
 
 FC_ROS_DOMAIN_ID = "0"
 NODE_KEYS = ("status_publisher", "command_service", "rtcm_relay")
@@ -68,14 +70,16 @@ class PublishRateController:
         # 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉
         # 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段
         self.topic_intervals = {
-            'position_gnss': 0.0,  # GNSS位置
-            'position_ned': 0.0,   # LOCAL_POSITION_NED (位置+速度)
-            'attitude': 0.0,       # 姿態 (pitch yaw row 與其加速狀態)
-            'velocity': 0.0,       # 速度 (已經包含在 vfr_hud 未來移除)
-            'battery': 0.0,        # 電池
-            'vfr_hud': 0.0,        # VFR HUD (地速 空速 絕對高度 爬升率 航向 油門)
-            'mode': 0.0,           # 飛行模式 (已經在 summary 裡 未來移除)
-            'summary': 0.0,        # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態)
+            'summary': 0.0,         # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態)
+            'position_gnss': 0.0,   # GNSS位置 (海拔高度)
+            'position_ned': 0.0,    # LOCAL_POSITION_NED (位置+速度+相對高度)
+            'attitude': 0.0,        # 姿態 (pitch yaw row 與其加速狀態)
+            'battery': 0.0,         # 電池
+            'vfr_hud': 0.0,         # VFR HUD (地速 空速 絕對高度 爬升率 航向 油門)
+            'sys_diags': 0.0,       # SYS_STATUS 系統診斷
+            'status_text': 0.0,     # STATUSTEXT 飛控文字（佇列驅動，>0 僅作啟用旗標）
+            'mode': 0.0,            # 飛行模式 (已經在 summary 裡 未來移除)
+            'velocity': 0.0,        # 速度 (已經包含在 vfr_hud 未來移除)
             # 在這裡新增更多 topics...
         }
         # 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp}
@@ -112,6 +116,10 @@ class PublishRateController:
         
         return False
     
+    def is_topic_enabled(self, topic: str) -> bool:
+        """檢查 topic 是否啟用（interval > 0）"""
+        return self.topic_intervals.get(topic, 0) > 0
+    
     def reset(self):
         """重置所有計時器"""
         self.last_publish_time.clear()
@@ -122,7 +130,7 @@ class VehicleStatusPublisher(Node):
     
     職責:
     - 定期從 vehicle_registry 讀取載具狀態
-    - 頻率控制（位置/姿態 2Hz，電池/摘要 1Hz）
+    - 頻率控制 (位置/姿態 2Hz, 電池/摘要 1Hz)
     - 發布標準 ROS2 消息類型
     - 檢測訂閱者，按需發布
     """
@@ -181,11 +189,13 @@ class VehicleStatusPublisher(Node):
         self._publish_position_gnss(sysid, status)
         self._publish_position_ned(sysid, status)
         self._publish_attitude(sysid, status)
-        self._publish_velocity(sysid, status)
         self._publish_battery(sysid, status)
         self._publish_vfr_hud(sysid, status)
-        self._publish_mode(sysid, status)
         self._publish_summary(vehicle)
+        self._publish_system_diagnostics(sysid, status)
+        self._publish_status_text(sysid, status)
+        self._publish_velocity(sysid, status)
+        self._publish_mode(sysid, status)
         # 在這裡新增更多 publish 方法調用...
     
     def _get_or_create_publisher(self, sysid: int, topic: str, msg_type, qos: int = 1):
@@ -206,7 +216,7 @@ class VehicleStatusPublisher(Node):
             topic_name = f'{self.topicString_prefix}/sys{sysid}/{topic}'
             publisher = self.create_publisher(msg_type, topic_name, qos)
             self.fc_publishers[key] = publisher
-            logger.info(f"Created publisher: {topic_name}")
+            logger.debug(f"Created publisher: {topic_name}")
         return self.fc_publishers[key]
     
     def _publish_position_gnss(self, sysid: int, status: mvv.ComponentStatus):
@@ -230,9 +240,7 @@ class VehicleStatusPublisher(Node):
         msg.stamp = self.get_clock().now().to_msg()
         msg.latitude = float(pos.latitude)
         msg.longitude = float(pos.longitude)
-        # 發布「相對 home 高度」而非 AMSL：下游 GUI / mission planner 一律以相對高度做運算與指令，
-        # 若這裡發 AMSL（msg.alt）會在實飛時把海拔當成相對高度傳給飛控，造成 Circle 高度衝高的 bug
-        msg.altitude = float(pos.relative_altitude) if pos.relative_altitude is not None else 0.0
+        msg.altitude = float(pos.altitude) if pos.altitude is not None else 0.0
         
         # GPS 狀態資訊
         gps = status.gps
@@ -441,11 +449,7 @@ class VehicleStatusPublisher(Node):
             'armed': status.armed if status.armed is not None else False,
             # 'mode_custom': status.mode.custom_mode if status.mode.custom_mode else 0,
             'mode_name': status.mode.mode_name if status.mode.mode_name else "UNKNOWN",
-            # 'latitude': status.position.latitude if status.position.latitude else 0.0,
-            # 'longitude': status.position.longitude if status.position.longitude else 0.0,
-            # 'altitude': status.position.altitude if status.position.altitude else 0.0,
-            # 'battery_percent': status.battery.remaining if status.battery.remaining else 0,
-            # 'gps_fix': status.gps.fix_type if status.gps.fix_type else 0,
+            'mav_status': status.system_status if status.system_status is not None else 0,
             'connection_type': vehicle.connected_via.value,
             'last_update': component.packet_stats.last_msg_time if component.packet_stats.last_msg_time else 0.0,
         }
@@ -454,6 +458,62 @@ class VehicleStatusPublisher(Node):
         msg.data = json.dumps(summary)
         publisher.publish(msg)
     
+    def _publish_system_diagnostics(self, sysid: int, status: mvv.ComponentStatus):
+        """發布 SYS_STATUS 系統診斷資訊"""
+        if not self.rate_controller.should_publish(sysid, 'sys_diags'):
+            return
+        
+        diag = status.sys_diag
+        if diag.sensors_install_mask is None:
+            return
+        
+        publisher = self._get_or_create_publisher(
+            sysid, 'sys_diags', fcmsg.SystemDiagnosticsRaw
+        )
+        
+        if publisher.get_subscription_count() == 0:
+            return
+        
+        msg = fcmsg.SystemDiagnosticsRaw()
+        msg.stamp = self.get_clock().now().to_msg()
+        msg.sensors_install_mask = int(diag.sensors_install_mask)
+        msg.sensors_enabled_mask = int(diag.sensors_enabled_mask or 0)
+        msg.sensors_health_mask = int(diag.sensors_health_mask or 0)
+        msg.mcu_load = int(diag.mcuLoad or 0)
+        msg.bus_error_rate = int(diag.busErrorRate or 0)
+        msg.bus_error_count = int(diag.busErrorCount or 0)
+        msg.errors_count1 = int(diag.errors_count1 or 0)
+        msg.errors_count2 = int(diag.errors_count2 or 0)
+        msg.errors_count3 = int(diag.errors_count3 or 0)
+        msg.errors_count4 = int(diag.errors_count4 or 0)
+        
+        publisher.publish(msg)
+    
+    def _publish_status_text(self, sysid: int, status: mvv.ComponentStatus):
+        """發布 STATUSTEXT 飛控文字 (佇列 drain, 無訂閱者直接丟棄) """
+        # 是否啟用
+        if not self.rate_controller.is_topic_enabled('status_text'):
+            return
+        # 是否有資料
+        queue = status.status_text_queue
+        if not queue:
+            return
+
+        publisher = self._get_or_create_publisher(sysid, 'status_text', std_msgs.msg.String)
+
+        # 是否有監聽者
+        if publisher.get_subscription_count() == 0:
+            queue.clear()
+            return
+
+        while queue:
+            entry = queue.popleft()
+            msg = std_msgs.msg.String()
+            ts = entry.timestamp if entry.timestamp is not None else 0.0
+            sev = entry.severity if entry.severity is not None else -1
+            msg.data = f'[{ts:.3f}] [{sev}] {entry.text}'
+            publisher.publish(msg)
+    
     # ═══════════════════════════════════════════════════════════════
     # 【新增 Topic 位置 3/4】
     # 若要新增 topic，請在此處實作對應的發布方法
@@ -472,29 +532,6 @@ class VehicleStatusPublisher(Node):
     #     # ... 實作發布邏輯
     # ═══════════════════════════════════════════════════════════════
     
-    @staticmethod
-    def _euler_to_quaternion(roll, pitch, yaw):
-        """
-        歐拉角轉四元數
-        
-        Args:
-            roll: 橫滾角 (弧度)
-            pitch: 俯仰角 (弧度)
-            yaw: 偏航角 (弧度)
-            
-        Returns:
-            tuple: (qx, qy, qz, qw)
-        """
-        qx = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - \
-             math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
-        qy = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + \
-             math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2)
-        qz = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - \
-             math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2)
-        qw = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + \
-             math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
-        return (qx, qy, qz, qw)
-    
     def stop(self):
         """停止發布"""
         self.running = False
@@ -532,6 +569,14 @@ class MavlinkCommandService(Node):
     講白話一點就是
     每次接到一個 service 請求 要整個系統丟某種指令給載具時
     會做兩件事 1."丟出mavlink封包" 2."創造一個臨時信箱 Pending"
+    
+    然後透過每次 manager spin 
+    會去呼叫 return_router() 方法
+    這個方法會監聽 return_packet_ring 跟臨時信箱的 Pending 做配對
+    配對到的解開 Pending 
+
+    解開後 相對應的 handle_XXX 就會開始做事
+    
 
     """
 
@@ -1136,6 +1181,58 @@ class RtcmRelay(Node):
         # logger.info("RtcmRelay stopped")
 
 
+# ============================================================================
+#                    FC Network Log Publisher Node
+# ============================================================================
+
+class FcNetworkLogPublisher(Node):
+    """從 Python logging queue 發布結構化 /fc_network/logs 訊息。"""
+
+    def __init__(self):
+        super().__init__('fc_network_log_publisher')
+        qos = QoSProfile(
+            history=HistoryPolicy.KEEP_LAST,
+            depth=200,
+            reliability=ReliabilityPolicy.RELIABLE,
+            durability=DurabilityPolicy.VOLATILE,
+        )
+        self.publisher = self.create_publisher(
+            fcmsg.FcNetworkLog, '/fc_network/logs', qos)
+        self.log_queue = get_ros_log_queue()
+        self.running = True
+        self.timer = self.create_timer(0.05, self._drain_queue)
+
+    def _drain_queue(self):
+        if not self.running:
+            return
+
+        for _ in range(100):
+            try:
+                item = self.log_queue.get_nowait()
+            except queue.Empty:
+                break
+
+            created = max(0.0, float(item.get('created', 0.0)))
+            seconds = int(created)
+            nanoseconds = int((created - seconds) * 1_000_000_000)
+
+            msg = fcmsg.FcNetworkLog()
+            msg.stamp.sec = seconds
+            msg.stamp.nanosec = min(max(nanoseconds, 0), 999_999_999)
+            msg.level = min(max(int(item.get('level', 20)), 0), 255)
+            msg.source = str(item.get('source', 'fc_network'))
+            msg.event_code = str(item.get('event_code', ''))
+            msg.sysid = min(
+                max(int(item.get('sysid', -1)), -2_147_483_648),
+                2_147_483_647
+            )
+            msg.message = str(item.get('message', ''))
+            self.publisher.publish(msg)
+
+    def stop(self):
+        self.running = False
+
+
 # ============================================================================
 #                         ROS2 節點管理器
 # ============================================================================
@@ -1149,12 +1246,17 @@ class fc_ros_manager:
     stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。
     shutdown 是完全關閉 ROS2 並銷毀節點實例。
 
-    管理三個獨立的 ROS2 Node :
+    管理四個獨立的 ROS2 Node :
     - VehicleStatusPublisher
     - MavlinkCommandService
     - RtcmRelay
+    - FcNetworkLogPublisher
     
     提供統一的啟動/停止介面給 mainOrchestrator
+
+    另外 這邊用到 MultiThreadedExecutor 會開出額外的 thread 的特性 
+    使得就算 executor 在跑一些需要等待的方法 
+    常態的 spin_once 也不會被 block (spin_thread 是另一個支線)
     """
     
     def __init__(self):
@@ -1168,6 +1270,7 @@ class fc_ros_manager:
         self.status_publisher: Optional[VehicleStatusPublisher] = None
         self.command_service: Optional[MavlinkCommandService] = None
         self.rtcm_relay: Optional[RtcmRelay] = None
+        self.log_publisher: Optional[FcNetworkLogPublisher] = None
         
         # Executor & Thread
         self.spin_thread: Optional[threading.Thread] = None
@@ -1188,12 +1291,19 @@ class fc_ros_manager:
             self.status_publisher = VehicleStatusPublisher()
             self.command_service = MavlinkCommandService()
             self.rtcm_relay = RtcmRelay()
+            if hasattr(fcmsg, 'FcNetworkLog'):
+                self.log_publisher = FcNetworkLogPublisher()
+            else:
+                logger.warning(
+                    "FcNetworkLog interface unavailable; /fc_network/logs disabled")
     
             # 創建執行者 MultiThreadedExecutor 並把 node 加入其中
             self.executor = MultiThreadedExecutor()
             self.executor.add_node(self.status_publisher)
             self.executor.add_node(self.command_service)
             self.executor.add_node(self.rtcm_relay)
+            if self.log_publisher:
+                self.executor.add_node(self.log_publisher)
             
             self.initialized = True
             # logger.info("fc_ros_manager initialized")
@@ -1219,6 +1329,8 @@ class fc_ros_manager:
             self.status_publisher.running = True
             self.command_service.running = True
             self.rtcm_relay.running = True
+            if self.log_publisher:
+                self.log_publisher.running = True
             
             self.spin_thread = threading.Thread(
                 target=self._spin_executor,
@@ -1344,6 +1456,8 @@ class fc_ros_manager:
                 self.command_service.stop()
             if self.rtcm_relay:
                 self.rtcm_relay.stop()
+            if self.log_publisher:
+                self.log_publisher.stop()
             
             # 等待 spin 執行緒結束
             if self.spin_thread and self.spin_thread.is_alive():
@@ -1370,6 +1484,8 @@ class fc_ros_manager:
                     self.command_service.destroy_node()
                 if self.rtcm_relay:
                     self.rtcm_relay.destroy_node()
+                if self.log_publisher:
+                    self.log_publisher.destroy_node()
                 
                 # 關閉 ROS2
                 if rclpy.ok():
@@ -1388,6 +1504,7 @@ class fc_ros_manager:
             'status_publisher_active': self.status_publisher is not None and self.status_publisher.running,
             'command_service_active': self.command_service is not None,
             'rtcm_relay_active': self.rtcm_relay is not None and self.rtcm_relay.running,
+            'log_publisher_active': self.log_publisher is not None and self.log_publisher.running,
         }
 
     
@@ -1434,8 +1551,11 @@ ros2_manager = fc_ros_manager()
 2. schedule_restart_node / _restart_node : 手動重啟單一 node (spin thread 內執行）
 3. orchestrator cmd: ("RESTART_ROS_NODE", node_key), node_key 見 NODE_KEYS
 
+2026.06.10
+1. 增加了 _publish_system_diagnostics 與 _publish_status_text 功能
+
+
 TODO 
 1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期
 
 '''
-

src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py

@@ -5,6 +5,7 @@ VehicleView - Pure State Container
 """
 
 import os
+from collections import deque
 from typing import Dict, Optional, Any, Tuple
 from dataclasses import dataclass, field
 from enum import Enum
@@ -12,7 +13,7 @@ from enum import Enum
 from .utils import setup_logger
 
 logger = setup_logger(os.path.basename(__file__))
-MODULE_VER = "1.00"
+MODULE_VER = "1.10"
 
 # ====================== Enums =====================
 
@@ -117,6 +118,30 @@ class VFR:
     timestamp: Optional[float] = None  # 時間戳記
 
 
+@dataclass
+class StatusTextEntry:
+    """飛控狀態文字（來源：MAVLink STATUSTEXT）"""
+    text: str
+    severity: Optional[int] = None
+    timestamp: Optional[float] = None
+
+
+@dataclass
+class SystemDiagnostics:
+    """系統診斷資訊（來源：MAVLink SYS_STATUS，不含電池欄位）"""
+    sensors_install_mask: Optional[int] = None
+    sensors_enabled_mask: Optional[int] = None
+    sensors_health_mask: Optional[int] = None
+    mcuLoad: Optional[int] = None          # 單位 1%
+    busErrorRate: Optional[int] = None     # 單位 0.1%
+    busErrorCount: Optional[int] = None
+    errors_count1: Optional[int] = None
+    errors_count2: Optional[int] = None
+    errors_count3: Optional[int] = None
+    errors_count4: Optional[int] = None
+    timestamp: Optional[float] = None
+
+
 @dataclass
 class ComponentStatus:
     """組件狀態容器"""
@@ -127,6 +152,8 @@ class ComponentStatus:
     ekf: EKF = field(default_factory=EKF)
     gps: GPS = field(default_factory=GPS)
     vfr: VFR = field(default_factory=VFR)
+    sys_diag: SystemDiagnostics = field(default_factory=SystemDiagnostics)
+    status_text_queue: deque = field(default_factory=lambda: deque(maxlen=64))
     
     # 系統狀態
     system_status: Optional[int] = None  # MAV_STATE
@@ -160,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
@@ -283,6 +311,7 @@ class RFModule:
 
 class VehicleView:
     """
+    最上層
     載具視圖 - 純狀態容器
     
     特點:

src/fc_network_adapter/fc_network_adapter/serialManager.py

@@ -14,20 +14,30 @@ import signal
 import time
 import threading
 import struct
+from collections import deque
 from enum import Enum, auto
 from abc import ABC, abstractmethod
 from dataclasses import dataclass
+from typing import Callable, Optional
+from typing import List
 
 # # XBee 模組
 # from xbee.frame import APIFrame
 
 # 自定義的 import
+from .mavlinkVehicleView import (
+    vehicle_registry, # 儲存全部物件的地方
+    VehicleView,
+    RFModule,
+    RFModuleType,
+)
 from .utils import RingBuffer, setup_logger
+from .utils import pollStrategy
 
 # ====================== 分割線 =====================
 
 logger = setup_logger(os.path.basename(__file__))
-MODULE_VER = "0.80"
+MODULE_VER = "2.00"
 
 rx_module_ack = RingBuffer(capacity=64, buffer_id=253)
 
@@ -37,8 +47,8 @@ rx_module_ack = RingBuffer(capacity=64, buffer_id=253)
 class SerialMode(Enum):
     """連接類型"""
     STRAIGHT = auto()      # 原始數據直通
-    XBEEAPI2AT = auto()    # XBee API 模式
-    XBEEAPI_POLL = auto()
+    XBEEAPI2AT = auto()    # XBee API-AT 模式
+    XBEEAPI_espv1 = auto()  # XBee API-API 模式 esp v1
     NOT_USE = auto()       # 不使用
 
 
@@ -101,19 +111,28 @@ class RawFrameProcessor(FrameProcessor):
         return data
 
 
-class XBeeFrameProcessor(FrameProcessor):
+class XBeeFrameProcessor_Base(FrameProcessor):
     """
     XBee API 協議處理器
+    處理 XBEE API 端口 對應 -> 遠端 XBEE AT Mode
+    For SerialMode.XBEEAPI2AT
 
     職責：
     - XBee API frame 的拆幀 / 組幀
     - 0x90 (RX Packet)   -> 解出 payload 回傳
     - 0x88 (AT Response) -> 轉交 at_handler 處理（若有注入）
-    - 0x8B (TX Status)   -> 目前忽略
+    - 0x8B (TX Status)   -> 目前寫LOG (開發用)
     - 其他 frame type    -> 記 warning 忽略
 
-    若未來要做變化型 XBee（例如 API2 escape mode、不同 addressing），
+    若未來要做變化型 XBee (例如 API2 escape mode、不同 addressing)
     繼承此類並覆寫 _encapsulate / _decapsulate / _try_extract_frame 即可。
+
+    硬體產品系列 (Product Family) : XB3-24
+    晶片世代: Digi XBee 3 代架構（搭載 Silicon Labs EFR32 微控制器）。
+    運作頻段: 2.4 GHz RF。
+    硬體構型: TH (Through-Hole)，標準針腳插孔式引腳設計。
+    運行協議 (Protocol / Function Set): 802.15.4
+    韌體版本 (Firmware Version): 2014 -> 2 (XBee 3 平台) + 0 (802.15.4 協議代碼) + 14 (次要版本號)。
     """
 
     # XBee API frame type
@@ -122,6 +141,12 @@ class XBeeFrameProcessor(FrameProcessor):
     FRAME_TYPE_TX_STATUS   = 0x8B
     FRAME_TYPE_RX_PACKET   = 0x90
 
+    # ADDR16 選項
+    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
@@ -174,11 +199,15 @@ class XBeeFrameProcessor(FrameProcessor):
         return None
 
     def _dispatch_frame(self, frame: bytes) -> bytes:
-        """根據 frame type 分派；若是 RX payload 回傳 bytes，其餘回傳 None"""
+        """
+        根據 frame type 分派
+        1. 若是 RX payload 則 return bytes 遞交出去
+        2. 若是 AT command 則 進到相依類別 ATCommandHandler 處理
+        """
         frame_type = frame[3]
 
         if frame_type == self.FRAME_TYPE_RX_PACKET: # mavlink
-            return self._decapsulate(frame)
+            return self._decapsulate(frame)[0]
 
         if frame_type == self.FRAME_TYPE_AT_RESPONSE: # AT command
             if self.at_handler is not None:
@@ -186,6 +215,12 @@ class XBeeFrameProcessor(FrameProcessor):
             return None
 
         if frame_type == self.FRAME_TYPE_TX_STATUS:
+            length = (frame[1] << 8) | frame[2] # for debug
+            logger.info(
+                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]}"
+            ) # for debug
             return None
 
         logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}")
@@ -195,7 +230,8 @@ class XBeeFrameProcessor(FrameProcessor):
     @staticmethod
     def _encapsulate(
         data: bytes,
-        dest_addr64: bytes = b'\x00\x00\x00\x00\x00\x00\xFF\xFF',
+        dest_addr64: bytes = DEST_ADDR64_BRAODCAST,
+        dest_addr16 = DEST_ADDR16_BRAODCAST,
         frame_id: int = 0x01,
     ) -> bytes:
         """
@@ -203,8 +239,7 @@ class XBeeFrameProcessor(FrameProcessor):
         - 使用廣播地址
         - 添加適當的頭部和校驗和
         """
-        frame_type = XBeeFrameProcessor.FRAME_TYPE_TX_REQUEST
-        dest_addr16 = b'\xFF\xFE'
+        frame_type = XBeeFrameProcessor_Base.FRAME_TYPE_TX_REQUEST
         broadcast_radius = 0x00
         options = 0x00
 
@@ -219,7 +254,557 @@ class XBeeFrameProcessor(FrameProcessor):
         """從 RX Packet (0x90) 取出 payload"""
         length = (frame[1] << 8) | frame[2]
         rf_data_start = 3 + 12
-        return frame[rf_data_start:3 + length]
+        payload = frame[rf_data_start:3 + length]
+        senderAddr = frame[4:12]
+        return payload, senderAddr
+
+
+class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
+    '''
+    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'
+    POLL_HEADER = b'POLL'
+    DONE_HEADER = b'DONE'
+
+    MAX_BYTES_PER_FLUSH = 150
+    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
+            self.address_64 = address_64
+            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 # 收到封包累計
+
+    def __init__(self, at_handler: "ATCommandHandler" = None):
+        super().__init__(at_handler)
+
+        self.max_discovery_window_ms = 220
+        self.is_discovery_phase = False
+        self.esp32_address_mapping = {}
+
+        self.operator_busy = False
+        self.operator_running = False
+
+        self.serial_writer: Optional[Callable[[bytes], None]] = None
+        self.event_loop: Optional[asyncio.AbstractEventLoop] = None
+        self.serial_baudrate = 115200
+
+        self.gcs_transmit_queue: deque[bytearray] = deque()
+        self.poll_scheduler_state = pollStrategy.PollSchedulerState()
+
+        self.command_pending_event: Optional[asyncio.Event] = None
+        self.poll_done_event: Optional[asyncio.Event] = None
+        self.current_poll_address_64: Optional[bytes] = None
+
+        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 # 
+        self.guard_milliseconds = 50 # POLL DONE 的保底時間間隔
+
+        self.pending_manual_discovery = False
+        self.pending_manual_poll: Optional[tuple[int, Optional[int]]] = None
+
+    # ---- 注入與設定 ----
+
+    def set_writer(self, writer: Callable[[bytes], None]) -> None:
+        self.serial_writer = writer
+
+    def set_event_loop(self, loop: asyncio.AbstractEventLoop) -> None:
+        self.event_loop = loop
+        self._ensure_async_primitives()
+
+    def set_serial_baudrate(self, baudrate: int) -> None:
+        self.serial_baudrate = baudrate
+
+    def _ensure_async_primitives(self) -> None:
+        if self.command_pending_event is None:
+            self.command_pending_event = asyncio.Event()
+        if self.poll_done_event is None:
+            self.poll_done_event = asyncio.Event()
+
+    # 有急事 叫醒 operator 做下一個 tick
+    def wake_operator(self) -> None:
+        self._ensure_async_primitives()
+        self.command_pending_event.set()
+
+    # ---- 拆幀分派 ----
+
+    def _dispatch_frame(self, frame: bytes) -> Optional[bytes]:
+        frame_type = frame[3]
+
+        if frame_type == self.FRAME_TYPE_RX_PACKET:
+            payload, sender_address_64 = self._decapsulate(frame)
+
+            if payload.startswith(self.HELLO_HEADER) and len(payload) == 5:
+                self.handle_hello_report(payload, sender_address_64)
+                return None
+
+            if payload.startswith(self.DONE_HEADER) and len(payload) == 9:
+                self.handle_done_report(payload, sender_address_64)
+                return None
+
+            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:
+            if self.at_handler is not None:
+                self.at_handler.handle_frame(frame)
+            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]}"
+            )
+            return None
+
+        logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}")
+        return None
+
+    # ---- DISC / POLL 封裝 ----
+
+    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)
+
+        if remote_device is None:
+            self.esp32_address_mapping[sender_address_64] = self.Esp32DeviceInfo(
+                system_id, sender_address_64, time.time()
+            )
+            logger.debug(
+                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:
+            logger.warning(
+                f"SYSID duplicated system_id={system_id}, "
+                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:
+            return None
+
+        poll_payload = self.POLL_HEADER + struct.pack(
+            '>BH', remote_device.system_id, grant_bytes
+        )
+        remote_device.received_len = 0
+        return self._encapsulate(
+            poll_payload,
+            dest_addr64=remote_device.address_64,
+            dest_addr16=self.DEST_ADDR16_UNICAST,
+            frame_id=0x02,
+        )
+
+    def handle_done_report(self, payload: bytes, sender_address_64: bytes) -> None:
+        remote_device = self.esp32_address_mapping.get(sender_address_64)
+        if remote_device is None:
+            return
+
+        # 這段是有問題的 因為會有整數封包切割問題 以及載具端的 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()
+
+        if (
+            self.current_poll_address_64 is not None
+            and sender_address_64 == self.current_poll_address_64
+            and self.poll_done_event is not None
+        ):
+            self.poll_done_event.set()
+
+    # ---- UDP到Serial 佇列 ----
+
+    def enqueue_gcs_transmit(self, payload: bytes) -> None:
+        if not payload:
+            return
+        self.gcs_transmit_queue.append(bytearray(payload))
+
+    # 只是 show 狀態
+    def get_gcs_queue_byte_count(self) -> int:
+        return sum(len(packet) for packet in self.gcs_transmit_queue)
+
+    # 只是 show 狀態
+    def get_gcs_queue_packet_count(self) -> int:
+        return len(self.gcs_transmit_queue)
+
+    # 把 gcs_transmit_queue 的 mavlink 封包依照大小打包出來
+    def _pop_flush_batch(self, max_bytes: int) -> List[bytes]:
+        if not self.gcs_transmit_queue:
+            return []
+
+        batch: List[bytes] = []
+        total_bytes = 0
+
+        while self.gcs_transmit_queue:
+            next_packet = bytes(self.gcs_transmit_queue[0])
+            packet_length = len(next_packet)
+
+            if not batch:
+                batch.append(self.gcs_transmit_queue.popleft())
+                total_bytes = packet_length
+                if packet_length > max_bytes:
+                    break
+                continue
+
+            if total_bytes + packet_length <= max_bytes:
+                batch.append(self.gcs_transmit_queue.popleft())
+                total_bytes += packet_length
+            else:
+                break
+
+        return batch
+
+    # 把數據封裝好以後 交給 serial_writer 排程送出
+    async def _send_gcs_packet(self, packet: bytes) -> None:
+        # 重複判斷
+        # if self.serial_writer is None:
+        #     return
+
+        sent_offset = 0
+        while sent_offset < len(packet):
+            chunk_end = min(
+                sent_offset + self.MAX_PAYLOAD_PER_FRAME,
+                len(packet),
+            )
+            chunk = packet[sent_offset:chunk_end]
+            sent_offset = chunk_end
+            self.serial_writer(self._encapsulate(chunk))
+            await asyncio.sleep(self.CHUNK_SEND_INTERVAL_SEC)
+
+    # 把上面兩個步驟打包起來 處理從 UDP 來的資訊 丟給 Serial
+    async def flush_gcs_transmit_queue(
+        self,
+        max_bytes: int = MAX_BYTES_PER_FLUSH,
+    ) -> None:
+        if self.serial_writer is None:
+            logger.warning("GCS flush skipped: serial writer not ready")
+            return
+
+        for packet in self._pop_flush_batch(max_bytes):
+            await self._send_gcs_packet(packet)
+
+    # ---- POLL 排程輔助 ----
+
+    # 計算下次要 poll 的對象跟大小
+    def _pick_poll_target(self):
+        poll_devices = [
+            pollStrategy.PollDevice(
+                address_64=address_64,
+                # system_id=device.system_id,
+                remain_bytes=device.remain_bytes,
+                last_done_time=device.last_done_time,
+            )
+            for address_64, device in self.esp32_address_mapping.items()
+        ]
+
+        return pollStrategy.pick_next(
+            poll_devices,
+            self.poll_scheduler_state
+        )
+
+    # 移除長時間沒有 HELLO 或 DONE 的 Dongle
+    def _prune_stale_devices(self) -> None:
+        now = time.time()
+        stale_addresses = [
+            address_64
+            for address_64, device in self.esp32_address_mapping.items()
+            if (now - device.last_hello_time > self.device_offline_timeout) and \
+               (now - device.last_done_time > self.device_offline_timeout)
+        ]
+        for address_64 in stale_addresses:
+            device = self.esp32_address_mapping.pop(address_64, None)
+            if device is not None:
+                logger.info(
+                    f"Removed stale device system_id={device.system_id} "
+                    f"address_64={address_64.hex()}"
+                )
+
+    # 判斷是否進入 discovery 程序
+    def _should_run_discovery(self) -> bool:
+        # 條件1. 目前沒有任何遠端ESP裝置被紀錄 或者 手動啟動
+        if (not self.esp32_address_mapping) or (self.pending_manual_discovery):
+            return True
+        # 條件2. 每個固定週期 會做一次
+        return (
+            time.time() - self.last_discovery_time
+            >= self.discovery_interval_seconds
+        )
+
+    # ---- 手動請求（thread-safe 對外 API）----
+
+    def request_discovery(self) -> bool:
+        """
+        手動觸發 discovery 可從任意 thread 呼叫。
+        實際排程在 serial_manager 的 event loop 內執行。
+        """
+        if self.event_loop is None:
+            logger.warning("ESPv1 request_discovery: event loop not ready")
+            return False
+        if not self.operator_running:
+            logger.warning("ESPv1 request_discovery: operator not running")
+            return False
+        self.event_loop.call_soon_threadsafe(self._apply_request_discovery)
+        return True
+
+    def request_poll(
+        self,
+        target_system_id: int,
+        grant_bytes: Optional[int] = None,
+    ) -> bool:
+        """
+        手動對指定 system_id 排入一輪 POLL 可從任意 thread 呼叫。
+        實際排程在 serial_manager 的 event loop 內執行。
+        """
+        if self.event_loop is None:
+            logger.warning("ESPv1 request_poll: event loop not ready")
+            return False
+        if not self.operator_running:
+            logger.warning("ESPv1 request_poll: operator not running")
+            return False
+        self.event_loop.call_soon_threadsafe(
+            self._apply_request_poll,
+            target_system_id,
+            grant_bytes,
+        )
+        return True
+
+    def _apply_request_discovery(self) -> None:
+        self.pending_manual_discovery = True
+        self.wake_operator()
+
+    def _apply_request_poll(
+        self,
+        target_system_id: int,
+        grant_bytes: Optional[int],
+    ) -> None:
+        self.pending_manual_poll = (target_system_id, grant_bytes)
+        self.wake_operator()
+
+    def get_status_snapshot(self) -> dict:
+        """
+        唯讀狀態快照 for debug
+        從其他 thread 讀取時不保證與 operator 原子一致。
+        """
+        now = time.time()
+        devices = []
+        for address_64, device in self.esp32_address_mapping.items():
+            devices.append({
+                "system_id": device.system_id,
+                "address_64": address_64.hex(),
+                "remain_bytes": device.remain_bytes,
+                "last_hello_age_seconds": now - device.last_hello_time,
+                "last_done_age_seconds": (
+                    now - device.last_done_time if device.last_done_time else None
+                ),
+            })
+        return {
+            "operator_busy": self.operator_busy,
+            "is_discovery_phase": self.is_discovery_phase,
+            "gcs_queue_bytes": self.get_gcs_queue_byte_count(),
+            "gcs_queue_packets": self.get_gcs_queue_packet_count(),
+            "devices": devices,
+        }
+
+    # ---- operator 主循環 ----
+
+    # operator 的運行鐘 有點像是 spin_once
+    async def operator_loop(self) -> None:
+        self._ensure_async_primitives()
+        logger.info("ESPv1 operator loop started")
+
+        while self.operator_running:
+            try:
+                await self._operator_tick() # TODO 最好不要在 while loop 塞 try 想辦法改掉
+            except asyncio.CancelledError:
+                raise
+            except Exception as exc:
+                logger.error(f"ESPv1 operator tick error: {exc}")
+
+            await self._wait_for_next_tick()
+
+        logger.info("ESPv1 operator loop stopped")
+
+    # 安排啥時要醒來做事
+    async def _wait_for_next_tick(self) -> None:
+        self.command_pending_event.clear()
+
+        # 固定時間醒來
+        sleep_task = asyncio.create_task( asyncio.sleep(self.operator_tick_interval_seconds) )
+        # 有"急事"被叫醒
+        wake_task = asyncio.create_task(self.command_pending_event.wait())
+
+        done, pending = await asyncio.wait(
+            {sleep_task, wake_task},
+            return_when=asyncio.FIRST_COMPLETED,
+        )
+
+        for task in pending:
+            task.cancel()
+
+    async def _operator_tick(self) -> None:
+        # 1. 移除沒反應 dongle
+        self._prune_stale_devices()
+
+        # 2. 忙碌時 略過這次循環
+        if self.operator_busy:
+            return
+
+        # 3. (最優先) 處理從 UDP 過來的封包 並且透過 Serial 送出
+        if self.gcs_transmit_queue:
+            await self.flush_gcs_transmit_queue()
+            return
+
+        # 4. 檢測要不要跑 discovery 程序
+        if self._should_run_discovery():
+            await self._run_discovery()
+            return
+
+        # 5. POLL 程序 (若有手動先執行 若無自動則策略決策)
+        if self.pending_manual_poll is not None:
+            target_system_id, grant_bytes = self.pending_manual_poll
+            self.pending_manual_poll = None
+            target_address = self._find_address_by_system_id(target_system_id)
+        else:
+            target_address, grant_bytes = self._pick_poll_target()
+
+        if target_address is None:
+            return
+        await self._run_one_poll(target_address, grant_bytes if grant_bytes is not None else 0)
+
+    def _find_address_by_system_id(self, target_system_id: int) -> Optional[bytes]:
+        for address_64, device in self.esp32_address_mapping.items():
+            if device.system_id == target_system_id:
+                return address_64
+        return None
+
+    # discovery 程序 
+    async def _run_discovery(self) -> None:
+        if self.serial_writer is None:
+            self.pending_manual_discovery = False
+            return
+
+        self.operator_busy = True
+        self.is_discovery_phase = True
+        
+        self.serial_writer(self.pack_discovery())
+        self.last_discovery_time = time.time()
+        await asyncio.sleep(self.max_discovery_window_ms / 1000.0)
+
+        self.operator_busy = False
+        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:
+            return
+
+        # 這邊把要求封包最小量放在36 是讓載具端正好可以回傳一個有簽章的 HEARTBEAT 封包的大小
+        # 算是 "探測封包" 這樣讓系統可以更快速的知道載具端殘餘的資料量
+        grant_bytes = max(36, min(int(grant_bytes), 65535))
+
+        poll_frame = self.pack_poll(target_address_64, grant_bytes)
+        if poll_frame is None:
+            return
+
+        self._ensure_async_primitives()
+        self.operator_busy = True
+        self.current_poll_address_64 = target_address_64
+        self.poll_done_event.clear()
+        self.serial_writer(poll_frame)
+        self.last_recieve_mavlink = time.time()
+
+        try:
+            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
+            await asyncio.sleep(self.guard_milliseconds / 1000.0)
+
+    def stop_operator(self) -> None:
+        self.operator_running = False
+        self.gcs_transmit_queue.clear()
+        self.wake_operator()
 
 
 # ====================== Dongle Command Handler =====================
@@ -281,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:
@@ -326,30 +911,30 @@ 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
 
-
 # ================ Serial UDP Socket Object ==============
 class SerialHandler(asyncio.Protocol):
     """asyncio.Protocol 用於處理 Serial 收發"""
@@ -368,13 +953,13 @@ class SerialHandler(asyncio.Protocol):
         if self.serial_mode == SerialMode.STRAIGHT:
             return RawFrameProcessor()
 
-        if self.serial_mode == SerialMode.XBEEAPI2AT:
+        elif self.serial_mode == SerialMode.XBEEAPI2AT:
             at_handler = ATCommandHandler(self.serial_port_str)
-            return XBeeFrameProcessor(at_handler=at_handler)
+            return XBeeFrameProcessor_Base(at_handler=at_handler)
 
-        # if self.serial_mode == SerialMode.XBEEAPI_POLL:
-        #     at_handler = ATCommandHandler_new(self.serial_port_str)
-        #     return XBeeFrameProcessor(at_handler=at_handler)
+        elif self.serial_mode == SerialMode.XBEEAPI_espv1:
+            at_handler = ATCommandHandler(self.serial_port_str)
+            return XBeeFrameProcessor_ESPv1(at_handler=at_handler)
 
         logger.warning(f"Unknown serial mode: {self.serial_mode}, using Raw")
         return RawFrameProcessor()
@@ -387,6 +972,13 @@ class SerialHandler(asyncio.Protocol):
         if self.serial_mode == SerialMode.XBEEAPI2AT:
             self.processor.at_handler.set_writer(self.transport.write)
 
+        elif self.serial_mode == SerialMode.XBEEAPI_espv1:
+            if isinstance(self.processor, XBeeFrameProcessor_ESPv1):
+                self.processor.set_writer(self.transport.write)
+                self.processor.set_event_loop(asyncio.get_running_loop())
+                if self.processor.at_handler is not None:
+                    self.processor.at_handler.set_writer(self.transport.write)
+
         if hasattr(self.udp_handler, 'set_serial_handler'):
             self.udp_handler.set_serial_handler(self)
         logger.debug(f"Serial port {self.serial_port_str} connected")
@@ -429,11 +1021,15 @@ class UDPHandler(asyncio.DatagramProtocol):
         if not self.serial_handler:
             logger.warning("Serial handler not set, dropping UDP packet")
             return
-        
-        # 使用 processor 封裝數據
+
+        if self.serial_mode == SerialMode.XBEEAPI_espv1:
+            processor = self.serial_handler.processor
+            # if isinstance(processor, XBeeFrameProcessor_ESPv1): # 預設唯一綁定 少一個 if 多一點效率
+            processor.enqueue_gcs_transmit(data)
+            processor.wake_operator()
+            return
+
         processed_data = self.serial_handler.processor.process_outgoing(data)
-        
-        # 發送到串口
         self.serial_handler.transport.write(processed_data)
 
 
@@ -454,6 +1050,7 @@ class serial_manager:
             self.protocol = None
             self.udp_handler = None
             self.serial_handler = None
+            self.operator_task = None
 
     def __init__(self):
         self.thread = None
@@ -594,6 +1191,16 @@ class serial_manager:
             
             logger.debug(f"Serial connection created for {serial_port}")
 
+            if serial_mode == SerialMode.XBEEAPI_espv1:
+                processor = serial_obj.serial_handler.processor
+                if isinstance(processor, XBeeFrameProcessor_ESPv1):
+                    processor.set_serial_baudrate(baudrate)
+                    processor.operator_running = True
+                    serial_obj.operator_task = asyncio.create_task(
+                        processor.operator_loop()
+                    )
+                    logger.debug(f"ESPv1 operator task started for {serial_port}")
+
             # 將 serial_object 加入管理列表
             serial_id = self.serial_count + 1
             self.serial_objects[serial_id] = serial_obj
@@ -646,7 +1253,19 @@ class serial_manager:
         
         try:
             serial_obj = self.serial_objects[serial_id]
-            
+
+            if serial_obj.serial_mode == SerialMode.XBEEAPI_espv1:
+                processor = serial_obj.serial_handler.processor
+                if isinstance(processor, XBeeFrameProcessor_ESPv1):
+                    processor.stop_operator()
+                if serial_obj.operator_task is not None:
+                    serial_obj.operator_task.cancel()
+                    try:
+                        await serial_obj.operator_task
+                    except asyncio.CancelledError:
+                        pass
+                    serial_obj.operator_task = None
+
             # 關閉 UDP transport
             if hasattr(serial_obj, 'transport') and serial_obj.transport:
                 serial_obj.transport.close()
@@ -674,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
         回傳是否成功排進事件圈
@@ -687,6 +1306,7 @@ class serial_manager:
             logger.error(f"Serial object {serial_id} not found")
             return False
 
+        # TODO 這邊的防呆 應該可以不用 if 有空再改
         serial_obj = self.serial_objects[serial_id]
         if serial_obj.serial_mode != SerialMode.XBEEAPI2AT:
             logger.error(
@@ -699,6 +1319,18 @@ class serial_manager:
         self.loop.call_soon_threadsafe(at_handler.send_command, request)
         return True
 
+    def get_espv1_processor(self, serial_id: int) -> Optional[XBeeFrameProcessor_ESPv1]:
+        """依 serial_id 取得 ESPv1 processor 不存在或模式不符回傳 None。"""
+        if serial_id not in self.serial_objects:
+            return None
+        serial_obj = self.serial_objects[serial_id]
+        if serial_obj.serial_mode != SerialMode.XBEEAPI_espv1:
+            return None
+        processor = serial_obj.serial_handler.processor
+        if not isinstance(processor, XBeeFrameProcessor_ESPv1):
+            return None
+        return processor
+
     @staticmethod
     def check_serial_port(serial_port, baudrate):
         """檢查串口是否存在與可用"""
@@ -736,42 +1368,105 @@ if __name__ == '__main__':
     sm = serial_manager()
     sm.start()
 
+    # # 測試項一 
     # SERIAL_PORT = '/dev/ttyACM0'  # 手動指定
     # SERIAL_BAUDRATE = 115200
     # 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
 
     linked_serial = sm.get_serial_link()
-    print(linked_serial)
+    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):
+    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)
+    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)
 
+    # 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()
 
 '''
 ================= 改版記錄 ============================
 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 
+2. XBeeFrameProcessor_Base 新增 _encapsulate, _decapsulate 編碼解碼 xbee 封包的功能 (原來在 XBeeFrameHandler 中)
+3. XBeeFrameProcessor_Base 新增 _try_extract_frame 處理被可能截斷的 UART 封包
+4. XBeeFrameProcessor_Base 新增 _dispatch_frame 分配封包到 UDP 或者 Dongle Command Handler 
 5. ATCommandHandler 新增 _parse 去拆解 0x88 AT Command Response
 6. ATCommandHandler 新增 _dispatch 把拆解的結果 分配到 _handle_XXX
 7. ATCommandHandler 新增各項 _handle_XXX (未實作)
 
+2026.06.15
+1. 修改 XBeeFrameProcessor_Base _decapsulate 使其解出發送端 dongle 的 src64 地址
+2. 新增 XBeeFrameProcessor_ESPv1 這個類別繼承 XBeeFrameProcessor_Base 
+3. 新增 XBeeFrameProcessor_ESPv1 組合 DISC 跟 POLL 訊息 處理 HELO 跟 DONE 的能力
+4. DISC 自動化與手動
+5. POLL 的自動化
+
+2026.06.16
+1. 移除 DongleCommandHandler_ESPv1
+2. ESPv1 對外 API（request_discovery / request_poll）移至 XBeeFrameProcessor_ESPv1，thread-safe
+3. serial_manager 僅保留 get_espv1_processor(serial_id) lookup
+
+
+註解1 : FRAME_TYPE_TX_STATUS 的對應解說 (我不喜歡程式塞太多為了顯示而顯示的東西 錯誤碼自己下來看)
+    TX_DELIVERY_STATUS
+        0x00: "Success",
+        0x01: "No ACK received",
+        0x02: "CCA failure",
+        0x15: "Invalid destination endpoint",
+        0x21: "Network ACK failure",
+        0x22: "Not joined to network",
+        0x23: "Self-addressed",
+        0x24: "Address not found",
+        0x25: "Route not found",
+        0x26: "Broadcast source relay",
+        0x27: "Insufficient data",
+        0x28: "TX buffered",
+        0x32: "Invalid send flag",
+        0x74: "Resource error",
+
+    TX_DISCOVERY_STATUS
+        0x00: "No discovery overhead",
+        0x01: "Address discovery",
+        0x02: "Route discovery",
+        0x03: "Address and route discovery",
+
+
 '''
 

src/fc_network_adapter/fc_network_adapter/utils/__init__.py

@@ -2,6 +2,6 @@
 共用工具模組
 """
 from .ringBuffer import RingBuffer
-from .theLogger import setup_logger
+from .theLogger import get_ros_log_queue, setup_logger
 
-__all__ = ['RingBuffer', 'setup_logger']
+__all__ = ['RingBuffer', 'get_ros_log_queue', 'setup_logger']

src/fc_network_adapter/fc_network_adapter/utils/pollStrategy.py

@@ -0,0 +1,48 @@
+"""
+POLL 輪詢策略，供 XBeeFrameProcessor_ESPv1 使用。
+
+不依賴 serialManager，僅接受裝置快照與排程狀態。
+"""
+
+from dataclasses import dataclass
+from typing import List
+
+
+@dataclass(frozen=True)
+class PollDevice:
+    """從 esp32AddrMapping 抽出的唯讀快照。"""
+    address_64: bytes
+    # system_id: int
+    remain_bytes: int
+    last_done_time: float
+
+
+@dataclass
+class PollSchedulerState:
+    """每條 serial link 一份，保存 round-robin 索引。"""
+    round_robin_index: int = 0
+
+
+def pick_next(
+    devices: List[PollDevice],
+    scheduler_state: PollSchedulerState
+):
+    """
+    選下一個 POLL 目標。
+
+    回傳 (target_address_64, grant_bytes)；devices 為空時回傳 (None, 0)。
+    """
+    if not devices:
+        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
+    if selected_device.remain_bytes > 0:
+        grant_bytes = min( 65535, max(0, selected_device.remain_bytes))
+
+    return selected_device.address_64, grant_bytes
+

src/fc_network_adapter/fc_network_adapter/utils/theLogger.py

@@ -1,9 +1,47 @@
 import logging
 import os
+import queue
 from logging.handlers import TimedRotatingFileHandler
 
 # 全域 Logger 實例
 _global_logger = None
+_ros_log_queue = queue.Queue(maxsize=2000)
+
+
+class RosTopicQueueHandler(logging.Handler):
+    """將 FC Network log record 非阻塞地排入 ROS publisher 佇列。"""
+
+    def emit(self, record: logging.LogRecord) -> None:
+        try:
+            sysid = int(getattr(record, 'sysid', -1))
+        except (TypeError, ValueError):
+            sysid = -1
+
+        item = {
+            'created': float(record.created),
+            'level': int(record.levelno),
+            'source': str(record.name),
+            'event_code': str(getattr(record, 'event_code', '')),
+            'sysid': sysid,
+            'message': record.getMessage(),
+        }
+        try:
+            _ros_log_queue.put_nowait(item)
+        except queue.Full:
+            # GUI 不應反過來拖慢通訊；滿載時淘汰最舊一筆。
+            try:
+                _ros_log_queue.get_nowait()
+            except queue.Empty:
+                return
+            try:
+                _ros_log_queue.put_nowait(item)
+            except queue.Full:
+                pass
+
+
+def get_ros_log_queue():
+    """供 ROS publisher node 取得共用、thread-safe 的紀錄佇列。"""
+    return _ros_log_queue
 
 def setup_logger(name: str, log_dir: str = "logs", level=logging.DEBUG) -> logging.Logger:
     global _global_logger
@@ -37,6 +75,10 @@ def setup_logger(name: str, log_dir: str = "logs", level=logging.DEBUG) -> loggi
         console_handler.setFormatter(formatter)
         _global_logger.addHandler(console_handler)
 
+        ros_queue_handler = RosTopicQueueHandler()
+        ros_queue_handler.setLevel(logging.INFO)
+        _global_logger.addHandler(ros_queue_handler)
+
     # 為每個模組建立子 Logger，並設定名稱
     module_logger = _global_logger.getChild(name)
     module_logger.name = name  # 修改子 Logger 的名稱，僅保留子 Logger 名稱

src/fc_network_apps/vehicleDiagShow.py

@@ -0,0 +1,544 @@
+#!/usr/bin/env python3
+"""Standalone curses TUI for fc_network vehicle diagnostics (sys_diags, status_text, summary)."""
+
+from __future__ import annotations
+
+import curses
+import json
+import re
+import sys
+import threading
+import time
+from dataclasses import dataclass, field
+from typing import Dict, List, Optional, Tuple
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+
+from fc_interfaces.msg import SystemDiagnosticsRaw
+
+VEHICLE_TOPIC_RE = re.compile(
+    r'^/fc_network/vehicle/(sys\d+)/(sys_diags|status_text|summary)$'
+)
+STATUS_TEXT_RE = re.compile(r'^\[([\d.]+)\]\s*\[(-?\d+)\]\s*(.*)$')
+
+STATUS_TEXT_TTL_SEC = 30.0
+STALE_SEC = 3.0
+RESCAN_SEC = 2.0
+OFFLINE_SEC = 10.0
+DRAW_HZ = 5.0
+
+# MAV_SYS_STATUS_SENSOR bit labels (MAVLink common)
+SENSOR_BITS: Tuple[Tuple[int, str], ...] = (
+    (268435456, 'PreArm'),
+    (1, 'Gyro'),
+    (2, 'Accel'),
+    (4, 'Mag'),
+    (8, 'Baro'),
+    (16, 'DiffPress'),
+    (32, 'GPS'),
+    (64, 'OptFlow'),
+    (128, 'Vision'),
+    (256, 'Laser'),
+    (512, 'ExtGT'),
+    (1024, 'AngRate'),
+    (2048, 'AttStab'),
+    (4096, 'YawPos'),
+    (8192, 'AltCtrl'),
+    (16384, 'XYCtrl'),
+    (32768, 'Motor'),
+    (65536, 'RC'),
+    (131072, 'Gyro2'),
+    (262144, 'Accel2'),
+    (524288, 'Mag2'),
+    (1048576, 'Geofence'),
+    (2097152, 'AHRS'),
+    (4194304, 'Terrain'),
+    (8388608, 'RevMotor'),
+    (16777216, 'Log'),
+    (33554432, 'Batt'),
+    (67108864, 'Prox'),
+    (134217728, 'Satcom'),
+    (536870912, 'ObsAvoid'),
+    (1073741824, 'Propulsion'),
+)
+
+MAV_STATE_LABELS = {
+    0: 'UNINIT',
+    1: 'BOOT',
+    2: 'CAL',
+    3: 'STANDBY',
+    4: 'ACTIVE',
+    5: 'CRITICAL',
+    6: 'EMERGENCY',
+    7: 'OFF',
+    8: 'TERM',
+}
+
+SEVERITY_LABELS = {
+    0: 'EMERG',
+    1: 'ALERT',
+    2: 'CRIT',
+    3: 'ERROR',
+    4: 'WARN',
+    5: 'NOTICE',
+    6: 'INFO',
+    7: 'DEBUG',
+}
+
+
+@dataclass
+class StatusTextEntry:
+    received_at: float
+    severity: int
+    text: str
+    raw: str
+    sys_key: str
+
+
+@dataclass
+class VehicleDiagState:
+    sys_key: str
+    sysid: int
+    summary: Optional[dict] = None
+    diag: Optional[SystemDiagnosticsRaw] = None
+    status_entries: List[StatusTextEntry] = field(default_factory=list)
+    last_summary_ts: float = 0.0
+    last_diag_ts: float = 0.0
+    last_topic_seen_ts: float = 0.0
+    offline: bool = False
+
+
+def sys_key_to_id(sys_key: str) -> int:
+    return int(sys_key.replace('sys', '', 1))
+
+
+def purge_status_entries(entries: List[StatusTextEntry], now: float, ttl: float = STATUS_TEXT_TTL_SEC) -> None:
+    cutoff = now - ttl
+    entries[:] = [e for e in entries if e.received_at >= cutoff]
+
+
+def parse_status_text(raw: str, sys_key: str, received_at: float) -> StatusTextEntry:
+    match = STATUS_TEXT_RE.match(raw.strip())
+    if match:
+        severity = int(match.group(2))
+        text = match.group(3)
+    else:
+        severity = -1
+        text = raw
+    return StatusTextEntry(
+        received_at=received_at,
+        severity=severity,
+        text=text,
+        raw=raw,
+        sys_key=sys_key,
+    )
+
+
+def decode_sensors(install: int, enabled: int, health: int) -> List[Tuple[str, str]]:
+    """Return list of (label, status) where status is OK / FAIL / OFF."""
+    items: List[Tuple[str, str]] = []
+    for bit, label in SENSOR_BITS:
+        if not (install & bit):
+            continue
+        if not (enabled & bit):
+            items.append((label, 'OFF'))
+        elif health & bit:
+            items.append((label, 'OK'))
+        else:
+            items.append((label, 'FAIL'))
+    return items
+
+
+def format_age(seconds: float) -> str:
+    if seconds < 0:
+        return 'never'
+    if seconds < 1.0:
+        return f'{seconds * 1000:.0f}ms'
+    if seconds < 60.0:
+        return f'{seconds:.1f}s'
+    return f'{seconds / 60:.1f}m'
+
+
+def truncate(text: str, width: int) -> str:
+    if width <= 0:
+        return ''
+    if len(text) <= width:
+        return text
+    if width <= 3:
+        return text[:width]
+    return text[: width - 3] + '...'
+
+
+class VehicleDiagNode(Node):
+    """ROS2 node: discover vehicles and subscribe to diagnostic topics."""
+
+    def __init__(self) -> None:
+        super().__init__('vehicle_diag_show')
+        self._lock = threading.Lock()
+        self._vehicles: Dict[str, VehicleDiagState] = {}
+        self._subs: Dict[str, Dict[str, object]] = {}
+        self._scan_timer = self.create_timer(RESCAN_SEC, self._rescan_topics)
+        self._rescan_topics()
+
+    def snapshot(self) -> Dict[str, VehicleDiagState]:
+        now = time.monotonic()
+        with self._lock:
+            for state in self._vehicles.values():
+                purge_status_entries(state.status_entries, now)
+                if state.last_topic_seen_ts > 0:
+                    state.offline = (now - state.last_topic_seen_ts) > OFFLINE_SEC
+            return {k: state for k, state in self._vehicles.items()}
+
+    def _get_or_create_state(self, sys_key: str) -> VehicleDiagState:
+        if sys_key not in self._vehicles:
+            self._vehicles[sys_key] = VehicleDiagState(
+                sys_key=sys_key,
+                sysid=sys_key_to_id(sys_key),
+            )
+        return self._vehicles[sys_key]
+
+    def _rescan_topics(self) -> None:
+        now = time.monotonic()
+        found: set = set()
+        for topic_name, _ in self.get_topic_names_and_types():
+            match = VEHICLE_TOPIC_RE.match(topic_name)
+            if not match:
+                continue
+            sys_key = match.group(1)
+            found.add(sys_key)
+            with self._lock:
+                state = self._get_or_create_state(sys_key)
+                state.last_topic_seen_ts = now
+                state.offline = False
+            self._ensure_subscriptions(sys_key)
+
+        with self._lock:
+            for sys_key, state in self._vehicles.items():
+                if sys_key not in found and state.last_topic_seen_ts > 0:
+                    state.offline = (now - state.last_topic_seen_ts) > OFFLINE_SEC
+
+    def _ensure_subscriptions(self, sys_key: str) -> None:
+        if sys_key in self._subs:
+            return
+        base = f'/fc_network/vehicle/{sys_key}'
+        subs = {
+            'summary': self.create_subscription(
+                String,
+                f'{base}/summary',
+                lambda msg, sk=sys_key: self._on_summary(sk, msg),
+                10,
+            ),
+            'sys_diags': self.create_subscription(
+                SystemDiagnosticsRaw,
+                f'{base}/sys_diags',
+                lambda msg, sk=sys_key: self._on_sys_diags(sk, msg),
+                10,
+            ),
+            'status_text': self.create_subscription(
+                String,
+                f'{base}/status_text',
+                lambda msg, sk=sys_key: self._on_status_text(sk, msg),
+                10,
+            ),
+        }
+        with self._lock:
+            self._subs[sys_key] = subs
+
+    def _on_summary(self, sys_key: str, msg: String) -> None:
+        now = time.monotonic()
+        try:
+            summary = json.loads(msg.data)
+        except json.JSONDecodeError:
+            summary = {'raw': msg.data}
+        with self._lock:
+            state = self._get_or_create_state(sys_key)
+            state.summary = summary
+            state.last_summary_ts = now
+            state.last_topic_seen_ts = now
+
+    def _on_sys_diags(self, sys_key: str, msg: SystemDiagnosticsRaw) -> None:
+        now = time.monotonic()
+        with self._lock:
+            state = self._get_or_create_state(sys_key)
+            state.diag = msg
+            state.last_diag_ts = now
+            state.last_topic_seen_ts = now
+
+    def _on_status_text(self, sys_key: str, msg: String) -> None:
+        now = time.monotonic()
+        entry = parse_status_text(msg.data, sys_key, now)
+        with self._lock:
+            state = self._get_or_create_state(sys_key)
+            state.status_entries.append(entry)
+            purge_status_entries(state.status_entries, now)
+            state.last_topic_seen_ts = now
+
+
+class DiagCursesApp:
+    """Curses front-end for vehicle diagnostics."""
+
+    def __init__(self, node: VehicleDiagNode) -> None:
+        self._node = node
+        self._running = True
+
+    def run(self) -> None:
+        curses.wrapper(self._main)
+
+    def _init_colors(self) -> None:
+        curses.start_color()
+        curses.use_default_colors()
+        curses.init_pair(1, curses.COLOR_GREEN, -1)    # OK / DISARMED / INFO
+        curses.init_pair(2, curses.COLOR_RED, -1)        # FAIL / ARMED / ERROR+
+        curses.init_pair(3, curses.COLOR_YELLOW, -1)   # WARN / OFF
+        curses.init_pair(4, curses.COLOR_CYAN, -1)     # header / INFO
+        curses.init_pair(5, curses.COLOR_WHITE, -1)    # dim / offline
+        curses.init_pair(6, curses.COLOR_MAGENTA, -1)  # NOTICE
+
+    def _main(self, stdscr) -> None:
+        curses.curs_set(0)
+        stdscr.nodelay(True)
+        stdscr.keypad(True)
+        self._init_colors()
+
+        while self._running and rclpy.ok():
+            try:
+                self._draw(stdscr)
+            except curses.error:
+                pass
+            key = stdscr.getch()
+            if key in (ord('q'), ord('Q'), 27):
+                self._running = False
+            time.sleep(1.0 / DRAW_HZ)
+
+    def _draw(self, stdscr) -> None:
+        stdscr.erase()
+        height, width = stdscr.getmaxyx()
+        now = time.monotonic()
+        vehicles = self._node.snapshot()
+        sorted_keys = sorted(vehicles.keys(), key=sys_key_to_id)
+
+        header = f' Vehicle Diagnostics Monitor  vehicles:{len(sorted_keys)}  Q:quit '
+        stdscr.attron(curses.color_pair(4) | curses.A_BOLD)
+        stdscr.addstr(0, 0, truncate(header.ljust(width - 1), width - 1))
+        stdscr.attroff(curses.color_pair(4) | curses.A_BOLD)
+
+        row = 1
+        if not sorted_keys:
+            msg = ' Waiting for vehicles... (need /fc_network/vehicle/sysN/* topics) '
+            if row < height - 1:
+                stdscr.addstr(row, max(0, (width - len(msg)) // 2), truncate(msg, width - 1))
+            row += 2
+
+        vehicle_rows_end = max(row, height - 8)
+        for sys_key in sorted_keys:
+            if row >= vehicle_rows_end:
+                break
+            state = vehicles[sys_key]
+            row = self._draw_vehicle_block(stdscr, row, width, state, now)
+            if row < height - 1:
+                try:
+                    stdscr.addstr(row, 0, truncate('-' * (width - 1), width - 1))
+                except curses.error:
+                    pass
+                row += 1
+
+        log_title = ' Status Text (last 30s) '
+        if row < height - 1:
+            stdscr.attron(curses.color_pair(4))
+            stdscr.addstr(row, 0, truncate(log_title.ljust(width - 1), width - 1))
+            stdscr.attroff(curses.color_pair(4))
+            row += 1
+
+        all_entries: List[StatusTextEntry] = []
+        for sys_key in sorted_keys:
+            all_entries.extend(vehicles[sys_key].status_entries)
+        all_entries.sort(key=lambda e: e.received_at)
+
+        log_rows = height - row - 1
+        if log_rows <= 0:
+            stdscr.refresh()
+            return
+
+        if not all_entries:
+            empty = ' (no messages in last 30s) '
+            if row < height - 1:
+                stdscr.addstr(row, 0, truncate(empty, width - 1))
+        else:
+            visible = all_entries[-log_rows:]
+            for entry in visible:
+                if row >= height - 1:
+                    break
+                self._draw_status_line(stdscr, row, width, entry)
+                row += 1
+
+        stdscr.refresh()
+
+    def _draw_vehicle_block(
+        self,
+        stdscr,
+        row: int,
+        width: int,
+        state: VehicleDiagState,
+        now: float,
+    ) -> int:
+        summary = state.summary or {}
+        mode = summary.get('mode_name', 'UNKNOWN')
+        armed = summary.get('armed', False)
+        conn = summary.get('connection_type', '?')
+        socket_id = summary.get('socket_id', -1)
+        mav_status = summary.get('mav_status', 0)
+        mav_label = MAV_STATE_LABELS.get(int(mav_status), str(mav_status))
+
+        last_update = max(state.last_summary_ts, state.last_diag_ts)
+        age_sec = (now - last_update) if last_update > 0 else -1.0
+        stale = age_sec >= 0 and age_sec > STALE_SEC
+
+        armed_str = 'ARMED' if armed else 'DISARMED'
+        flags = []
+        if state.offline:
+            flags.append('OFFLINE')
+        if stale and not state.offline:
+            flags.append('STALE')
+        flag_str = f"  [{' '.join(flags)}]" if flags else ''
+
+        line1 = (
+            f' {state.sys_key} (MAV {state.sysid})  {mode}  {armed_str}  '
+            f'{conn}  socket:{socket_id}  {mav_label}  '
+            f'updated {format_age(age_sec)}{flag_str}'
+        )
+        if row < stdscr.getmaxyx()[0] - 1:
+            if state.offline:
+                stdscr.attron(curses.color_pair(5))
+                stdscr.addstr(row, 0, truncate(line1, width - 1))
+                stdscr.attroff(curses.color_pair(5))
+            else:
+                stdscr.addstr(row, 0, truncate(line1, width - 1))
+                armed_pos = line1.find(armed_str)
+                if armed_pos >= 0:
+                    pair = curses.color_pair(2) if armed else curses.color_pair(1)
+                    stdscr.attron(pair | curses.A_BOLD)
+                    stdscr.addstr(row, armed_pos, armed_str)
+                    stdscr.attroff(pair | curses.A_BOLD)
+        row += 1
+
+        if state.diag is not None and row < stdscr.getmaxyx()[0] - 1:
+            load_pct = state.diag.mcu_load / 10.0
+            drop_pct = state.diag.bus_error_rate / 10.0
+            line2 = f' MCU {load_pct:.1f}%  CommDrop {drop_pct:.1f}%'
+            stdscr.addstr(row, 0, truncate(line2, width - 1))
+            row += 1
+
+            sensors = decode_sensors(
+                state.diag.sensors_install_mask,
+                state.diag.sensors_enabled_mask,
+                state.diag.sensors_health_mask,
+            )
+            row = self._draw_sensor_line(stdscr, row, width, sensors)
+        elif row < stdscr.getmaxyx()[0] - 1:
+            stdscr.addstr(row, 0, truncate(' (no sys_diags yet)', width - 1))
+            row += 1
+
+        return row
+
+    def _draw_sensor_line(
+        self,
+        stdscr,
+        row: int,
+        width: int,
+        sensors: List[Tuple[str, str]],
+    ) -> int:
+        max_row, _ = stdscr.getmaxyx()
+        if not sensors:
+            if row < max_row - 1:
+                stdscr.addstr(row, 0, truncate(' Sensors: (none installed)', width - 1))
+            return row + 1
+
+        lines: List[List[Tuple[str, str]]] = [[]]
+        line_len = len(' Sensors: ')
+        for label, status in sensors:
+            token = f'{label}:{status}  '
+            if lines[-1] and line_len + len(token) >= width - 1:
+                lines.append([])
+                line_len = 2
+            lines[-1].append((label, status))
+            line_len += len(token)
+
+        for idx, line_items in enumerate(lines):
+            if row >= max_row - 1:
+                break
+            prefix = ' Sensors: ' if idx == 0 else '  '
+            col = 0
+            stdscr.addstr(row, col, prefix)
+            col += len(prefix)
+            for label, status in line_items:
+                token = f'{label}:{status}  '
+                if col + len(token) >= width - 1:
+                    stdscr.addstr(row, col, truncate('...', width - col - 1))
+                    return row + 1
+                pair = curses.A_NORMAL
+                if status == 'OK':
+                    pair = curses.color_pair(1)
+                elif status == 'FAIL':
+                    pair = curses.color_pair(2) | curses.A_BOLD
+                elif status == 'OFF':
+                    pair = curses.color_pair(3)
+                stdscr.attron(pair)
+                stdscr.addstr(row, col, token)
+                stdscr.attroff(pair)
+                col += len(token)
+            row += 1
+        return row
+
+    def _draw_status_line(
+        self,
+        stdscr,
+        row: int,
+        width: int,
+        entry: StatusTextEntry,
+    ) -> None:
+        sev_label = SEVERITY_LABELS.get(entry.severity, str(entry.severity))
+        prefix = f'[{entry.sys_key:5}] [{entry.severity} {sev_label:5}] '
+        body = truncate(entry.text, max(0, width - len(prefix) - 2))
+        line = prefix + body
+
+        pair = curses.A_NORMAL
+        if entry.severity <= 3:
+            pair = curses.color_pair(2)
+        elif entry.severity == 4:
+            pair = curses.color_pair(3)
+        elif entry.severity == 5:
+            pair = curses.color_pair(6)
+        elif entry.severity == 6:
+            pair = curses.color_pair(1)
+        elif entry.severity == 7:
+            pair = curses.color_pair(5)
+
+        stdscr.attron(pair)
+        stdscr.addstr(row, 0, truncate(line, width - 1))
+        stdscr.attroff(pair)
+
+
+def main() -> int:
+    if not sys.stdout.isatty():
+        print('vehicleDiagShow requires an interactive terminal (TTY).', file=sys.stderr)
+        return 1
+
+    rclpy.init()
+    node = VehicleDiagNode()
+    spin_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
+    spin_thread.start()
+
+    try:
+        DiagCursesApp(node).run()
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+        spin_thread.join(timeout=2.0)
+
+    return 0
+
+
+if __name__ == '__main__':
+    sys.exit(main())

src/fc_network_module/fc_network_module/ntrip_client.py

@@ -2,17 +2,72 @@ import socket
 import base64
 import threading
 import time
+from typing import Optional, Tuple
 
 import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy
 from mavros_msgs.msg import RTCM
 
+# TODO: ROS_DOMAIN_ID 要補一下
+
+class GGA_stream():
+
+    @classmethod
+    def nmea_checksum(cls, body: str) -> str:
+        """body 不含 '$' 與 '*checksum'，例如 'GPGGA,123519,...'"""
+        value = 0
+        for ch in body:
+            value ^= ord(ch)
+        return f"{value:02X}"
+
+    @classmethod
+    def decimal_to_nmea_dm(cls, deg: float, *, is_latitude: bool) -> Tuple[str, str]:
+        """十進位度 → NMEA 的 (d)dmm.mmmm 與半球字元。"""
+        if is_latitude:
+            hemi = "N" if deg >= 0 else "S"
+            deg_width = 2
+        else:
+            hemi = "E" if deg >= 0 else "W"
+            deg_width = 3
+        deg = abs(deg)
+        d = int(deg)
+        m = (deg - d) * 60.0
+        return f"{d:0{deg_width}d}{m:07.4f}", hemi
+
+    @classmethod
+    def build_gga_sentence(cls, lat_deg: float, lon_deg: float, alt_m: float = 100.0) -> bytes:
+        """
+        組一筆 $GPGGA 句子（含 checksum) 回傳 bytes 可直接 sock.sendall。
+
+        lat_deg / lon_deg : 十進位經緯度(北、東為正)。
+        測試時請改成 mount 服務範圍內的近似位置；正式使用應來自 GNSS 真實定位。
+        """
+        utc = time.gmtime()
+        t_str = f"{utc.tm_hour:02d}{utc.tm_min:02d}{utc.tm_sec:02d}.00"
+
+        lat_dm, ns = cls.decimal_to_nmea_dm(lat_deg, is_latitude=True)
+        lon_dm, ew = cls.decimal_to_nmea_dm(lon_deg, is_latitude=False)
+
+        # quality=1（GPS fix）、8 顆星、HDOP=1.0 僅供測試示意
+        body = (
+            f"GPGGA,{t_str},{lat_dm},{ns},{lon_dm},{ew},"
+            f"1,08,1.0,{alt_m:.1f},M,0.0,M,,"
+        )
+        sentence = f"${body}*{cls.nmea_checksum(body)}\r\n"
+        return sentence.encode("ascii")
+
+    @classmethod
+    def send_gga(cls, sock: socket.socket, lat_deg: float, lon_deg: float, alt_m: float = 100.0) -> str:
+        """送出 GGA，回傳可讀句子供列印。"""
+        payload = cls.build_gga_sentence(lat_deg, lon_deg, alt_m)
+        sock.sendall(payload)
+        return payload.decode("ascii").strip()
 
 class NtripClientNode(Node):
-    """連線 NTRIP caster，接收 RTCM3 資料流並發布至 ROS2 topic。"""
+    """連線 NTRIP caster, 接收 RTCM3 資料流並發布至 ROS2 topic """
 
-    RECONNECT_BASE_SEC = 2.0
+    RECONNECT_BASE_SEC = 10.0
     RECONNECT_MAX_SEC = 60.0
 
     def __init__(self):
@@ -21,9 +76,15 @@ class NtripClientNode(Node):
         self.declare_parameter('host', 'rtk2go.com')
         self.declare_parameter('port', 2101)
         self.declare_parameter('mountpoint', '')
-        self.declare_parameter('username', '')
+        self.declare_parameter('username', 'template@mail.com')
         self.declare_parameter('password', '')
 
+        self.declare_parameter('gga_on', False)
+        self.declare_parameter('gga_lat', None)
+        self.declare_parameter('gga_lon', None)
+        self.declare_parameter('gga_alt_m', 100)
+        self.declare_parameter('gga_interval_sec', 60)
+
         rtcm_qos = QoSProfile(
             history=HistoryPolicy.KEEP_LAST,
             depth=1,
@@ -32,7 +93,9 @@ class NtripClientNode(Node):
         )
         self.publisher_ = self.create_publisher(RTCM, '/fc_network/rtcm_input', rtcm_qos)
 
-        self._running = True
+        self._stop_event = threading.Event()
+        self._sock_lock = threading.Lock()
+        self._active_sock: Optional[socket.socket] = None
         self._thread = threading.Thread(target=self._receive_loop, daemon=True)
         self._thread.start()
 
@@ -40,31 +103,50 @@ class NtripClientNode(Node):
 
     # ── NTRIP 連線 + RTCM 接收迴圈 ──────────────────────────────
 
+    def _request_stop(self):
+        self._stop_event.set()
+        with self._sock_lock:
+            if self._active_sock is not None:
+                try:
+                    self._active_sock.close()
+                except OSError:
+                    pass
+
     def _receive_loop(self):
         backoff = self.RECONNECT_BASE_SEC
 
-        while self._running:
-            host = self.get_parameter('host').value
-            port = self.get_parameter('port').value
-            mount = self.get_parameter('mountpoint').value
-            user = self.get_parameter('username').value
-            pwd = self.get_parameter('password').value
+        host = self.get_parameter('host').value
+        port = self.get_parameter('port').value
+        mountpoint = self.get_parameter('mountpoint').value
+        user = self.get_parameter('username').value
+        pwd = self.get_parameter('password').value
 
-            if not mount:
-                self.get_logger().error('mountpoint 參數未設定，等待重試...')
-                time.sleep(backoff)
-                continue
+        if not mountpoint:
+            self.get_logger().error('mountpoint 參數未設定...')
+            return
+
+        while not self._stop_event.is_set():
 
             sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
             sock.settimeout(10)
+            with self._sock_lock:
+                self._active_sock = sock
+
+            self.gga_lat = self.get_parameter('gga_lat').value
+            self.gga_lon = self.get_parameter('gga_lon').value
+            self.gga_alt_m = self.get_parameter('gga_alt_m').value
+            self.gga_interval_sec = self.get_parameter('gga_interval_sec').value
+            self.gga_on = self.get_parameter('gga_on').value
+            self.gga_on = self.gga_lat is not None and self.gga_lon is not None and self.gga_on
+
 
             try:
-                self.get_logger().info(f'正在連線 {host}:{port}/{mount} ...')
+                self.get_logger().info(f'正在連線 {host}:{port}/{mountpoint} ...')
                 sock.connect((host, port))
 
                 auth = base64.b64encode(f'{user}:{pwd}'.encode()).decode()
                 request = (
-                    f'GET /{mount} HTTP/1.0\r\n'
+                    f'GET /{mountpoint} HTTP/1.0\r\n'
                     f'User-Agent: NTRIP PythonClient\r\n'
                     f'Authorization: Basic {auth}\r\n'
                     f'Connection: close\r\n\r\n'
@@ -78,29 +160,54 @@ class NtripClientNode(Node):
                     )
                     raise ConnectionError('handshake failed')
 
-                self.get_logger().info(f'已連接至 {mount}，開始接收 RTCM 資料流')
+                self.get_logger().info(f'已成功連接至 {mountpoint}')
+
+
                 backoff = self.RECONNECT_BASE_SEC
 
                 self._read_stream(sock)
 
             except Exception as e:
-                if self._running:
-                    self.get_logger().warn(f'連線中斷: {e}，{backoff:.0f}s 後重連')
-                    time.sleep(backoff)
-                    backoff = min(backoff * 2, self.RECONNECT_MAX_SEC)
+                if self._stop_event.is_set():
+                    break
+                self.get_logger().warn(f'連線中斷: {e}，{backoff:.0f}s 後重連')
+                if self._stop_event.wait(timeout=backoff):
+                    break
+                backoff = min(backoff * 2, self.RECONNECT_MAX_SEC)
             finally:
-                sock.close()
+                with self._sock_lock:
+                    if self._active_sock is sock:
+                        self._active_sock = None
+                try:
+                    sock.close()
+                except OSError:
+                    pass
 
     def _read_stream(self, sock: socket.socket):
         buf = b''
+        last_gga_time = 0.0
+
+        while not self._stop_event.is_set():
+            now = time.time()
+            if self.gga_on and (now - last_gga_time >= self.gga_interval_sec):
+                line = GGA_stream.send_gga(sock, self.gga_lat, self.gga_lon, self.gga_alt_m)
+                self.get_logger().info(f"[GGA] {line}")
+                last_gga_time = now
 
-        while self._running:
             try:
                 chunk = sock.recv(4096)
             except socket.timeout:
+                if self._stop_event.is_set():
+                    break
                 continue
+            except OSError:
+                if self._stop_event.is_set():
+                    break
+                raise
             if not chunk:
-                break
+                if self._stop_event.is_set():
+                    break
+                raise ConnectionError("stream ended. Empty Chunk")
             buf += chunk
 
             while len(buf) >= 6:
@@ -126,8 +233,11 @@ class NtripClientNode(Node):
         self.publisher_.publish(msg)
 
     def destroy_node(self):
-        self._running = False
-        self._thread.join(timeout=3.0)
+        self._request_stop()
+        if self._thread.is_alive():
+            self._thread.join(timeout=5.0)
+            if self._thread.is_alive():
+                self.get_logger().warn('receive thread 未在時限內結束')
         super().destroy_node()
 
 

src/unitdev02/unitdev02/devnote.txt

@@ -8,7 +8,8 @@
  RTK
    跟 RTK2go 抓取列表 Done
    從特定 mount point 得到數據 Done
-   做一個 ros2 service 接到數據並包裝 
+   做一個 ros2 service 接到數據並包裝 Done
+   RTK 實機測試
  
 
 下一步
@@ -34,7 +35,11 @@ python -m fc_network_module.fc_network_module.ntrip_client --ros-args -p mountpo
 
 python -m fc_network_module.fc_network_module.ntrip_client --ros-args -p host:=210.241.63.193 -p port:=81 -p mountpoint:=2020_GNSS -p username:=uavlab6061 -p password:=iamsupersmart
 
-export ROS_DOMAIN_ID=13
+python3 -m fc_network_module.fc_network_module.ntrip_client --ros-args -p host:=210.241.63.193 -p port:=81 -p mountpoint:=2020_GNSS -p username:=uavlab6061 -p password:=iamsupersmart -p gga_on:=True -p gga_lat:=24.155792000 -p gga_lon:=120.630679000 -p gga_interval_sec:=60
+
+watch GPS_RAW_INT
+
+export ROS_DOMAIN_ID=1
 ros2 daemon stop
 ros2 daemon start