feat(GUI): 交戰高度分層改「跟著敵機高度 ±offset」浮動分層

修實飛重編隊後待機組 B 掉高度的 bug（原 _alt_for 寫死 formation_alt
沒讀敵機高度）。高度來源改用 local_pose z（相對 home，非 AMSL gps alt），
新增 rel_alt_provider / _enemy_base_alt（None 沿用上次有效值），
A=base+offset、B=base−offset；EngagementPanel 加「高度分層 ±」欄位即時生效。
SITL 驗證通過。

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

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

@@ -69,6 +69,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)
 
@@ -782,25 +794,34 @@ class DroneMonitor(Node):
         self.serial_receivers = []
         
         # ================================================================================
-        # 【新增】初始化 CommandLongClient 字典（為每個 drone 維護獨立的 client）
+        # 共用 command / position client（各一個節點，靠 request 的 target_sysid 路由）
         # ================================================================================
-        # 改為為每個 drone 創建獨立的 client，避免多機並行時的競態條件
+        # 兩個 client 都只打到單一 service（send_command_long / pos_global_int），
-        self.command_long_clients = {}  # {drone_id: CommandLongClient}
+        # 沒有任何 per-drone 狀態，故不需要 per-drone 節點。改成「啟動時各預建一個、
-        self.client_lock = Lock()  # 保護 clients 字典的訪問
+        # 執行期只重用」，避免：
-        self.client_counter = 0  # 用於生成唯一的 client 節點名稱
+        #   (1) 切 mode / 首次 goto 時於執行期新建 ROS2 node = 新 DDS participant，
-        self.executor = None  # 將在 gui.py 中設置，用於添加新的 clients
+        #       其 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（共用）
-        # PositionTargetGlobalIntClient 字典（per-drone，用於 Offboard goto）
+        self.position_target_client = None   # PositionTargetGlobalIntClient（共用）
-        # ================================================================================
+        self.client_lock = Lock()            # 保護共用 client 的建立
-        self.position_target_clients = {}  # {drone_id: PositionTargetGlobalIntClient}
+        self.executor = None                 # 將在 gui.py 中設置，用於 add_node
-        self.pos_client_counter = 0
         # ================================================================================
         
         # 主题检测定时器
         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_next_socket_id(self):
         """取得目前最小的未使用 socket_id（從 0 開始）。"""
         with self.socket_id_lock:
@@ -854,57 +875,69 @@ class DroneMonitor(Node):
             
             return self.socket_id_mapping[original_socket_id]
        
-    def get_or_create_client(self, drone_id):
+    def init_shared_command_clients(self):
-        """為每個 drone 獲取或創建獨立的 CommandLongClient，避免競態條件"""
+        """啟動時預建共用的 command / position client（各一個節點）。
+
+        必須在 _ros_spin_thread 啟動「之前」於主執行緒呼叫：此時 executor 已建立
+        但尚未被 spin，add_node 不會與 spin_once 跨執行緒競爭。之後執行期送指令
+        一律重用這兩個節點，不再於飛行中新建 DDS participant。"""
         with self.client_lock:
-            if drone_id not in self.command_long_clients:
+            if self.command_long_client is None and CommandLongClient is not None:
                 try:
-                    # 生成唯一的 client 節點名稱
+                    self.command_long_client = CommandLongClient(node_name="cmd_long_client_shared")
-                    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)
+                        self.executor.add_node(self.command_long_client)
-                        _log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器")
+                    _log("INFO", "已預建共用 CommandLongClient (node=cmd_long_client_shared)")
-                    
-                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}")
+                    _log("WARN", f"預建 CommandLongClient 失敗: {e}")
-                    return None
+            if self.position_target_client is None and PositionTargetGlobalIntClient is not None:
-            return self.command_long_clients[drone_id]
+                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_position_client(self, drone_id):
+    def get_or_create_client(self, drone_id=None):
-        """為每個 drone 獲取或創建獨立的 PositionTargetGlobalIntClient。"""
+        """回傳共用的 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
+                    self.position_target_client = PositionTargetGlobalIntClient(node_name="pos_target_client_shared")
-                    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})")
                     if self.executor:
-                        self.executor.add_node(client)
+                        self.executor.add_node(self.position_target_client)
-                        _log("INFO", f"已將 {drone_id} 的 PositionTargetGlobalIntClient 加入主執行器")
+                    _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 +1002,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, ...
@@ -1041,6 +1100,21 @@ class DroneMonitor(Node):
                 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 +1163,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 +1190,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 +1247,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 +1285,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 +1420,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:

src/GUI/gui.py

@@ -18,11 +18,9 @@ import re
 import threading
 from concurrent.futures import ThreadPoolExecutor
 
-
 def _log(level, message):
     print(f"[{level}] {message}", flush=True)
 
-
 # 導入分離的類別
 from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver
 from map_layout import DroneMap
@@ -40,6 +38,7 @@ from mission_group import (
     MissionGroup, GroupPanel, DroneAssignDialog, GROUP_COLORS,
     DEFAULT_MISSION_PARAM_VALUES
 )
+from mission_orchestrator import EngagementPanel, MissionOrchestrator, MissionPhase
 # ================================================================================
 
 
@@ -148,7 +147,7 @@ class ToggleSwitch(QWidget):
 class ControlStationUI(QMainWindow):
     planning_finished = pyqtSignal(object)
 
-    VERSION = '2.6.0'
+    VERSION = '2.7.1'
     FONT_SCALE_MIN = 70
     FONT_SCALE_MAX = 180
     FONT_SCALE_DEFAULT = 100
@@ -171,9 +170,14 @@ class ControlStationUI(QMainWindow):
         self.executor = rclpy.executors.SingleThreadedExecutor()
         self.executor.add_node(self.monitor)
         
-        # 將執行器註冊到 DroneMonitor，以便動態創建的 CommandLongClient 能被添加
+        # 將執行器註冊到 DroneMonitor，以便共用 command/position client 能被添加
         self.monitor.executor = self.executor
 
+        # 啟動時先預建共用 command/position client（必須在 spin thread 啟動前、主執行緒），
+        # 避免執行期（切 mode / 首次 goto）新建 DDS participant 觸發 discovery 風暴(OOM)
+        # 與跨執行緒 add_node race(CPU 100%)。
+        self.monitor.init_shared_command_clients()
+
         # 在背景執行緒處理 ROS2 spin，避免佔用 Qt 主執行緒時間
         self.ros_thread_running = True
         self.ros_thread = threading.Thread(target=self._ros_spin_thread, daemon=True)
@@ -211,8 +215,15 @@ class ControlStationUI(QMainWindow):
         self._attitude_cache = {}
         self._overview_cache = {}
         self._map_dirty_drones = set()
-        self.message_history = []
+        # 「紀錄」分頁分成三個獨立來源；message_history 保留為 GUI 操作紀錄的
+        # 相容別名，避免既有程式仍存取舊屬性時失效。
+        self.drone_message_history = []
+        self.gui_operation_history = []
+        self.fcnetwork_history = []
+        self.message_history = self.gui_operation_history
         self.max_message_history = 500
+        self._sys_diags_warning_signatures = {}
+        self._suppressed_status_message = None
         
         # 初始化UI
         self.drones = {}
@@ -249,6 +260,11 @@ class ControlStationUI(QMainWindow):
         self.overview_timer.timeout.connect(self._flush_overview_table)
         self.overview_timer.start(200)
 
+        # 交戰面板：敵機↔最近友機距離讀數（5Hz）
+        self.engagement_timer = QTimer()
+        self.engagement_timer.timeout.connect(self._update_engagement_panel)
+        self.engagement_timer.start(200)
+
         # 初始化連接列表
         self.udp_receivers = []
         self.udp_connections = []
@@ -341,6 +357,17 @@ class ControlStationUI(QMainWindow):
         self.settings_tab = self._create_settings_tab()
         self.left_tab.addTab(self.settings_tab, "設定")
 
+        # — 分頁 6：交戰（敵機選擇 + 即時距離 + 階段狀態機）
+        self.engagement_panel = EngagementPanel()
+        self.engagement_panel.enemy_changed.connect(self._on_enemy_changed)
+        self.engagement_panel.confirm_requested.connect(self._on_engagement_confirm)
+        self.engagement_panel.stop_requested.connect(self._on_engagement_stop)
+        self.engagement_panel.groups_changed.connect(self._on_engagement_groups_changed)
+        self.engagement_panel.apply_speed_requested.connect(self._on_engagement_apply_speed)
+        self.engagement_panel.alt_offset_changed.connect(self._on_engagement_alt_offset)
+        self.left_tab.addTab(self.engagement_panel, "交戰")
+        self._init_orchestrator()
+
         # 右侧容器
         right_container = QWidget()
         right_layout = QVBoxLayout(right_container)
@@ -448,19 +475,19 @@ class ControlStationUI(QMainWindow):
         self.statusBar().messageChanged.connect(self._on_status_bar_message_changed)
 
     def _create_message_history_tab(self):
-        """建立左側訊息歷史分頁。"""
+        """建立上、中、下三區的紀錄分頁。"""
         widget = QWidget()
         layout = QVBoxLayout(widget)
         layout.setContentsMargins(10, 10, 10, 10)
         layout.setSpacing(8)
 
         header_layout = QHBoxLayout()
-        title = QLabel("操作與訊息歷史")
+        title = QLabel("訊息與操作紀錄")
         title.setStyleSheet("color: #DDD; font-size: 14px; font-weight: bold;")
         header_layout.addWidget(title)
         header_layout.addStretch()
 
-        clear_btn = QPushButton("清空")
+        clear_btn = QPushButton("全部清空")
         clear_btn.setStyleSheet("""
             QPushButton { background-color: #555; color: white; border: none;
                 padding: 5px 10px; border-radius: 4px; font-size: 12px; }
