2.9.1: new mission_orch

src/GUI/communication.py

@@ -11,6 +11,7 @@ import json
 import socket
 import sys
 import os
+import time
 import traceback
 try:
     import serial
@@ -214,23 +215,23 @@ class JsonTelemetryProcessor:
             pos = data.get('position')
             if isinstance(pos, dict):
                 gps_data = {
-                    'lat': pos.get('lat', pos.get('latitude', 0)),
-                    'lon': pos.get('lon', pos.get('longitude', 0)),
-                    'alt': pos.get('alt', pos.get('altitude', 0))
+                    'lat': pos.get('lat', pos.get('latitude')),
+                    'lon': pos.get('lon', pos.get('longitude')),
+                    'alt': pos.get('alt', pos.get('altitude'))
                 }
                 self.signals.update_signal.emit('gps', drone_id, gps_data)
             elif 'lat' in data or 'latitude' in data:
                 self.signals.update_signal.emit('gps', drone_id, {
-                    'lat': data.get('lat', data.get('latitude', 0)),
-                    'lon': data.get('lon', data.get('longitude', 0)),
-                    'alt': data.get('alt', data.get('altitude', 0))
+                    'lat': data.get('lat', data.get('latitude')),
+                    'lon': data.get('lon', data.get('longitude')),
+                    'alt': data.get('alt', data.get('altitude'))
                 })
             elif isinstance(data.get('p'), (list, tuple)) and len(data.get('p')) >= 2:
                 dop = data.get('d') if isinstance(data.get('d'), (list, tuple)) else []
                 gps_data = {
                     'lat': data['p'][0],
                     'lon': data['p'][1],
-                    'alt': data.get('h', 0)
+                    'alt': data.get('h')
                 }
                 if 'g' in data:
                     gps_data['fix_type'] = data.get('g')
@@ -935,7 +936,6 @@ class DroneMonitor(Node):
     
     def __init__(self):
         # Use a unique node name with timestamp to avoid conflicts on restart
-        import time
         node_name = f'drone_monitor_{int(time.time() * 1000) % 100000}'
         super().__init__(node_name)
         self.signals = DroneSignals()
@@ -960,6 +960,9 @@ class DroneMonitor(Node):
         # 【新增】儲存 GPS 資料的字典
         # ================================================================================
         self.drone_gps = {}  # {drone_id: {'lat': ..., 'lon': ..., 'alt': ...}}
+        # GnssRaw.altitude 是 AMSL；交戰/航點指令的 alt 則是相對 Home 高度。
+        self.drone_relative_alt = {}
+        self.drone_relative_alt_updated_at = {}
         # ================================================================================
         
         # ================================================================================
@@ -1007,6 +1010,35 @@ class DroneMonitor(Node):
                 200
             )
 
+    def get_drone_gps_snapshot(self):
+        """回傳執行緒安全的定位快照。"""
+        with self.lock:
+            return {
+                drone_id: dict(position)
+                for drone_id, position in self.drone_gps.items()
+                if isinstance(position, dict)
+            }
+
+    def record_relative_gps(self, drone_id, gps_data, updated_at=None):
+        """記錄已知以 Home 為基準的 GPS/高度（UDP、Serial、WebSocket）。"""
+        now = time.monotonic() if updated_at is None else float(updated_at)
+        stored = dict(gps_data)
+        stored['_gps_updated_at'] = now
+        alt = stored.get('alt')
+        if isinstance(alt, (int, float)) and math.isfinite(float(alt)):
+            stored['alt'] = float(alt)
+            stored['_relative_alt_updated_at'] = now
+            stored['_altitude_reference'] = 'relative_home'
+        else:
+            stored['alt'] = None
+
+        with self.lock:
+            if stored.get('_altitude_reference') == 'relative_home':
+                self.drone_relative_alt[drone_id] = stored['alt']
+                self.drone_relative_alt_updated_at[drone_id] = now
+            self.drone_gps[drone_id] = stored
+        return stored
+    
     def get_next_socket_id(self):
         """取得目前最小的未使用 socket_id（從 0 開始）。"""
         with self.socket_id_lock:
@@ -1740,10 +1772,15 @@ class DroneMonitor(Node):
         if actual_drone_id is None:
             return
 
