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/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,
-                 barrier_timeout_sec=20.0):
+                 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):
         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(