@@ -470,9 +497,7 @@ class ControlStationUI(QMainWindow):
         header_layout.addWidget(clear_btn)
         layout.addLayout(header_layout)
 
-        self.message_history_view = QPlainTextEdit()
+        history_style = """
-        self.message_history_view.setReadOnly(True)
-        self.message_history_view.setStyleSheet("""
             QPlainTextEdit {
                 background-color: #1E1E1E;
                 color: #DDD;
@@ -482,9 +507,67 @@ class ControlStationUI(QMainWindow):
                 font-family: monospace;
                 font-size: 12px;
             }
-        """)
+        """
-        self.message_history_view.setPlaceholderText("左下角狀態訊息會顯示在這裡...")
+
-        layout.addWidget(self.message_history_view)
+        def create_section(section_title, category):
+            section = QWidget()
+            section_layout = QVBoxLayout(section)
+            section_layout.setContentsMargins(0, 0, 0, 0)
+            section_layout.setSpacing(4)
+
+            section_header = QHBoxLayout()
+            section_label = QLabel(section_title)
+            section_label.setStyleSheet(
+                "color: #CCC; font-size: 12px; font-weight: bold;")
+            section_header.addWidget(section_label)
+            section_header.addStretch()
+
+            section_clear_btn = QPushButton("清空")
+            section_clear_btn.setStyleSheet("""
+                QPushButton { background-color: #444; color: #DDD; border: none;
+                    padding: 3px 8px; border-radius: 3px; font-size: 11px; }
+                QPushButton:hover { background-color: #555; }
+            """)
+            section_clear_btn.clicked.connect(
+                lambda _checked=False, name=category:
+                    self._clear_message_history(name)
+            )
+            section_header.addWidget(section_clear_btn)
+            section_layout.addLayout(section_header)
+
+            view = QPlainTextEdit()
+            view.setReadOnly(True)
+            view.setStyleSheet(history_style)
+            view.document().setMaximumBlockCount(self.max_message_history)
+            section_layout.addWidget(view)
+            return section, view
+
+        history_splitter = QSplitter(Qt.Orientation.Vertical)
+        history_splitter.setChildrenCollapsible(False)
+
+        gui_section, self.gui_operation_history_view = create_section(
+            "操作紀錄", "gui")
+        drone_section, self.drone_message_history_view = create_section(
+            "無人機狀態", "drone")
+        fcnetwork_section, self.fcnetwork_history_view = create_section(
+            "FC Network", "fcnetwork")
+
+        history_splitter.addWidget(gui_section)
+        history_splitter.addWidget(drone_section)
+        history_splitter.addWidget(fcnetwork_section)
+        history_splitter.setStretchFactor(0, 1)
+        history_splitter.setStretchFactor(1, 1)
+        history_splitter.setStretchFactor(2, 1)
+        history_splitter.setSizes([220, 220, 220])
+        layout.addWidget(history_splitter)
+
+        self._history_views = {
+            'drone': self.drone_message_history_view,
+            'gui': self.gui_operation_history_view,
+            'fcnetwork': self.fcnetwork_history_view,
+        }
+        # 舊名稱指向 GUI 操作紀錄，保留向後相容性。
+        self.message_history_view = self.gui_operation_history_view
 
         return widget
 
@@ -741,27 +824,114 @@ class ControlStationUI(QMainWindow):
 
         return super().eventFilter(obj, event)
 
-    def _clear_message_history(self):
+    def _history_list(self, category):
-        """清空訊息歷史。"""
+        return {
-        self.message_history.clear()
+            'drone': self.drone_message_history,
-        if hasattr(self, 'message_history_view'):
+            'gui': self.gui_operation_history,
-            self.message_history_view.clear()
+            'fcnetwork': self.fcnetwork_history,
+        }.get(category)
 
-    def _on_status_bar_message_changed(self, message):
+    def _append_history(self, category, message):
-        """同步狀態列訊息到歷史紀錄。"""
+        """加入指定來源的紀錄並讓畫面保持在最新一筆。"""
         if not message:
             return
+
         timestamp = time.strftime("%H:%M:%S")
         entry = f"[{timestamp}] {message}"
-        self.message_history.append(entry)
+        history = self._history_list(category)
-        if len(self.message_history) > self.max_message_history:
+        if history is None:
-            self.message_history = self.message_history[-self.max_message_history:]
+            return
+        history.append(entry)
+        if len(history) > self.max_message_history:
+            del history[:-self.max_message_history]
 
-        if hasattr(self, 'message_history_view'):
+        view = getattr(self, '_history_views', {}).get(category)
-            self.message_history_view.appendPlainText(entry)
+        if view is not None:
-            scrollbar = self.message_history_view.verticalScrollBar()
+            view.appendPlainText(entry)
+            scrollbar = view.verticalScrollBar()
             scrollbar.setValue(scrollbar.maximum())
 
