diff --git a/src/GUI/communication.py b/src/GUI/communication.py index 1975141..c09cc41 100644 --- a/src/GUI/communication.py +++ b/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 = {} # ================================================================================ # ================================================================================ @@ -1006,6 +1009,35 @@ class DroneMonitor(Node): self.fc_network_log_callback, 200 ) + + def get_drone_gps_snapshot(self): + """回傳執行緒安全的定位快照。""" + with self.lock: + return { + drone_id: dict(position) + for drone_id, position in self.drone_gps.items() + if isinstance(position, dict) + } + + def record_relative_gps(self, drone_id, gps_data, updated_at=None): + """記錄已知以 Home 為基準的 GPS/高度（UDP、Serial、WebSocket）。""" + now = time.monotonic() if updated_at is None else float(updated_at) + stored = dict(gps_data) + stored['_gps_updated_at'] = now + alt = stored.get('alt') + if isinstance(alt, (int, float)) and math.isfinite(float(alt)): + stored['alt'] = float(alt) + stored['_relative_alt_updated_at'] = now + stored['_altitude_reference'] = 'relative_home' + else: + stored['alt'] = None + + with self.lock: + if stored.get('_altitude_reference') == 'relative_home': + self.drone_relative_alt[drone_id] = stored['alt'] + self.drone_relative_alt_updated_at[drone_id] = now + self.drone_gps[drone_id] = stored + return stored def get_next_socket_id(self): """取得目前最小的未使用 socket_id（從 0 開始）。""" @@ -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): @@ -1820,6 +1865,18 @@ class DroneMonitor(Node): vx = msg.twist.twist.linear.y vy = msg.twist.twist.linear.x vz = -msg.twist.twist.linear.z + + now = time.monotonic() + with self.lock: + self.drone_relative_alt[actual_drone_id] = z + self.drone_relative_alt_updated_at[actual_drone_id] = now + current_gps = self.drone_gps.get(actual_drone_id) + if isinstance(current_gps, dict): + current_gps = dict(current_gps) + current_gps['alt'] = z + current_gps['_relative_alt_updated_at'] = now + current_gps['_altitude_reference'] = 'relative_home' + self.drone_gps[actual_drone_id] = current_gps # 儲存高度信息 self.latest_data[(actual_drone_id, 'altitude')] = { diff --git a/src/GUI/gui.py b/src/GUI/gui.py index 4f83bbd..2bc3d24 100644 --- a/src/GUI/gui.py +++ b/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 '-' ) diff --git a/src/GUI/mission_orchestrator.py b/src/GUI/mission_orchestrator.py index f963657..6cac6a6 100644 --- a/src/GUI/mission_orchestrator.py +++ b/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