fix: 修正實飛 Circle 高度爆衝與 Leader-Follower 卡點

1. mavlinkROS2Nodes: position_gnss topic 改發 relative_altitude 而非 AMSL，
   讓 drone_gps['alt'] 在實飛與 SITL 都代表「相對 home 高度」
2. mission_planner: Circle stage1.z 改用 final_z（不再用 current_z 做中點插值），
   避免實飛時 current_z 是 AMSL 而把過渡高度衝到海拔值的一半
3. mission_executor: 到達判定改為 hover-stable（radius * 1.5 hysteresis +
   穩定計時），預設 arrival_radius 2→4m，解決實飛 GPS 抖動永遠進不了
   2m radius 的卡死問題
4. mission_executor: 新增 progress log，每 3 秒印一行距離與穩定度，
   方便實飛 debug 卡點問題
5. mission_group + gui: 暴露 arrival_radius / hover_stable_sec 給 GUI 任務參數面板，
   每次 start 重讀以支援中途調整

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

src/GUI/gui.py

@@ -1310,12 +1310,18 @@ class ControlStationUI(QMainWindow):
                 self.drone_map.set_mission_mode(mission_type)
 
     def _create_executor_for_group(self, group):
-        """為群組建立 MissionExecutor"""
+        """為群組建立 MissionExecutor，讀取 panel 上的執行參數"""
+        exec_params = {}
+        panel = self.group_panels.get(group.group_id)
+        if panel and hasattr(panel, 'get_execution_params'):
+            exec_params = panel.get_execution_params()
+
         executor = MissionExecutor(
             sender=self.command_sender,
             drone_gps=self.monitor.drone_gps,
             monitor=self.monitor,
-            arrival_radius=2.0,
+            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,
         )
         executor.drone_waypoint_reached.connect(self.on_drone_waypoint_reached)
@@ -1334,6 +1340,15 @@ class ControlStationUI(QMainWindow):
             return
         if group.executor is None:
             self._create_executor_for_group(group)
+        else:
+            # 已存在的 executor 也要重讀執行參數，讓使用者調整能生效
+            panel = self.group_panels.get(group_id)
+            if panel and hasattr(panel, 'get_execution_params'):
+                ep = panel.get_execution_params()
+                group.executor.arrival_radius = ep.get(
+                    'arrival_radius', group.executor.arrival_radius)
+                group.executor.hover_stable_sec = ep.get(
+                    'hover_stable_sec', group.executor.hover_stable_sec)
         group.executor.start(group.planned_waypoints)
         panel = self.group_panels.get(group_id)
         if panel:

src/GUI/mission_executor.py

@@ -43,6 +43,7 @@ class DroneTask:
     __slots__ = (
         'drone_id', 'sysid', 'waypoints', 'wp_index', 'done',
         'sent_current_wp', 'fail_count', 'status', 'waiting_since',
+        'entered_radius_at', 'last_log_at',
     )
 
     def __init__(self, drone_id, sysid, waypoints):
@@ -55,6 +56,8 @@ class DroneTask:
         self.fail_count = 0
         self.status = TaskStatus.NORMAL
         self.waiting_since = 0.0  # monotonic time 進入 WAITING_AT_BARRIER 的瞬間
+        self.entered_radius_at = 0.0  # 進入 arrival_radius 的時間，0 = 尚未進入
+        self.last_log_at = 0.0  # 上次印 distance log 的時間
 
     @property
     def current_target(self):
@@ -89,14 +92,21 @@ class MissionExecutor(QObject):
     mission_completed = pyqtSignal()
 
     def __init__(self, sender, drone_gps, monitor=None,
-                 arrival_radius=2.0, tick_rate_hz=2.0,
+                 arrival_radius=4.0, tick_rate_hz=2.0,
-                 barrier_timeout_sec=20.0):
+                 barrier_timeout_sec=20.0,
+                 hover_stable_sec=2.0,
+                 progress_log_interval_sec=3.0):
         super().__init__()
         self.sender = sender
         self.drone_gps = drone_gps
         self.monitor = monitor  # 用於失敗 fallback 到 LOITER
         self.arrival_radius = arrival_radius
         self.barrier_timeout_sec = barrier_timeout_sec
+        # hover-stable 判定：進入 radius 後須穩定停留 hover_stable_sec 秒才算到達，
+        # 容忍 GPS 抖動跨越邊界（用 radius * 1.5 作 hysteresis）
+        self.hover_stable_sec = hover_stable_sec
+        self.hysteresis_factor = 1.5
+        self.progress_log_interval_sec = progress_log_interval_sec
         self.state = MissionState.IDLE
         self.tasks = {}
         self.rendezvous_indices = set()
@@ -140,7 +150,7 @@ class MissionExecutor(QObject):
             "INFO",
             f"任務已啟動: {len(self.tasks)} 架無人機, "
             f"共 {total_wps} 個航點, "