+    def _clear_message_history(self, category=None):
+        """清空單一來源；未指定來源時清空全部。"""
+        categories = (category,) if category else ('drone', 'gui', 'fcnetwork')
+        for name in categories:
+            history = self._history_list(name)
+            if history is not None:
+                history.clear()
+            view = getattr(self, '_history_views', {}).get(name)
+            if view is not None:
+                view.clear()
+
+    def _record_drone_status(self, sys_id, data):
+        """顯示由 status_text topic 收到的飛行檢查與報錯。"""
+        severity_labels = {
+            0: 'EMERGENCY', 1: 'ALERT', 2: 'CRITICAL', 3: 'ERROR',
+            4: 'WARNING', 5: 'NOTICE', 6: 'INFO', 7: 'DEBUG'
+        }
+        severity = data.get('severity', -1)
+        label = severity_labels.get(severity, 'STATUS')
+        self._append_history(
+            'drone', f"/fc_network/vehicle/{sys_id}/status_text "
+                     f"[{label}] {data.get('text', '')}")
+
+    def _record_sys_diags_warning(self, sys_id, data):
+        """sys_diags 只在診斷值異常或內容改變時顯示警告。"""
+        installed = int(data.get('sensors_install_mask', 0))
+        enabled = int(data.get('sensors_enabled_mask', 0))
+        healthy = int(data.get('sensors_health_mask', 0))
+        unhealthy_mask = installed & enabled & ~healthy
+        load = float(data.get('mcu_load_percent', 0.0))
+        bus_rate = float(data.get('bus_error_rate_percent', 0.0))
+        error_counts = tuple(int(data.get(f'errors_count{i}', 0)) for i in range(1, 5))
+
+        warnings = []
+        if unhealthy_mask:
+            warnings.append(f"感測器異常 mask=0x{unhealthy_mask:08X}")
+        if load >= 90.0:
+            warnings.append(f"MCU 負載過高 {load:.1f}%")
+        if bus_rate > 0.0 or int(data.get('bus_error_count', 0)) > 0:
+            warnings.append(
+                f"匯流排錯誤率 {bus_rate:.1f}% / "
+                f"count={int(data.get('bus_error_count', 0))}")
+        if any(error_counts):
+            warnings.append(f"錯誤計數={error_counts}")
+
+        signature = tuple(warnings)
+        if not signature:
+            self._sys_diags_warning_signatures.pop(sys_id, None)
+            return
+        if self._sys_diags_warning_signatures.get(sys_id) == signature:
+            return
+        self._sys_diags_warning_signatures[sys_id] = signature
+        self._append_history(
+            'drone', f"/fc_network/vehicle/{sys_id}/sys_diags [WARNING] "
+                     + "；".join(warnings))
+
+    def _record_fcnetwork_log(self, logger_name, data):
+        """顯示 /fc_network/logs 的結構化 FC Network 紀錄。"""
+        level_labels = {
+            10: 'DEBUG', 20: 'INFO', 30: 'WARN', 40: 'ERROR', 50: 'FATAL'
+        }
+        level = level_labels.get(data.get('level'), str(data.get('level', '')))
+        context = []
+        if data.get('event_code'):
+            context.append(data['event_code'])
+        if data.get('sysid', -1) >= 0:
+            context.append(f"sys{data['sysid']}")
+        context_text = f" [{' / '.join(context)}]" if context else ''
+        self._append_history(
+            'fcnetwork', f"/fc_network/logs [{level}] [{logger_name}]"
+                         f"{context_text} {data.get('message', '')}")
+
+    def _on_status_bar_message_changed(self, message):
+        """同步 GUI 狀態列訊息到操作紀錄。"""
+        if not message:
+            return
+        if message == self._suppressed_status_message:
+            self._suppressed_status_message = None
+            return
+        self._append_history('gui', message)
+
     def _setup_stream_redirector(self):
         """將 stdout/stderr 同步到左下角狀態列與訊息紀錄。"""
         self._original_stdout = sys.stdout
@@ -783,10 +953,14 @@ class ControlStationUI(QMainWindow):
             sys.stderr = self._original_stderr
 
     def show_in_bottom_left(self, text):
-        """將重導向的輸出顯示在左下角狀態列。"""
+        """背景輸出只短暫顯示在狀態列，不混入三類紀錄。"""
         if not text:
             return
+        # 背景輸出仍沿用既有狀態列提示，但避免又被當成 GUI 操作紀錄。
+        self._suppressed_status_message = text
         self.statusBar().showMessage(text, 5000)
+        if self._suppressed_status_message == text:
+            self._suppressed_status_message = None
     
 
     # ================================================================================
@@ -1323,6 +1497,7 @@ class ControlStationUI(QMainWindow):
             arrival_radius=exec_params.get('arrival_radius', 4.0),
             hover_stable_sec=exec_params.get('hover_stable_sec', 2.0),
             tick_rate_hz=2.0,
+            resend_interval_sec=exec_params.get('resend_interval_sec', 2.5),
         )
         executor.drone_waypoint_reached.connect(self.on_drone_waypoint_reached)
         executor.task_status_changed.connect(self._on_task_status_changed)
@@ -1399,26 +1574,29 @@ class ControlStationUI(QMainWindow):
         _log("INFO", f"Group {group_id} 準備切換模式為 {mode}")
         self.statusBar().showMessage("正在切換模式", 1000)
         
-        # 使用 asyncio 執行（通過事件循環）
+        # 使用 asyncio 執行（通過事件循環）。
+        # 並發：每台各自 set_mode + 各自等自己的 ACK，用 gather 同時發，
+        # 避免逐台 await 造成序列化（最壞情況 N×timeout 一路卡著）。
+        async def _change_one(drone_id):
+            try:
+                result = await self.monitor.set_mode(drone_id, mode)
+                if result:
+                    msg = f"{drone_id} 切換成功"
+                    _log("INFO", msg)
+                else:
+                    msg = f"{drone_id} 切換失敗"
+                    _log("WARN", msg)
+                self.message_queue.put((msg, 2000))
+            except Exception as e:
+                msg = f"{drone_id} 錯誤: {str(e)}"
+                _log("ERROR", msg)
+                traceback.print_exc()
+                self.message_queue.put((msg, 2000))
+
         async def do_mode_changes_async():
-            for drone_id in group.selected_drone_ids:
+            await asyncio.gather(
-                try:
+                *(_change_one(did) for did in group.selected_drone_ids)
-                    result = await self.monitor.set_mode(drone_id, mode)
+            )
-                    
-                    if result:
-                        msg = f"{drone_id} 切換成功"
-                        _log("INFO", msg)
-                        self.message_queue.put((msg, 2000))
-                    else:
-                        msg = f"{drone_id} 切換失敗"
-                        _log("WARN", msg)
-                        self.message_queue.put((msg, 2000))
-                
-                except Exception as e:
-                    msg = f"{drone_id} 錯誤: {str(e)}"
-                    _log("ERROR", msg)
-                    traceback.print_exc()
-                    self.message_queue.put((msg, 2000))
 
         # 通過事件循環提交異步任務
         loop = asyncio.get_event_loop()
@@ -1660,6 +1838,15 @@ class ControlStationUI(QMainWindow):
 
     def update_ui(self, msg_type, drone_id, data):
         """只做數據快取，不在這裡更新 UI"""
+        if msg_type == 'status_text':
+            self._record_drone_status(drone_id, data)
+            return
+        if msg_type == 'sys_diags':
+            self._record_sys_diags_warning(drone_id, data)
+            return
+        if msg_type == 'fc_network_log':
+            self._record_fcnetwork_log(drone_id, data)
+            return
         if msg_type == 'connection_type':
             conn_type = data.get('type', 'Unknown')
             parts = drone_id.split('_')
@@ -2133,14 +2320,40 @@ class ControlStationUI(QMainWindow):
     # 任務執行回呼
     # ================================================================================
 