+        # GnssRaw.altitude 是 GLOBAL_POSITION_INT.alt（AMSL）。交戰航點使用
+        # GLOBAL_RELATIVE_ALT_INT，因此 alt 僅能使用 position_ned 的相對高度。
+        now = time.monotonic()
         gps_data = {
             'lat': msg.latitude,
             'lon': msg.longitude,
-            'alt': msg.altitude
+            'alt': None,
+            'amsl_alt': msg.altitude,
+            '_gps_updated_at': now,
         }
 
         if hasattr(msg, 'fix_type'):
@@ -1755,12 +1792,20 @@ class DroneMonitor(Node):
         if hasattr(msg, 'epv'):
             gps_data['epv'] = msg.epv
         
-        self.latest_data[(actual_drone_id, 'gps')] = gps_data
-        
         # ================================================================================
         # 【新增】儲存 GPS 資料到 drone_gps 字典
         # ================================================================================
-        self.drone_gps[actual_drone_id] = gps_data.copy()
+        with self.lock:
+            relative_alt = self.drone_relative_alt.get(actual_drone_id)
+            relative_alt_updated_at = self.drone_relative_alt_updated_at.get(
+                actual_drone_id
+            )
+            if relative_alt_updated_at is not None:
+                gps_data['alt'] = relative_alt
+                gps_data['_relative_alt_updated_at'] = relative_alt_updated_at
+                gps_data['_altitude_reference'] = 'relative_home'
+            self.drone_gps[actual_drone_id] = gps_data.copy()
+        self.latest_data[(actual_drone_id, 'gps')] = gps_data
         # ================================================================================
 
     def local_vel_callback(self, drone_id, msg):
@@ -1821,6 +1866,18 @@ class DroneMonitor(Node):
             vy = msg.twist.twist.linear.x
             vz = -msg.twist.twist.linear.z
 