-            f"到達半徑={self.arrival_radius}m, "
+            f"到達半徑={self.arrival_radius}m (hover-stable {self.hover_stable_sec}s), "
             f"tick 週期={self._interval_ms}ms, "
             f"barrier timeout={self.barrier_timeout_sec}s, "
             f"{rv_info}",
@@ -177,7 +187,7 @@ class MissionExecutor(QObject):
         """
         now = time.monotonic()
 
-        # ---- Phase 1: 到達判定 ----
+        # ---- Phase 1: 到達判定（hover-stable） ----
         for task in self.tasks.values():
             if task.done or task.status == TaskStatus.FALLBACK_LOITER:
                 continue
@@ -195,10 +205,32 @@ class MissionExecutor(QObject):
 
             tgt_lat, tgt_lon, _ = target
             distance = _haversine(gps['lat'], gps['lon'], tgt_lat, tgt_lon)
-            if distance >= self.arrival_radius:
+
+            # hover-stable 判定：
+            #   - 首次進入 radius → 記錄時間
+            #   - 在 radius * hysteresis 內持續 → 保留計時
+            #   - 離開 hysteresis → 重置
+            #   - 在 radius 內穩定 hover_stable_sec 秒 → 視為到達
+            hysteresis_radius = self.arrival_radius * self.hysteresis_factor
+            if distance < self.arrival_radius:
+                if task.entered_radius_at == 0.0:
+                    task.entered_radius_at = now
+                stable_for = now - task.entered_radius_at
+                if stable_for < self.hover_stable_sec:
+                    self._maybe_log_progress(task, now, distance, stable_for)
+                    continue
+            elif distance < hysteresis_radius and task.entered_radius_at > 0.0:
+                # 還在 hysteresis 內，計時不重置（容忍 GPS 抖動跨越邊界）
+                self._maybe_log_progress(task, now, distance, now - task.entered_radius_at)
+                continue
+            else:
+                # 離開 hysteresis：重置計時，繼續等待到達
+                task.entered_radius_at = 0.0
+                self._maybe_log_progress(task, now, distance, 0.0)
                 continue
 
             # 到達當前 wp_index
+            task.entered_radius_at = 0.0  # 重置給下個 wp 用
             if (task.wp_index in self.rendezvous_indices
                     and task.wp_index < task.total_waypoints - 1):
                 # rendezvous 點 → 不推進，進入 barrier 等待
@@ -245,11 +277,28 @@ class MissionExecutor(QObject):
             self.mission_completed.emit()
             _log("INFO", "任務全部完成")
 
+    def _maybe_log_progress(self, task, now, distance, stable_for):
+        """週期性印出 drone 與當前 wp 的距離，方便實飛 debug 卡點問題。"""
+        if now - task.last_log_at < self.progress_log_interval_sec:
+            return
+        task.last_log_at = now
+        if stable_for > 0:
+            stable_info = f", 在內側 {stable_for:.1f}s/{self.hover_stable_sec:.1f}s"
+        else:
+            stable_info = ""
+        _log(
+            "INFO",
+            f"{task.drone_id} → WP {task.wp_index}: "
+            f"距離 {distance:.1f}m (半徑 {self.arrival_radius:.1f}m{stable_info})",
+        )
+
     def _advance_waypoint(self, task, arrived_distance):
         """把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。"""
         task.wp_index += 1
         task.sent_current_wp = False
         task.fail_count = 0
+        task.entered_radius_at = 0.0
+        task.last_log_at = 0.0
         if task.wp_index >= task.total_waypoints:
             task.done = True
             self.drone_waypoint_reached.emit(

src/GUI/mission_group.py

@@ -32,9 +32,16 @@ DEFAULT_MISSION_PARAM_VALUES = {
     'lateral_offset': '3.0',
     'longitudinal_spacing': '5.0',
     'line_spacing': '5.0',
+    # 執行參數（所有任務類型共用）
+    'arrival_radius': '4.0',     # 到達半徑 (m)，實飛建議 3~5，SITL 可調小到 2
+    'hover_stable_sec': '2.0',   # 在 radius 內穩定停留多久才視為到達 (s)
 }
 
 
+# 執行參數的 key 集合（所有 mission type 共用，不隨類型切換）
+EXECUTION_PARAM_KEYS = ('arrival_radius', 'hover_stable_sec')
+
+
 class MissionGroup:
     """單一任務群組的資料"""
 
@@ -424,6 +431,12 @@ class GroupPanel(QWidget):
             ],
         }
 
+        # 所有 mission type 共用的執行參數（不隨類型切換隱藏）
+        self._exec_param_defs = [
+            ('arrival_radius', '到達半徑 (m)', self.default_params['arrival_radius']),
+            ('hover_stable_sec', '停留判定 (s)', self.default_params['hover_stable_sec']),
+        ]
+
         # 建立所有參數列的 widget（先全部建好，切換時顯示/隱藏）
         self._param_widgets = {}  # key → (label_widget, input_widget)
         self._param_rows = []     # 所有 row layout 對應的 container widget
@@ -447,6 +460,23 @@ class GroupPanel(QWidget):
                 self._param_widgets[key] = (row_w, inp)
                 self._param_rows.append(row_w)
 
+        # 執行參數列（所有 mission type 共用，永遠顯示）
+        for key, label_text, default in self._exec_param_defs:
+            row_w = QWidget()
+            row_l = QHBoxLayout(row_w)
+            row_l.setContentsMargins(0, 0, 0, 0)
+            row_l.setSpacing(3)
+            lbl = QLabel(label_text)
+            lbl.setStyleSheet(LBL)
+            inp = QLineEdit(default)
+            inp.setStyleSheet(INPUT)
+            inp.setFixedWidth(50)
+            row_l.addWidget(lbl, 1)
+            row_l.addWidget(inp)
+            param_col.addWidget(row_w)
+            self._param_widgets[key] = (row_w, inp)
+            self._param_rows.append(row_w)
+
         # LEADER_FOLLOWER 專用：領隊下拉選單
         self._leader_row = QWidget()
         leader_layout = QHBoxLayout(self._leader_row)
@@ -546,15 +576,17 @@ class GroupPanel(QWidget):
         self.delete_group_btn.setEnabled(enabled)
 
     def _update_param_visibility(self, _=None):
-        """根據當前任務類型，顯示/隱藏對應的參數列"""
+        """根據當前任務類型，顯示/隱藏對應的參數列。執行參數永遠顯示。"""
         mission_type = self.type_combo.currentText()
-        visible_keys = {d[0] for d in self._param_defs.get(mission_type, [])}
+        mission_keys = {d[0] for d in self._param_defs.get(mission_type, [])}
+        exec_keys = {d[0] for d in self._exec_param_defs}
+        visible_keys = mission_keys | exec_keys
         for key, (row_w, _inp) in self._param_widgets.items():
             row_w.setVisible(key in visible_keys)
         self._leader_row.setVisible(mission_type == 'LEADER_FOLLOWER')
 
     def get_mission_params(self):
-        """讀取當前顯示的參數值，回傳 dict"""
+        """讀取當前任務類型的規劃參數，回傳 dict（不含執行參數）"""
         mission_type = self.type_combo.currentText()
         params = {}
         for key, _label, default in self._param_defs.get(mission_type, []):
@@ -566,6 +598,18 @@ class GroupPanel(QWidget):
                     params[key] = float(default)
         return params
 
+    def get_execution_params(self):
+        """讀取執行參數（arrival_radius, hover_stable_sec 等），回傳 dict"""
+        params = {}
+        for key, _label, default in self._exec_param_defs:
+            if key in self._param_widgets:
+                _row_w, inp = self._param_widgets[key]
+                try:
+                    params[key] = float(inp.text())
+                except ValueError:
+                    params[key] = float(default)
+        return params
+
     def set_param_value(self, key, value):
         """更新指定參數欄位的文字值。"""
         if key not in self._param_widgets:

src/GUI/mission_planner.py

@@ -193,11 +193,13 @@ class FormationPlanner:
             final_y = center_y + radius * math.sin(angle_rad)
             final_z = altitude
 
-            current_x, current_y, current_z = drone_positions[i]
+            current_x, current_y, _current_z = drone_positions[i]
+            # stage1 高度直接用 final_z：先爬升到目標高度再水平集合，
+            # 避免用 current_z 做中點插值（current_z 在實飛時可能是 AMSL 而非相對 home，會導致衝高）
             stage1_positions.append((
                 current_x + (final_x - current_x) * 0.5,
                 current_y + (final_y - current_y) * 0.5,
-                current_z + (final_z - current_z) * 0.5
+                final_z,
             ))
             stage2_positions.append((final_x, final_y, final_z))
 

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -230,7 +230,9 @@ class VehicleStatusPublisher(Node):
         msg.stamp = self.get_clock().now().to_msg()
         msg.latitude = float(pos.latitude)
         msg.longitude = float(pos.longitude)
-        msg.altitude = float(pos.altitude) if pos.altitude is not None else 0.0
+        # 發布「相對 home 高度」而非 AMSL：下游 GUI / mission planner 一律以相對高度做運算與指令，
+        # 若這裡發 AMSL（msg.alt）會在實飛時把海拔當成相對高度傳給飛控，造成 Circle 高度衝高的 bug
+        msg.altitude = float(pos.relative_altitude) if pos.relative_altitude is not None else 0.0
         
         # GPS 狀態資訊
         gps = status.gps