+    # 任務狀態列的通訊狀態短標籤（對應 MissionExecutor 的 TaskStatus.value）
+    _MISSION_COMM_LABELS = {
+        "normal": "送出OK",
+        "retrying": "送出未驗證",
+        "waiting_at_barrier": "等待同伴",
+        "fallback_loiter": "LOITER",
+    }
+
+    def _set_overview_mission(self, drone_id, wp=None, comm=None):
+        """組合 WP 進度 + 通訊狀態，寫進總覽表『任務狀態』列（每台一格，實飛一眼看全部）。"""
+        cache = getattr(self, '_mission_ui', None)
+        if cache is None:
+            cache = self._mission_ui = {}
+        slot = cache.setdefault(drone_id, {'wp': '', 'comm': ''})
+        if wp is not None:
+            slot['wp'] = wp
+        if comm is not None:
+            slot['comm'] = comm
+        text = ' · '.join(p for p in (slot['wp'], slot['comm']) if p) or '--'
+        self.update_overview_table(drone_id, 'mission', text)
+
     def on_drone_waypoint_reached(self, drone_id, wp_index, total):
         if wp_index >= total:
             self.statusBar().showMessage(f"{drone_id} 已完成所有航點", 3000)
+            self._set_overview_mission(drone_id, wp=f"完成 {total}/{total}")
         else:
             self.statusBar().showMessage(f"{drone_id} 到達 WP {wp_index}/{total}", 2000)
+            self._set_overview_mission(drone_id, wp=f"WP {wp_index}/{total}")
 
     def _on_task_status_changed(self, drone_id, status, message):
-        """MissionExecutor.task_status_changed slot：把 goto 失敗/重試/fallback/barrier 丟到 status bar"""
+        """MissionExecutor.task_status_changed slot：狀態列提示 + 更新總覽表『任務狀態』列"""
+        comm = self._MISSION_COMM_LABELS.get(status)
+        if comm is not None:
+            self._set_overview_mission(drone_id, comm=comm)
         if status == "retrying":
             self.statusBar().showMessage(f"{drone_id} {message}", 4000)
         elif status == "fallback_loiter":
@@ -2275,6 +2488,152 @@ class ControlStationUI(QMainWindow):
         match = re.match(r's(\d+)_(\d+)', drone_id)
         return match.group(1) if match else 'unknown'
 
+    # ================================================================
+    # 交戰任務編排器（MissionOrchestrator）接線
+    # ================================================================
+    _PHASE_LABELS = {
+        MissionPhase.IDLE.value: "待機（尚未設定敵機/A 組）",
+        MissionPhase.WATCH.value: "監看偵測中",
+        MissionPhase.DETECTED.value: "已偵測到敵機 — 等待確認重編隊",
+        MissionPhase.PURSUE.value: "前進中（4 機 leader-follow）",
+        MissionPhase.ENCIRCLE.value: "包圍中",
+    }
+
+    def _init_orchestrator(self):
+        """建立 MissionOrchestrator，用 callback 與 GUI 解耦。"""
+        self.orchestrator = MissionOrchestrator(
+            make_executor=self._make_orchestrator_executor,
+            gps_provider=lambda: self.monitor.drone_gps,
+            # 敵機相對 home 高度（local_pose z，與 setpoint 同座標系）；勿用 drone_gps.alt（AMSL）
+            rel_alt_provider=lambda did: (
+                getattr(self.monitor, 'drone_local', {}).get(did) or {}).get('z'),
+            stop_groups_fn=self._stop_engagement_group_missions,
+        )
+        self._engagement_a_group = None
+        self._engagement_b_group = None
+        self.orchestrator.phase_changed.connect(self._on_orchestrator_phase_changed)
+        self.orchestrator.detection_prompt.connect(self._on_orchestrator_detection)
+        self.orchestrator.status_message.connect(
+            lambda msg: self.statusBar().showMessage(msg, 4000))
+
+    def _make_orchestrator_executor(self):
+        """為編排器建立 MissionExecutor（hover-hold：到達不自動結束，靠瞬時 slot 持續更新）。"""
+        ex = MissionExecutor(
+            sender=self.command_sender,
+            drone_gps=self.monitor.drone_gps,
+            monitor=self.monitor,
+            tick_rate_hz=2.0,
+            complete_stops=False,
+        )
+        ex.task_status_changed.connect(self._on_task_status_changed)
+        return ex
+
+    def _engagement_group_drone_ids(self, group_id):
+        """回傳指定 group 已分派的無人機清單（排序，穩定 rank）。"""
+        group = self.mission_groups.get(group_id)
+        if not group:
+            return []
+        return sorted(group.selected_drone_ids)
+
+    def _stop_engagement_group_missions(self):
+        """重編隊時，停掉 A/B 群組的掃描/待機任務，避免與合併 executor 雙重送點。"""
+        for gid in (self._engagement_a_group, self._engagement_b_group):
+            if gid and gid in self.mission_groups:
+                self._handle_group_stop(gid)
+
+    def _sync_orchestrator_config(self):
+        """把面板上的敵機 + A/B 組同步進 orchestrator（監看階段可連續更新）。"""
+        if not hasattr(self, 'orchestrator'):
+            return
+        a_ids = self._engagement_group_drone_ids(self._engagement_a_group) \
+            if self._engagement_a_group else []
+        b_ids = self._engagement_group_drone_ids(self._engagement_b_group) \
+            if self._engagement_b_group else []
+        enemy = self.engagement_panel.current_enemy()
+        self.orchestrator.configure(a_ids, b_ids, enemy)
+
+    def _update_engagement_panel(self):
+        """交戰面板週期更新：刷新群組清單、距離讀數、同步設定、驅動 orchestrator tick。"""
+        if not hasattr(self, 'engagement_panel'):
+            return
+        labels = [(gid, g.display_name) for gid, g in self.mission_groups.items()]
+        self.engagement_panel.refresh_group_list(labels)  # 內部只在變動時重建
+        self.engagement_panel.tick(self.drone_positions)
+        if hasattr(self, 'orchestrator'):
+            self._sync_orchestrator_config()
+            self.orchestrator.tick()
+
+    def _on_enemy_changed(self, enemy_id):
+        """敵機選擇改變的回呼。"""
+        self._sync_orchestrator_config()
+        if enemy_id:
+            self.statusBar().showMessage(f"已指定敵機：{enemy_id}", 3000)
+        else:
+            self.statusBar().showMessage("已清除敵機指定", 3000)
+
+    def _on_engagement_groups_changed(self, a_id, b_id):
+        """A/B 組指派改變 → 同步到 orchestrator。"""
+        self._engagement_a_group = a_id or None
+        self._engagement_b_group = b_id or None
+        self._sync_orchestrator_config()
+
+    def _on_engagement_apply_speed(self, friendly_speed, enemy_ratio):
+        """套用速度比例：友方 A+B 各機下 DO_CHANGE_SPEED = V，敵機 = ratio×V。
+        同步 orchestrator.friendly_speed，讓前置攔截 lead_time 用同一速度基準。"""
+        enemy_speed = friendly_speed * enemy_ratio
+        friendly_ids = []
+        for gid in (self._engagement_a_group, self._engagement_b_group):
+            friendly_ids.extend(self._engagement_group_drone_ids(gid) if gid else [])
+        enemy_id = self.engagement_panel.current_enemy()
+        if not friendly_ids and not enemy_id:
+            self.statusBar().showMessage("尚未指派 A/B 組或敵機，無法套用速度", 3000)
+            return
+        if hasattr(self, 'orchestrator'):
+            self.orchestrator.friendly_speed = float(friendly_speed)
+
+        async def _apply_one(did, speed, label):
+            try:
+                await self.monitor.set_speed(did, speed)
+            except Exception as e:
+                _log("ERROR", f"[套用速度] {label} {did} 失敗: {e}")
+
+        async def do_apply():
+            # 並發：友機各自 + 敵機，用 gather 同時發，各自等各自 ACK
+            tasks = [_apply_one(did, friendly_speed, "友機") for did in friendly_ids]
+            if enemy_id:
+                tasks.append(_apply_one(enemy_id, enemy_speed, "敵機"))
+            await asyncio.gather(*tasks)
+
+        asyncio.run_coroutine_threadsafe(do_apply(), asyncio.get_event_loop())
+        self.statusBar().showMessage(
+            f"套用速度：友機 {friendly_speed:.1f} m/s ({len(friendly_ids)} 台)、"
+            f"敵機 {enemy_speed:.1f} m/s", 4000)
+
+    def _on_engagement_alt_offset(self, offset):
+        """高度分層偏移即時生效：A=敵機高度+offset、B=敵機高度−offset。
+        下個 tick 的瞬時 slot 就會套用（PURSUE/ENCIRCLE 中調整也立即反映）。"""
+        if hasattr(self, 'orchestrator'):
+            self.orchestrator.alt_layer_offset = float(offset)
+            self.statusBar().showMessage(f"高度分層 ±{offset:.1f} m（A 上層 / B 下層）", 3000)
+
+    def _on_engagement_confirm(self):
+        if hasattr(self, 'orchestrator'):
+            self.orchestrator.confirm_detection()
+
+    def _on_engagement_stop(self):
+        if hasattr(self, 'orchestrator'):
+            self.orchestrator.stop()
+
+    def _on_orchestrator_phase_changed(self, phase_value):
+        """階段變更：更新面板文字與『確認重編隊』按鈕致能。"""
+        self.engagement_panel.set_phase_text(
+            self._PHASE_LABELS.get(phase_value, phase_value))
+        self.engagement_panel.set_confirm_enabled(
+            phase_value == MissionPhase.DETECTED.value)
+
+    def _on_orchestrator_detection(self, message):
+        self.statusBar().showMessage(message, 6000)
+
     def add_drone(self, drone_id):
         if drone_id in self.drones: return
         socket_id = self.get_socket_id(drone_id)