+            now = time.monotonic()
+            with self.lock:
+                self.drone_relative_alt[actual_drone_id] = z
+                self.drone_relative_alt_updated_at[actual_drone_id] = now
+                current_gps = self.drone_gps.get(actual_drone_id)
+                if isinstance(current_gps, dict):
+                    current_gps = dict(current_gps)
+                    current_gps['alt'] = z
+                    current_gps['_relative_alt_updated_at'] = now
+                    current_gps['_altitude_reference'] = 'relative_home'
+                    self.drone_gps[actual_drone_id] = current_gps
+            
             # 儲存高度信息
             self.latest_data[(actual_drone_id, 'altitude')] = {
                 'altitude': z

src/GUI/gui.py

@@ -147,10 +147,11 @@ class ToggleSwitch(QWidget):
 class ControlStationUI(QMainWindow):
     planning_finished = pyqtSignal(object)
 
-    VERSION = '2.9.0'
+    VERSION = '2.9.1'
     FONT_SCALE_MIN = 70
     FONT_SCALE_MAX = 180
     FONT_SCALE_DEFAULT = 100
+    ENGAGEMENT_TELEMETRY_TIMEOUT_SEC = 5.0
 
     def __init__(self):
         super().__init__()
@@ -2647,13 +2648,27 @@ class ControlStationUI(QMainWindow):
         lat/lon/alt，因此在這裡把 GUI 最新 heading 合併進去，避免下一筆 GPS
         覆蓋資料後讓交戰 X 陣形失去敵機機首方向。
         """
-        source = getattr(self.monitor, 'drone_gps', {})
-        snapshot = {
-            drone_id: dict(pos)
-            for drone_id, pos in source.items()
-            if isinstance(pos, dict)
-        }
+        if hasattr(self.monitor, 'get_drone_gps_snapshot'):
+            snapshot = self.monitor.get_drone_gps_snapshot()
+        else:
+            source = getattr(self.monitor, 'drone_gps', {})
+            snapshot = {
+                drone_id: dict(pos)
+                for drone_id, pos in source.items()
+                if isinstance(pos, dict)
+            }
         now = time.monotonic()
+        for position in snapshot.values():
+            relative_updated_at = position.get('_relative_alt_updated_at')
+            relative_is_fresh = (
+                position.get('_altitude_reference') == 'relative_home'
+                and isinstance(relative_updated_at, (int, float))
+                and 0.0 <= now - relative_updated_at
+                <= self.ENGAGEMENT_TELEMETRY_TIMEOUT_SEC
+            )
+            if not relative_is_fresh:
+                # 不得把 AMSL 或過期高度帶入 GLOBAL_RELATIVE_ALT 指令。
+                position.pop('alt', None)
         for drone_id, heading in self.drone_headings.items():
             updated_at = self.drone_heading_updated_at.get(drone_id)
             heading_is_fresh = (
@@ -2672,6 +2687,7 @@ class ControlStationUI(QMainWindow):
             make_executor=self._make_orchestrator_executor,
             gps_provider=self._engagement_gps_provider,
             stop_groups_fn=self._stop_engagement_group_missions,
+            target_lost_timeout_sec=self.ENGAGEMENT_TELEMETRY_TIMEOUT_SEC,
         )
         self._engagement_a_group = None
         self._engagement_b_group = None
@@ -2832,8 +2848,8 @@ class ControlStationUI(QMainWindow):
 
         assignments = {
             'A': {drones_by_sysid[11], drones_by_sysid[12]},
-            'B': {drones_by_sysid[14], drones_by_sysid[15]},
-            'C': {drones_by_sysid[13]},
+            'B': {drones_by_sysid[13], drones_by_sysid[14]},
+            'C': {drones_by_sysid[15]},
         }
         for gid, members in assignments.items():
             group = self.mission_groups[gid]
@@ -2853,13 +2869,13 @@ class ControlStationUI(QMainWindow):
         labels = [(gid, group.display_name)
                   for gid, group in self.mission_groups.items()]
         self.engagement_panel.refresh_group_list(labels)
-        enemy_id = drones_by_sysid[13]
+        enemy_id = drones_by_sysid[15]
         if not self.engagement_panel.set_configuration(enemy_id, 'A', 'B'):
             _log("WARN", "自動交戰編組完成，但交戰面板設定失敗")
             return
         self._sync_orchestrator_config()
         self.statusBar().showMessage(
-            "已自動設定交戰：A=sysid 11,12；B=sysid 14,15；敵機=sysid 13（Group C）",
+            "已自動設定交戰：A=sysid 11,12；B=sysid 13,14；敵機=sysid 15（Group C）",
             6000)
 
     def reorganize_socket_groups(self):
@@ -3021,7 +3037,7 @@ class ControlStationUI(QMainWindow):
                         )
                     
                     elif msg_type == 'gps':
-                        gps_data = data
+                        gps_data = dict(data)
                         lat, lon = gps_data.get('lat'), gps_data.get('lon')
                         has_position = (
                             isinstance(lat, (int, float)) and isinstance(lon, (int, float))
@@ -3031,7 +3047,12 @@ class ControlStationUI(QMainWindow):
                             self._map_dirty_drones.add(drone_id)
                             if not hasattr(self.monitor, 'drone_gps'):
                                 self.monitor.drone_gps = {}
-                            self.monitor.drone_gps[drone_id] = gps_data.copy()
+                            # ROS callback 已先寫入含時間戳的快照；其餘來源的
+                            # GLOBAL_POSITION_INT/JSON 高度均定義為相對 Home。
+                            if '_gps_updated_at' not in gps_data:
+                                gps_data = self.monitor.record_relative_gps(
+                                    drone_id, gps_data
+                                )
                         self.queue_overview_update(
                             drone_id, 'latitude', f"{lat:.6f}°" if has_position else '-'
                         )

src/GUI/mission_orchestrator.py

@@ -182,9 +182,9 @@ DEFAULT_REASSIGN_COOLDOWN = 2.0       # slot 交換最短間隔 (s)；安全時
 DEFAULT_REPLAN_MOVE_M = 1.0           # 敵機移動 >= 1m 觸發更新
 DEFAULT_REPLAN_CAP_SEC = 0.4           # 最慢 0.4 秒更新一次瞬時目標
 DEFAULT_LEAD_TIME_CAP = 4.0
-DEFAULT_FRIENDLY_SPEED = 5.0
+DEFAULT_FRIENDLY_SPEED = 3.0
 DEFAULT_ENEMY_VEL_WINDOW = 1.0
-DEFAULT_TARGET_LOST_TIMEOUT = 2.0
+DEFAULT_TARGET_LOST_TIMEOUT = 5.0
 
 # assignment cost
 DEFAULT_CROSSING_PENALTY = 20000.0
@@ -437,9 +437,13 @@ class MissionOrchestrator(QObject):
 
         gps = self._gps_provider()
         epos = gps.get(self.enemy_id)
-        if not epos:
+        now = time.monotonic()
+        if not epos or not self._position_is_fresh(epos, now):
             self.status_message.emit("敵機定位遺失，無法開始包圍")
             return
+        if self._position_alt(epos) is None:
+            self.status_message.emit("敵機相對 Home 高度尚未收到，無法開始包圍")
+            return
 
         friendly_ids = self._unique_keep_order(self.a_drone_ids + self.b_drone_ids)
         if not friendly_ids:
@@ -467,7 +471,7 @@ class MissionOrchestrator(QObject):
         self._motion_dirs = {}
         self._slot_groups = {}  # {slot_index: "A"/"B"}
         self._vertical_hold_ids = set()
-        self._last_enemy_seen_at = time.monotonic()
+        self._last_enemy_seen_at = self._position_received_at(epos, now)
         self._update_enemy_velocity(epos)
 
         center = self._planning_center(gps, epos, reset=True)
@@ -558,7 +562,8 @@ class MissionOrchestrator(QObject):
     def _tick_watch(self):
         gps = self._gps_provider()
         epos = gps.get(self.enemy_id)
-        if not epos:
+        now = time.monotonic()
+        if not epos or not self._position_is_fresh(epos, now):
             self._detect_since = 0.0
             return
 
@@ -566,13 +571,12 @@ class MissionOrchestrator(QObject):
         nearest_d = None
         for did in self.a_drone_ids:
             pos = gps.get(did)
-            if not pos:
+            if not pos or not self._position_is_fresh(pos, now):
                 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
@@ -589,11 +593,12 @@ class MissionOrchestrator(QObject):
         epos = gps.get(self.enemy_id)
         now = time.monotonic()
 
-        if not epos:
+        if (not epos or not self._position_is_fresh(epos, now)
+                or self._position_alt(epos) is None):
             self._handle_enemy_missing(gps, now)
             return
 
-        self._last_enemy_seen_at = now
+        self._last_enemy_seen_at = self._position_received_at(epos, now)
         self._update_enemy_velocity(epos)
 
         if self._positions_for(self._friendly_ids, gps) is None:
@@ -614,11 +619,12 @@ class MissionOrchestrator(QObject):
         epos = gps.get(self.enemy_id)
         now = time.monotonic()
 
-        if not epos:
+        if (not epos or not self._position_is_fresh(epos, now)
+                or self._position_alt(epos) is None):
             self._handle_enemy_missing(gps, now)
             return
 
-        self._last_enemy_seen_at = now
+        self._last_enemy_seen_at = self._position_received_at(epos, now)
         self._update_enemy_velocity(epos)
 
         if self._positions_for(self._friendly_ids, gps) is None:
@@ -638,13 +644,15 @@ class MissionOrchestrator(QObject):
     def _tick_target_lost(self):
         gps = self._gps_provider()
         epos = gps.get(self.enemy_id)
-        if not epos:
+        now = time.monotonic()
+        if (not epos or not self._position_is_fresh(epos, now)
+                or self._position_alt(epos) is None):
             return
 
         # 目標重新出現：不沿用過期軌跡，重新估速並回 CONVERGE。
         self._enemy_hist = []
         self._enemy_vel = (0.0, 0.0)
-        self._last_enemy_seen_at = time.monotonic()
+        self._last_enemy_seen_at = self._position_received_at(epos, now)
         self._update_enemy_velocity(epos)
         self._set_phase(MissionPhase.CONVERGE)
         self.status_message.emit("敵機定位恢復，重新建立包圍軌跡")
@@ -667,14 +675,18 @@ class MissionOrchestrator(QObject):
         if self._merged_ex is None:
             return
         targets = {}
+        now = time.monotonic()
         for did in self._friendly_ids:
             p = gps.get(did)
-            if not p:
+            if not p or not self._position_is_fresh(p, now):
+                continue
+            current_alt = self._position_alt(p)
+            if current_alt is None:
                 continue
             targets[did] = (
                 p['lat'],
                 p['lon'],
-                max(self.min_alt_floor, float(p.get('alt', self.formation_alt))),
+                max(self.min_alt_floor, current_alt),
             )
         if len(targets) == len(self._friendly_ids):
             self._merged_ex.update_plan(self._targets_to_plan(targets))
@@ -844,6 +856,35 @@ class MissionOrchestrator(QObject):
             return None
         return alt if math.isfinite(alt) else None
 
+    def _position_is_fresh(self, pos, now=None):
+        """檢查 GPS 水平位置是否仍在可接受的遙測時間內。"""
+        if not pos:
+            return False
+        updated_at = pos.get('_gps_updated_at')
+        # 保留舊 gps_provider/測試資料的相容性；GUI 實飛資料一定帶時間戳。
+        if updated_at is None:
+            return True
+        try:
+            updated_at = float(updated_at)
+        except (TypeError, ValueError):
+            return False
+        if not math.isfinite(updated_at):
+            return False
+        now = time.monotonic() if now is None else float(now)
+        age = now - updated_at
+        return -0.1 <= age <= self.target_lost_timeout_sec
+
+    @staticmethod
+    def _position_received_at(pos, now):
+        """取得最後一筆水平定位的 monotonic 時間，舊資料則使用目前時間。"""
+        try:
+            updated_at = float(pos.get('_gps_updated_at'))
+        except (AttributeError, TypeError, ValueError):
+            return now
+        if not math.isfinite(updated_at) or updated_at > now + 0.1:
+            return now
+        return updated_at
+
     def _cross_layer_vertical_hold_ids(self, gps):
         """
         找出必須先原地垂直分層的 A/B 友機。
@@ -1111,7 +1152,9 @@ class MissionOrchestrator(QObject):
             acc += seg
 
         lat, lon = _m_to_ll(chosen[0], chosen[1], center[0], center[1])
-        cur_alt = float(gps[did].get('alt', self.formation_alt))
+        cur_alt = self._position_alt(gps[did])
+        if cur_alt is None:
+            return None
         target_alt = slots[slot_index][2]
         # 高度用距離比例平滑，但近場直接交給 slot。
         frac = min(1.0, want / max(1e-6, traj['length']))
@@ -1317,7 +1360,9 @@ class MissionOrchestrator(QObject):
 
             lat, lon = _m_to_ll(tx, ty, ref_lat, ref_lon)
             target_alt = self._layer_alt(group)
-            cur_alt = float(p.get('alt', target_alt))
+            cur_alt = self._position_alt(p)
+            if cur_alt is None:
+                return None
             alt = cur_alt + 0.35 * (target_alt - cur_alt)
             raw[did] = (lat, lon, max(self.min_alt_floor, alt))
 
@@ -1360,10 +1405,10 @@ class MissionOrchestrator(QObject):
             tx = x + ox * self.recovery_spread_step
             ty = y + oy * self.recovery_spread_step
             lat, lon = _m_to_ll(tx, ty, ref_lat, ref_lon)
-            alt = max(
-                self.min_alt_floor,
-                float(gps[did].get('alt', self.formation_alt)),
-            )
+            current_alt = self._position_alt(gps[did])
+            if current_alt is None:
+                return None
+            alt = max(self.min_alt_floor, current_alt)
             targets[did] = (lat, lon, alt)
         return targets
 
@@ -1776,11 +1821,20 @@ class MissionOrchestrator(QObject):
 
     def _positions_for(self, drone_ids, gps):
         out = []
+        now = time.monotonic()
         for did in drone_ids:
             p = gps.get(did)
-            if not p:
+            if not p or not self._position_is_fresh(p, now):
+                return None
+            try:
+                lat = float(p['lat'])
+                lon = float(p['lon'])
+            except (KeyError, TypeError, ValueError):
+                return None
+            alt = self._position_alt(p)
+            if not math.isfinite(lat) or not math.isfinite(lon) or alt is None:
                 return None
-            out.append((p['lat'], p['lon'], p.get('alt', 0.0)))
+            out.append((lat, lon, alt))
         return out
 
     @staticmethod