@@ -2287,6 +2646,9 @@ class ControlStationUI(QMainWindow):
         self.update_overview_table()
         # 同步新 drone 到 UI
         self.refresh_selection_ui()
+        # 同步敵機下拉選單（保留當前選擇）
+        if hasattr(self, 'engagement_panel'):
+            self.engagement_panel.refresh_drone_list(list(self.drones.keys()))
 
     def reorganize_socket_groups(self):
         while self.drone_panel_layout.count():
@@ -2384,9 +2746,10 @@ class ControlStationUI(QMainWindow):
                     
                     elif msg_type == 'local_pose':
                         x, y = data.get('x', 0), data.get('y', 0)
+                        z = data.get('z', 0)  # 相對 home 高度（正向朝上），供交戰高度分層用
                         if not hasattr(self.monitor, 'drone_local'):
                             self.monitor.drone_local = {}
-                        self.monitor.drone_local[drone_id] = {'x': x, 'y': y}
+                        self.monitor.drone_local[drone_id] = {'x': x, 'y': y, 'z': z}
                         self.queue_overview_update(drone_id, 'local', f"{x:.1f}, {y:.1f}")
                     
                     elif msg_type == 'loss_rate':
@@ -2593,18 +2956,14 @@ class ControlStationUI(QMainWindow):
             # Clean up serial receivers
             for receiver in self.monitor.serial_receivers:
                 receiver.stop()
-            # Clean up all CommandLongClient instances
+            # Clean up shared CommandLongClient / PositionTargetGlobalIntClient
-            for drone_id, client in self.monitor.command_long_clients.items():
+            for client in (getattr(self.monitor, 'command_long_client', None),
-                try:
+                           getattr(self.monitor, 'position_target_client', None)):
-                    client.destroy_node()
+                if client is not None:
-                except:
+                    try:
-                    pass
+                        client.destroy_node()
-            # Clean up all PositionTargetGlobalIntClient instances
+                    except:
-            for drone_id, client in getattr(self.monitor, 'position_target_clients', {}).items():
+                        pass
-                try:
-                    client.destroy_node()
-                except:
-                    pass
             self.monitor.destroy_node()
             self.executor.shutdown()
         except Exception as e:

src/GUI/map_layout.py

@@ -377,6 +377,8 @@ class DroneMap:
                 var clickStartPos = null;
                 var lastManualMapMoveAt = 0;
                 var autoCenteringMap = false;
+                const autoFollowPanThresholdRatio = 0.25;
+                var lastAutoFollowPanAt = 0;
 
                 function markManualMapMove() {
                     if (!autoCenteringMap) {
@@ -386,6 +388,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 = [];
                 var routeMarkers = [];
@@ -738,13 +776,8 @@ class DroneMap:
 
                 setInterval(() => {
                     if (focusedId && markers[focusedId]) {
-                        if (Date.now() - lastManualMapMoveAt < 1000) {
-                            return;
-                        }
                         var latlng = markers[focusedId].getLatLng();
-                        autoCenteringMap = true;
+                        autoFollowPanTo(latlng);
-                        map.panTo(latlng);
-                        setTimeout(() => { autoCenteringMap = false; }, 300);
                     }
                 }, 1000);
 
@@ -760,6 +793,9 @@ class DroneMap:
                             .setLatLng([lat, lon])
                             .setRotationAngle(heading);
                         idLabels[id].setLatLng([lat, lon]);
+                        if (id === focusedId) {
+                            autoFollowPanTo(markers[id].getLatLng());
+                        }
                     } else {
                         initTrajectory(id);
                         addTrajectoryPoint(id, lat, lon);

src/GUI/mission_executor.py

@@ -3,14 +3,19 @@
 任務執行模組
 管理多架無人機的 GUIDED 模式飛行控制迴圈
 
-設計:
+設計 (閉環 / closed-loop):
     - 每架無人機持有一個航點序列，逐點推進
     - 各自到達就各自切換到下一個航點
-    - 事件驅動發送：航點切換時送一次，收到 send_result 才決定重送/前進
+    - 閉環發送：以固定週期「持續重送」當前目標 setpoint（GUIDED 位置目標冪等），
-    - 失敗策略 (b)：單機重試 MAX_RETRY 次仍失敗 → 該機 fallback LOITER，其他機繼續
+      掉包會在下個週期自癒；換點完全靠 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}")
+        task.status = TaskStatus.RETRYING  # 純觀測用途，Phase 1/3 不會因此停飛
-
+        _log(
-        if task.fail_count < self.MAX_RETRY:
+            "WARN",
-            task.status = TaskStatus.RETRYING
+            f"{drone_id} 送出未驗證 x{task.fail_count}（指令已送達飛控，由 GPS 續飛）: {message}",
-            task.sent_current_wp = False  # 下個 tick 會重送
+        )
-            self.task_status_changed.emit(
+        self.task_status_changed.emit(
-                drone_id, task.status.value,
+            drone_id, task.status.value,
-                f"retry {task.fail_count}/{self.MAX_RETRY}: {message}"
+            f"send unverified x{task.fail_count} (GPS 續飛): {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)
 
     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_orchestrator.py

@@ -0,0 +1,752 @@
+#!/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 = 15.0        # 接戰半徑 (m)：形心↔敵 ≤ 此值 → 切包圍
+DEFAULT_R_ENGAGE_EXIT = 20.0   # 包圍退出半徑 (m)：> 此值 → 退回前進（hysteresis）
+DEFAULT_REPLAN_MOVE_M = 2.0    # 敵機位移 ≥ 此值即重算（moving target）
+DEFAULT_REPLAN_CAP_SEC = 1.0   # 重算週期上限 (s)
+DEFAULT_CIRCLE_RADIUS = 10.0   # 包圍半徑 (m)
+DEFAULT_LON_SPACING = 5.0      # 隊形縱向間距 (m)
+DEFAULT_LAT_OFFSET = 3.0       # 隊形橫向錯開 (m)
+DEFAULT_FORMATION_ALT = 10.0   # fallback 隊形基準高度 (m，相對 home)：敵機相對高度都讀不到時才用
+DEFAULT_ALT_LAYER_OFFSET = 5.0 # A/B 相對敵機高度的上下偏移 (m)：A=敵高+offset、B=敵高−offset
+DEFAULT_MIN_ALT_FLOOR = 5.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'}}（alt 是 AMSL，勿當相對高度用）
+      rel_alt_provider(drone_id) -> 該機相對 home 的高度 z（local_pose，正向朝上）或 None
+      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,
+                 rel_alt_provider=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_offset=DEFAULT_ALT_LAYER_OFFSET,
+                 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._rel_alt_provider = rel_alt_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_offset = float(alt_layer_offset)
+        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
+        self._last_enemy_rel_z = None   # 上次有效的敵機相對高度（provider 暫讀不到時沿用）
+
+    # ------------------------------------------------------------- 設定（可連續呼叫）
+    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 _enemy_base_alt(self):
+        """分層基準 = 敵機當下『相對 home』高度（來自 local_pose z，與 setpoint 同座標系）。
+        provider 暫讀不到（None）時沿用上一次有效值；連一次都沒有才退回 formation_alt。
+        注意：不可用 gps_provider 的 alt（那是 AMSL 海拔，座標系不同會炸高）。"""
+        z = None
+        if self._rel_alt_provider is not None and self.enemy_id is not None:
+            try:
+                z = self._rel_alt_provider(self.enemy_id)
+            except Exception:
+                z = None
+        if z is not None:
+            self._last_enemy_rel_z = float(z)
+            return float(z)
+        if self._last_enemy_rel_z is not None:
+            return self._last_enemy_rel_z
+        return self.formation_alt
+
+    def _alt_for(self, did, base):
+        """S3 高度分層：以敵機高度 base 為中心，A 組（含領隊）走上層 base+offset、
+        B 組走下層 base−offset（不低於地面下限）。分層後 A/B 垂直錯開避免同高交叉撞機。"""
+        if did in set(self.b_drone_ids):
+            return max(self.min_alt_floor, base - self.alt_layer_offset)
+        return base + self.alt_layer_offset
+
+    # ------------------------------------------------------------- 前置攔截（估敵速）
+    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]  # 左垂直
+        base = self._enemy_base_alt()  # 高度分層基準：敵機相對高度（每 tick 取一次）
+        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, base))
+        return slots
+
+    def _circle_slots(self, gps, epos):
+        """繞敵機的瞬時包圍：N 機均分在半徑 circle_radius 的圓周上。"""
+        ids = self._merged_ids
+        elat, elon = epos['lat'], epos['lon']
+        n = len(ids)
+        base = self._enemy_base_alt()  # 高度分層基準：敵機相對高度（每 tick 取一次）
+        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, base))
+        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)
+    alt_offset_changed = pyqtSignal(float)  # A/B 相對敵機高度的上下偏移 (m)
+
+    def __init__(self, parent=None, r_detect=DEFAULT_R_DETECT,
+                 friendly_speed=DEFAULT_FRIENDLY_SPEED,
+                 alt_offset=DEFAULT_ALT_LAYER_OFFSET):
+        super().__init__(parent)
+        self.r_detect = float(r_detect)
+        self._init_friendly_speed = float(friendly_speed)
+        self._init_alt_offset = float(alt_offset)
+        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)
+
+        # 高度分層：A=敵機高度+offset、B=敵機高度−offset（即時生效，不需重編隊）
+        alt_row = QHBoxLayout()
+        alt_row.setSpacing(8)
+        altl = QLabel("高度分層 ±：")
+        altl.setStyleSheet("color: #CCC; font-size: 12px;")
+        self.alt_offset_spin = QDoubleSpinBox()
+        self.alt_offset_spin.setStyleSheet(spin_css)
+        self.alt_offset_spin.setRange(0.0, 30.0)
+        self.alt_offset_spin.setSingleStep(0.5)
+        self.alt_offset_spin.setDecimals(1)
+        self.alt_offset_spin.setSuffix(" m")
+        self.alt_offset_spin.setValue(self._init_alt_offset)
+        self.alt_offset_spin.valueChanged.connect(
+            lambda v: self.alt_offset_changed.emit(float(v)))
+        alt_hint = QLabel("（A 走上層 / B 走下層，繞敵機高度）")
+        alt_hint.setStyleSheet("color: #777; font-size: 11px;")
+        alt_row.addWidget(altl)
+        alt_row.addWidget(self.alt_offset_spin)
+        alt_row.addWidget(alt_hint)
+        alt_row.addStretch()
+        root.addLayout(alt_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/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,
+        "mission": 0,
-        "armed": 1,
+        "mode": 1,
-        "battery": 2,
+        "armed": 2,
-        "longitude": 3,
+        "battery": 3,
-        "latitude": 4,
+        "longitude": 4,
-        "fix_type": 5,
+        "latitude": 5,
-        "satellites_visible": 6,
+        "fix_type": 6,
-        "eph": 7,
+        "satellites_visible": 7,
-        "epv": 8,
+        "eph": 8,
-        "altitude": 9,
+        "epv": 9,
-        "local": 10,
+        "altitude": 10,
-        "velocity": 11,
+        "local": 11,
-        "groundspeed": 12,
+        "velocity": 12,
-        "heading": 13,
+        "groundspeed": 13,
-        "airspeed": 14,
+        "heading": 14,
-        "throttle": 15,
+        "airspeed": 15,
-        "hud_alt": 16,
+        "throttle": 16,
-        "climb": 17,
+        "hud_alt": 17,
-        "roll": 18,
+        "climb": 18,
-        "pitch": 19,
+        "roll": 19,
-        "yaw": 20,
+        "pitch": 20,
-        "loss_rate": 21,
+        "yaw": 21,
-        "ping": 22
+        "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)
@@ -76,6 +82,10 @@ class OverviewTable(QTableWidget):
             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}")
+                if field == 'mission':
-                val = lbl.text() if lbl else "--"
+                    # 任務狀態非遙測，取自獨立保存，避免被 "--" 蓋掉
+                    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/fc_interfaces/CMakeLists.txt

@@ -15,6 +15,7 @@ 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_network_adapter/fc_network_adapter/mainOrchestrator.py

@@ -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"),
@@ -1595,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,
             # 新增區
         }
@@ -1638,25 +1644,7 @@ class Orchestrator:
 
 def main():
 
-    # =========== 各項模組的版本先驗 ===========
+    logger.info(f"Each Module Running Version at mavlinkObkect:{mo.MODULE_VER}, mavlinkROS2Nodes:{mros.MODULE_VER}, mavlinkVehicleView:{mvv.MODULE_VER}, serialManager:{sm.MODULE_VER}")
-    # 除非你有在做這幾項模組的改版 不然動到這邊的版本號 代表執行環境有很大的問題!!!!!!
-    version_check = True
-    if mo.MODULE_VER != "1.50":
-        print("Module Version Error! : mavlinkObkect")
-        version_check = False
-    if mros.MODULE_VER != "2.50":
-        print("Module Version Error! : mavlinkROS2Nodes")
-        version_check = False
-    if mvv.MODULE_VER != "1.10":
-        print("Module Version Error! : mavlinkVehicleView")
-        version_check = False
-    if sm.MODULE_VER != "2.00":
-        print("Module Version Error! : serialManager")
-        version_check = False
-    if version_check == False:
-        print("Environment Obstacle! Check YOUR Execution System Path First!!")
-        return
-    # ========================================
 
     stop_evt = threading.Event()
     def signal_handler(signum, frame):

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

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

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,7 +44,7 @@ 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.50"
@@ -214,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):
@@ -1179,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 節點管理器
 # ============================================================================
@@ -1192,10 +1246,11 @@ class fc_ros_manager:
     stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。
     shutdown 是完全關閉 ROS2 並銷毀節點實例。
 
-    管理三個獨立的 ROS2 Node :
+    管理四個獨立的 ROS2 Node :
     - VehicleStatusPublisher
     - MavlinkCommandService
     - RtcmRelay
+    - FcNetworkLogPublisher
     
     提供統一的啟動/停止介面給 mainOrchestrator
 
@@ -1215,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
@@ -1235,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")
@@ -1266,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,
@@ -1391,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():
@@ -1417,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():
@@ -1435,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,
         }
 
     
@@ -1489,4 +1559,3 @@ TODO
 1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期
 
 '''
-

src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py

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

src/fc_network_adapter/fc_network_adapter/serialManager.py

@@ -19,11 +19,18 @@ 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
 
@@ -138,6 +145,8 @@ class XBeeFrameProcessor_Base(FrameProcessor):
     DEST_ADDR16_UNICAST = b'\xFF\xFE'
     DEST_ADDR16_BRAODCAST = b'\xFF\xFF'
 
+    DEST_ADDR64_BRAODCAST = b'\x00\x00\x00\x00\x00\x00\x00\x00'
+
     def __init__(self, at_handler: "ATCommandHandler" = None):
         super().__init__()
         self.at_handler = at_handler
@@ -221,7 +230,7 @@ class XBeeFrameProcessor_Base(FrameProcessor):
     @staticmethod
     def _encapsulate(
         data: bytes,
-        dest_addr64: bytes = b'\x00\x00\x00\x00\x00\x00\x00\x00',
+        dest_addr64: bytes = DEST_ADDR64_BRAODCAST,
         dest_addr16 = DEST_ADDR16_BRAODCAST,
         frame_id: int = 0x01,
     ) -> bytes:
@@ -251,14 +260,26 @@ class XBeeFrameProcessor_Base(FrameProcessor):
 
 
 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)
 
-    # GCS -> UAV:
+    這邊認定 esp_sysid 會等於 mavlink sysid
-    #   DISC
+
-    #   POLL + esp_sysid(1) + grant_bytes(2)
+    硬體產品系列 (Product Family) : XBP9B-DM
-    #
+    晶片世代: XBee PRO 900HP 200K
-    # UAV -> GCS:
+    運作頻段: 900 MHz RF。
-    #   HELO + esp_sysid(1)
+    運行協議 (Protocol / Function Set): DigiMesh
-    #   DONE + sysid(1) + sent_len(2) + remain_len(2)
+    韌體版本 (Firmware Version): 8075
+    '''
 
     DISC_HEADER = b'DISC'
     HELLO_HEADER = b'HELO'
@@ -269,6 +290,11 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
     MAX_PAYLOAD_PER_FRAME = 80
     CHUNK_SEND_INTERVAL_SEC = 0.01
 
+    # ADDR16 選項
+    DEST_ADDR16_BRAODCAST = b'\xFF\xFF'
+
+    DEST_ADDR64_BRAODCAST = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'
+
     class Esp32DeviceInfo:
         def __init__(self, system_id, address_64, last_hello_time):
             self.system_id = system_id
@@ -276,6 +302,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
             self.last_hello_time = last_hello_time
 
             self.remain_bytes = 0 # 剩餘 buffer 量
+            self.last_poll_time = 0.0
             self.last_done_time = 0.0 # 最後送出Done的時間
             self.received_len = 0 # 收到封包累計
 
@@ -300,7 +327,9 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         self.poll_done_event: Optional[asyncio.Event] = None
         self.current_poll_address_64: Optional[bytes] = None
 
-        self.last_discovery_time = 0.0
+        self.last_discovery_time = 0.0 # 這個是最後做廣播 discovery 的時間  
+        self.last_recieve_mavlink = 0.0 # 這個是最後收到 mavlink payload 時間 為了定義 poll-done 之間不要超時用的
+        self.MAX_mavPack_interval_timeout = 100  # ms，poll 期間 MAVLink/DONE 最大閒置間隔
         self.discovery_interval_seconds = 30.0 # 每次做 discovery 程序的間隔時間
         self.device_offline_timeout = self.discovery_interval_seconds * 2 # 遠端沒有回應會被踢出 超時時限
         self.operator_tick_interval_seconds = 0.03 # 
@@ -351,6 +380,8 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
             remote_device = self.esp32_address_mapping.get(sender_address_64)
             if remote_device is not None:
                 remote_device.received_len += len(payload)
+                if (self.operator_busy and self.current_poll_address_64 == sender_address_64):
+                    self.last_recieve_mavlink = time.time()
             return payload
 
         if frame_type == self.FRAME_TYPE_AT_RESPONSE:
@@ -375,6 +406,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
     def pack_discovery(self) -> bytes:
         return self._encapsulate(self.DISC_HEADER, frame_id=0x00)
 
+    # 處理每個裝置回傳的 Hello 訊息
     def handle_hello_report(self, payload: bytes, sender_address_64: bytes) -> None:   
         system_id = payload[4]
         remote_device = self.esp32_address_mapping.get(sender_address_64)
@@ -384,8 +416,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
                 system_id, sender_address_64, time.time()
             )
             logger.debug(
-                f"new HELO system_id={system_id}, address_64={sender_address_64.hex()}"
+                f"new HELO system_id={system_id}, address_64={sender_address_64.hex()}")
-            )
         elif remote_device.address_64 == sender_address_64:
             remote_device.last_hello_time = time.time()
         else:
@@ -394,6 +425,10 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
                 f"address_64={sender_address_64.hex()}"
             )
         
+        if vehicle:=vehicle_registry.get(system_id):
+            if not vehicle.rf_module:
+                vehicle.rf_module = RFModule(RFModuleType.XBEE)
+
     def pack_poll(self, target_address_64: bytes, grant_bytes: int = 0) -> Optional[bytes]:
         remote_device = self.esp32_address_mapping.get(target_address_64)
         if remote_device is None:
@@ -415,13 +450,16 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         if remote_device is None:
             return
 
-        system_id, sent_length, remain_length = struct.unpack('>BHH', payload[4:9])
+        # 這段是有問題的 因為會有整數封包切割問題 以及載具端的 buffer 存量不足 故回傳的資訊量會與要求的不一致
-        if sent_length != remote_device.received_len:
+        # system_id, sent_length, remain_length = struct.unpack('>BHH', payload[4:9])
-            logger.info(
+        # if sent_length != remote_device.received_len:
-                f"POLL may be missing packets sent={sent_length} "
+        #     logger.info(
-                f"received={remote_device.received_len} system_id={system_id}"
+        #         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()
@@ -449,11 +487,11 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         return len(self.gcs_transmit_queue)
 
     # 把 gcs_transmit_queue 的 mavlink 封包依照大小打包出來
-    def _pop_flush_batch(self, max_bytes: int) -> list[bytes]:
+    def _pop_flush_batch(self, max_bytes: int) -> List[bytes]:
         if not self.gcs_transmit_queue:
             return []
 
-        batch: list[bytes] = []
+        batch: List[bytes] = []
         total_bytes = 0
 
         while self.gcs_transmit_queue:
@@ -507,7 +545,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
     # ---- POLL 排程輔助 ----
 
     # 計算下次要 poll 的對象跟大小
-    def _pick_poll_target(self) -> tuple[Optional[bytes], int]:
+    def _pick_poll_target(self):
         poll_devices = [
             pollStrategy.PollDevice(
                 address_64=address_64,
@@ -715,6 +753,22 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         self.is_discovery_phase = False
         self.pending_manual_discovery = False
 
+    async def _wait_poll_done_with_idle_timeout(self) -> bool:
+        idle_timeout_sec = self.MAX_mavPack_interval_timeout / 1000.0
+        poll_tick = min(0.02, idle_timeout_sec / 2)
+
+        while not self.poll_done_event.is_set():
+            if time.time() - self.last_recieve_mavlink >= idle_timeout_sec:
+                return False
+            try:
+                await asyncio.wait_for(
+                    self.poll_done_event.wait(),
+                    timeout=poll_tick,
+                )
+            except asyncio.TimeoutError:
+                continue
+        return True
+
     # poll 程序
     async def _run_one_poll(self, target_address_64: bytes, grant_bytes: int) -> None:
         if self.serial_writer is None:
@@ -733,22 +787,15 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         self.current_poll_address_64 = target_address_64
         self.poll_done_event.clear()
         self.serial_writer(poll_frame)
+        self.last_recieve_mavlink = time.time()
 
-        timeout_seconds = pollStrategy.estimate_poll_timeout(
-            grant_bytes,
-            self.serial_baudrate,
-            self.guard_milliseconds,
-        )
         try:
-            await asyncio.wait_for(
+            completed = await self._wait_poll_done_with_idle_timeout()
-                self.poll_done_event.wait(),
+            if not completed:
-                timeout=timeout_seconds,
+                logger.warning(
-            )
+                    f"POLL timeout address_64={target_address_64.hex()} "
-        except asyncio.TimeoutError:
+                    f"grant_bytes={grant_bytes}"
-            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
@@ -819,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)
+        parsed_at_ack = self._parse(frame)
-        if parsed is None:
+        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:
@@ -864,26 +911,27 @@ class ATCommandHandler:
 
         handler = self.handlers.get(response.command)
         if handler:
-            handler(response.data)
+            handler(response)
         else:
             logger.debug(
                 f"[{self.serial_port}] 未處理的 AT 指令: "
                 f"{response.command.decode()}"
             )
 
-    def _handle_rssi(self, data: bytes):
+    def _handle_rssi(self, response: ATResponse):
         """處理 DB (RSSI) 回應：單 byte 無號值，單位 dBm"""
-        pass
-        if data:
-            print(f"[{self.serial_port}] RSSI = -{data[0]} dBm")  # dev
-            # logger.debug(f"[{self.serial_port}] RSSI = -{data[0]} dBm") # dev
-            pass
 
-    def _handle_serial_high(self, data: bytes):
+        # print(f"[{self.serial_port}] RSSI = -{data[0]} dBm")  # dev
+        logger.debug(f"[{self.serial_port}] RSSI = -{response.data[0]} dBm") # dev
+
+
+
+
+    def _handle_serial_high(self, response: ATResponse):
         """處理 SH (Serial Number High)"""
         pass
 
-    def _handle_serial_low(self, data: bytes):
+    def _handle_serial_low(self, response: ATResponse):
         """處理 SL (Serial Number Low)"""
         pass
 
@@ -1245,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
         回傳是否成功排進事件圈
@@ -1326,46 +1374,51 @@ if __name__ == '__main__':
     # UDP_REMOTE_PORT = 14571
     # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.STRAIGHT)
 
-    # # 測試項二
+    # 測試項二
-    # SERIAL_PORT = '/dev/ttyUSB0'  # 手動指定
+    print("運行 測試項二")
-    # SERIAL_BAUDRATE = 115200
+    SERIAL_PORT = '/dev/ttyUSB0'  # 手動指定
-    # UDP_REMOTE_PORT = 14561
-    # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
-
-    # linked_serial = sm.get_serial_link()
-    # print(linked_serial)
-
-    # # 等 connection_made 完成 writer 注入，再發一筆 AT 指令測試
-    # time.sleep(5)
-    # rssi_request = ATRequest(command=b'DB', parameter=b'', frame_id=0x52)
-    # for i in range(60):
-    #     sm.send_at_command(1, rssi_request)
-    #     time.sleep(1)
-
-    # sm.remove_serial_link(1)
-    # time.sleep(3)
-    # sm.shutdown()
-
-    # # 測試項三
-    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)
+    sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
-
-    time.sleep(2)  # 等 serial 連線與 operator 啟動
-
     serial_id = 1
-    processor = sm.get_espv1_processor(serial_id)
+    device_sys_id = 10
-    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)
+    linked_serial = sm.get_serial_link()
-    time.sleep(3)
+    print(f"連結完成 : {linked_serial}. 等待兩秒")
+
+    # 等 connection_made 完成 writer 注入，再發一筆 AT 指令測試
+    time.sleep(2)
+    rssi_request = ATRequest(command=b'DB', parameter=b'', frame_id=device_sys_id)
+    print(f"手動送出 DB AT Command:")
+    for i in range(20):
+        sm.send_at_command(1, rssi_request)
+        time.sleep(1)
+
+    sm.remove_serial_link(1)
+    time.sleep(2)
     sm.shutdown()
+    print("結束運行")
+
+    # # 測試項三
+    # SERIAL_PORT = '/dev/ttyUSB0'
+    # SERIAL_BAUDRATE = 115200
+    # UDP_REMOTE_PORT = 14561
+    # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI_espv1)
+
+    # 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()
 
 '''
 ================= 改版記錄 ============================

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

@@ -5,6 +5,7 @@ POLL 輪詢策略，供 XBeeFrameProcessor_ESPv1 使用。
 """
 
 from dataclasses import dataclass
+from typing import List
 
 
 @dataclass(frozen=True)
@@ -23,10 +24,9 @@ class PollSchedulerState:
 
 
 def pick_next(
-    devices: list[PollDevice],
+    devices: List[PollDevice],
-    scheduler_state: PollSchedulerState,
+    scheduler_state: PollSchedulerState
-    # default_grant_bytes: int = 600,
+):
-) -> tuple[bytes | None, int]:
     """
     選下一個 POLL 目標。
 
@@ -46,26 +46,3 @@ def pick_next(
 
     return selected_device.address_64, grant_bytes
 
-
-def estimate_poll_timeout(
-    grant_bytes: int,
-    baudrate: int,
-    guard_milliseconds: int = 20,
-) -> float:
-    """
-    估算 POLL 後等待 DONE 的超時秒數。
-    移植自 udptest8 estimate_tdma_timeout。
-    """
-    grant_bytes = max(0, int(grant_bytes))
-    max_payload_per_chunk = 100
-    chunk_count = max(1, (grant_bytes + max_payload_per_chunk - 1) // max_payload_per_chunk)
-
-    uart_time = (grant_bytes + chunk_count * 18 + 32) * 10.0 / baudrate
-    timeout = (
-        uart_time
-        + chunk_count * 0.012
-        + (guard_milliseconds / 1000.0)
-        + 0.35
-    )
-    return timeout
-

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 名稱