#!/usr/bin/env python3 """ MissionOrchestrator —— 多機偵測後的動態包圍任務編排器。 設計重點 ======== 1. 偵察階段仍可使用既有 A/B 群組：A 搜尋、B 待機。 2. 偵測並由操作員確認後，A/B 角色立即解除，所有友機視為同一個集合。 3. 不再使用 leader-follow / PURSUE 隊形；確認後直接針對敵機未來位置的包圍 slot 收斂。 A/B 高度以敵機即時相對高度為基準，不再固定以 30m 為中心。 4. 每台友機使用 C2 平滑且持續前進的 quintic Cartesian「微幅發散 → 收斂」虛擬軌跡，避免繞大圈。 5. 對所有 slot assignment（4 機時最多 4! = 24 種）評分： - 路徑長度 - 路線交叉 - 預測最小間距 - 頻繁交換 slot 的成本 6. 同層路徑交叉維持硬限制；間距不足時在持續前進中疊加分離修正。 A/B 跨層若水平距離過近且尚未建立垂直間隔，先原地垂直分層，再水平移動。 7. 包圍完成條件改為「每台友機均到達自己的 slot」，不是形心距敵機。 8. 敵機定位遺失超時時進 TARGET_LOST，將友機 hold 在當下位置，避免繼續追過期目標。 相容性 ====== - 保留既有 MissionExecutor 介面：start(plan) / update_plan(plan) / stop() - 保留 EngagementPanel 的主要 signal/API。 - __init__ 保留舊版 r_engage / r_engage_exit / lon_spacing / lat_offset 參數， 讓既有 GUI 呼叫端不必立刻全部修改；新策略不再依賴 leader-follow 的 spacing。 """ import math import time from enum import Enum from itertools import permutations, product from PyQt6.QtWidgets import ( QWidget, QVBoxLayout, QHBoxLayout, QLabel, QComboBox, QFrame, QPushButton, QDoubleSpinBox ) from PyQt6.QtCore import Qt, QObject, pyqtSignal # --------------------------------------------------------------------------- # 基礎工具 EARTH_R = 6371000.0 def _log(level, message): print(f"[{level}] {message}", flush=True) def _haversine(lat1, lon1, lat2, lon2): """兩經緯度間水平地面距離 (m)。""" p1 = math.radians(lat1) p2 = math.radians(lat2) dphi = math.radians(lat2 - lat1) dlam = math.radians(lon2 - lon1) a = (math.sin(dphi / 2.0) ** 2 + math.cos(p1) * math.cos(p2) * math.sin(dlam / 2.0) ** 2) return 2.0 * EARTH_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) * EARTH_R * math.cos(math.radians(ref_lat)) y = math.radians(lat - ref_lat) * EARTH_R return x, y def _m_to_ll(x, y, ref_lat, ref_lon): """局部平面公尺座標 -> 經緯度。""" lat = ref_lat + math.degrees(y / EARTH_R) cos_lat = max(1e-9, math.cos(math.radians(ref_lat))) lon = ref_lon + math.degrees(x / (EARTH_R * cos_lat)) return lat, lon def _norm2(x, y): return math.hypot(x, y) def _normalize(x, y, fallback=(1.0, 0.0)): n = math.hypot(x, y) if n < 1e-9: return fallback return x / n, y / n def _bezier_point(p0, p1, p2, p3, t): """2D cubic Bezier。pX=(x,y)。""" u = 1.0 - t b0 = u ** 3 b1 = 3.0 * u * u * t b2 = 3.0 * u * t * t b3 = t ** 3 return ( b0 * p0[0] + b1 * p1[0] + b2 * p2[0] + b3 * p3[0], b0 * p0[1] + b1 * p1[1] + b2 * p2[1] + b3 * p3[1], ) def _smoothstep5(t): """Quintic smoothstep: 0/1 端點的一、二階導數皆為 0。""" t = min(1.0, max(0.0, float(t))) return t * t * t * (t * (t * 6.0 - 15.0) + 10.0) def _wrap_pi(a): """角度包到 [-pi, pi)。""" return (a + math.pi) % (2.0 * math.pi) - math.pi def _polyline_length(points): total = 0.0 for a, b in zip(points, points[1:]): total += math.hypot(b[0] - a[0], b[1] - a[1]) return total def _orientation(a, b, c): return ((b[0] - a[0]) * (c[1] - a[1]) - (b[1] - a[1]) * (c[0] - a[0])) def _segments_intersect(a, b, c, d, eps=1e-9): """一般位置線段相交測試；對任務規劃足夠。""" o1 = _orientation(a, b, c) o2 = _orientation(a, b, d) o3 = _orientation(c, d, a) o4 = _orientation(c, d, b) # 只把真正穿越當成 crossing；共線/端點接觸交給 minimum separation 處理。 return ((o1 > eps and o2 < -eps) or (o1 < -eps and o2 > eps)) and \ ((o3 > eps and o4 < -eps) or (o3 < -eps and o4 > eps)) # --------------------------------------------------------------------------- # 預設參數 DEFAULT_R_DETECT = 40.0 DEFAULT_DETECT_DWELL = 1.0 # 以下兩個保留作為舊呼叫端相容參數；新策略不用「距離到某半徑才切追擊/包圍」。 DEFAULT_R_ENGAGE = 25.0 DEFAULT_R_ENGAGE_EXIT = 35.0 # 新策略建議值 DEFAULT_CIRCLE_RADIUS = 10.0 # 最終包圍半徑 (m)；近場會以敵機當下位置為圓心 DEFAULT_FORMATION_ALT = 30.0 # 敵機高度缺失時的安全備援值 (m，相對 home) DEFAULT_ALT_LAYER_GAP = 10.0 # A/B 兩層的垂直間隔 (m) DEFAULT_MIN_ALT_FLOOR = 10.0 # 最低安全相對高度 (m) DEFAULT_CROSS_LAYER_HOLD_DISTANCE = 12.0 # A/B 水平距離小於此值時先確認垂直分層 (m) DEFAULT_CROSS_LAYER_MIN_VERTICAL_GAP = 7.0 # 跨層近距離時要求的最小實際垂直間隔 (m) DEFAULT_CROSS_LAYER_ALT_TOLERANCE = 1.0 # 垂直分層完成容許誤差 (m) DEFAULT_SPREAD_DISTANCE = 3.0 # 小幅外擴上限 (m)：避免繞大圈，只保留必要的發散 DEFAULT_APPROACH_MARGIN = 4.0 # 保留相容參數；新 Cartesian 平滑路徑不再強制繞到圈外 staging DEFAULT_LOOKAHEAD_T = 0.35 # 遠距離時的 Bezier 前視比例 DEFAULT_FINAL_DIRECT_DISTANCE = 4.0 # 只在最後 4m 直接送 slot；避免中途提早進入減速 DEFAULT_LEAD_FADE_START = 30.0 # 平均距敵 <= 此距離開始逐步減少敵機前置預測 DEFAULT_LEAD_FADE_END = 18.0 # 平均距敵 <= 此距離完全取消前置，圓心貼住敵機當下位置 DEFAULT_CENTER_SMOOTH_ALPHA = 0.25 # 規劃圓心低通係數，降低 GPS/敵速造成的路徑抖動 DEFAULT_LOOKAHEAD_DISTANCE = 14.0 # 保持較遠 carrot point，避免反覆追短 waypoint 而減速 DEFAULT_PATH_SAMPLES = 41 # 路徑/交叉預測取樣數；提高以降低漏判 DEFAULT_SAFE_SEPARATION = 8.0 # 硬安全間距 (m) DEFAULT_FORMATION_SPACING = 12.0 # 移動中希望逐步拉到的隊形間距；比硬安全距離更早開始展開 DEFAULT_INITIAL_SEP_TOLERANCE = 0.5 # 起始若小於 8m，允許最多只再縮短這麼多 (m) DEFAULT_EMERGENCY_SEPARATION = 3.0 # 無論如何不可低於的水平緊急下限 (m) DEFAULT_RECOVERY_SPREAD_STEP = 2.0 # 舊版保底：無 assignment 時的小幅外擴 (m) DEFAULT_SEPARATION_CORRECTION_GAIN = 1.25 # 更積極的移動中隊形修正 DEFAULT_SEPARATION_CORRECTION_MAX = 5.0 # 最大側向修正；仍保證主要前進分量 DEFAULT_DIRECTION_SMOOTH_ALPHA = 0.35 # 每 tick 接受新方向的比例；越小越平滑 DEFAULT_MAX_LATERAL_RATIO = 0.45 # 側向/前進最大比例，避免修隊形時原地橫移 DEFAULT_LAYER_LANE_SPACING = 4.0 # 同層同心圓車道的半徑間隔 (m) DEFAULT_LAYER_AVOID_DISTANCE = 14.0 # 同層距離小於此值開始啟用同心圓避讓 DEFAULT_LAYER_LANE_FADE_DISTANCE = 12.0 # 距最終 slot 小於此值時逐步收回共同包圍半徑 DEFAULT_HEADING_SMOOTH_ALPHA = 0.25 # 敵機 heading 低通，避免 X 陣形因航向抖動快速旋轉 DEFAULT_ENCIRCLE_TOLERANCE = 3.0 # 每台距自己 slot <= 此值才算完成包圍 DEFAULT_REASSIGN_IMPROVEMENT = 0.20 # 安全時至少改善 20% 才允許換 slot 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 = 3.0 DEFAULT_ENEMY_VEL_WINDOW = 1.0 DEFAULT_TARGET_LOST_TIMEOUT = 5.0 # assignment cost DEFAULT_CROSSING_PENALTY = 20000.0 DEFAULT_SEPARATION_PENALTY = 30000.0 DEFAULT_SLOT_SWITCH_PENALTY = 8.0 # 舊版相容參數：保留但不再當 leader-follow spacing 使用 DEFAULT_LON_SPACING = 8.0 DEFAULT_LAT_OFFSET = 5.0 _NONE_LABEL = "（未選擇）" class MissionPhase(Enum): IDLE = "idle" WATCH = "watch" DETECTED = "detected" CONVERGE = "converge" # 動態分配 slot + 發散/收斂曲線接近 ENCIRCLE = "encircle" # 所有友機已進入自己的 slot TARGET_LOST = "target_lost" # 目標定位逾時，友機 hold # --------------------------------------------------------------------------- # Mission Orchestrator class MissionOrchestrator(QObject): """ 任務狀態機。 callback： make_executor() -> MissionExecutor gps_provider() -> {drone_id: {'lat','lon','alt'}} stop_groups_fn() -> 操作員確認重編隊時，停止既有 A/B 群組任務 新策略： WATCH -> DETECTED -> CONVERGE -> ENCIRCLE 進入 CONVERGE 後保留 A/B 分層：A/B 各自固定在敵機高度上下層； 同層必要時使用同心圓半徑車道避讓。跨層近距離時先垂直、後水平。 """ phase_changed = pyqtSignal(str) detection_prompt = pyqtSignal(str) status_message = pyqtSignal(str) def __init__( self, make_executor, gps_provider, stop_groups_fn=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_gap=DEFAULT_ALT_LAYER_GAP, 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, spread_distance=DEFAULT_SPREAD_DISTANCE, approach_margin=DEFAULT_APPROACH_MARGIN, lookahead_t=DEFAULT_LOOKAHEAD_T, final_direct_distance=DEFAULT_FINAL_DIRECT_DISTANCE, lead_fade_start=DEFAULT_LEAD_FADE_START, lead_fade_end=DEFAULT_LEAD_FADE_END, center_smooth_alpha=DEFAULT_CENTER_SMOOTH_ALPHA, lookahead_distance=DEFAULT_LOOKAHEAD_DISTANCE, path_samples=DEFAULT_PATH_SAMPLES, safe_separation=DEFAULT_SAFE_SEPARATION, formation_spacing=DEFAULT_FORMATION_SPACING, initial_sep_tolerance=DEFAULT_INITIAL_SEP_TOLERANCE, emergency_separation=DEFAULT_EMERGENCY_SEPARATION, recovery_spread_step=DEFAULT_RECOVERY_SPREAD_STEP, separation_correction_gain=DEFAULT_SEPARATION_CORRECTION_GAIN, separation_correction_max=DEFAULT_SEPARATION_CORRECTION_MAX, direction_smooth_alpha=DEFAULT_DIRECTION_SMOOTH_ALPHA, max_lateral_ratio=DEFAULT_MAX_LATERAL_RATIO, layer_lane_spacing=DEFAULT_LAYER_LANE_SPACING, layer_avoid_distance=DEFAULT_LAYER_AVOID_DISTANCE, layer_lane_fade_distance=DEFAULT_LAYER_LANE_FADE_DISTANCE, heading_smooth_alpha=DEFAULT_HEADING_SMOOTH_ALPHA, encircle_tolerance=DEFAULT_ENCIRCLE_TOLERANCE, reassign_improvement=DEFAULT_REASSIGN_IMPROVEMENT, reassign_cooldown_sec=DEFAULT_REASSIGN_COOLDOWN, target_lost_timeout_sec=DEFAULT_TARGET_LOST_TIMEOUT, crossing_penalty=DEFAULT_CROSSING_PENALTY, separation_penalty=DEFAULT_SEPARATION_PENALTY, slot_switch_penalty=DEFAULT_SLOT_SWITCH_PENALTY, parent=None, cross_layer_hold_distance=DEFAULT_CROSS_LAYER_HOLD_DISTANCE, cross_layer_min_vertical_gap=DEFAULT_CROSS_LAYER_MIN_VERTICAL_GAP, cross_layer_alt_tolerance=DEFAULT_CROSS_LAYER_ALT_TOLERANCE, ): super().__init__(parent) self._make_executor = make_executor self._gps_provider = gps_provider self._stop_groups_fn = stop_groups_fn self.r_detect = float(r_detect) self.detect_dwell_sec = float(detect_dwell_sec) # legacy/compatibility self.r_engage = float(r_engage) self.r_engage_exit = float(r_engage_exit) self.lon_spacing = float(lon_spacing) self.lat_offset = float(lat_offset) self.replan_move_m = float(replan_move_m) self.replan_cap_sec = float(replan_cap_sec) self.circle_radius = float(circle_radius) self.formation_alt = float(formation_alt) self.alt_layer_gap = float(alt_layer_gap) self.min_alt_floor = float(min_alt_floor) self.cross_layer_hold_distance = max( 0.0, float(cross_layer_hold_distance) ) self.cross_layer_min_vertical_gap = max( 0.0, float(cross_layer_min_vertical_gap) ) self.cross_layer_alt_tolerance = max( 0.0, float(cross_layer_alt_tolerance) ) 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.spread_distance = float(spread_distance) self.approach_margin = float(approach_margin) self.lookahead_t = min(0.95, max(0.05, float(lookahead_t))) self.final_direct_distance = max(0.0, float(final_direct_distance)) self.lead_fade_start = max(0.0, float(lead_fade_start)) self.lead_fade_end = max(0.0, float(lead_fade_end)) self.center_smooth_alpha = min(1.0, max(0.02, float(center_smooth_alpha))) self.lookahead_distance = max(1.0, float(lookahead_distance)) if self.lead_fade_end > self.lead_fade_start: self.lead_fade_end, self.lead_fade_start = self.lead_fade_start, self.lead_fade_end self.path_samples = max(7, int(path_samples)) self.safe_separation = float(safe_separation) self.formation_spacing = max(self.safe_separation, float(formation_spacing)) self.initial_sep_tolerance = max(0.0, float(initial_sep_tolerance)) self.emergency_separation = max(0.5, float(emergency_separation)) self.recovery_spread_step = max(0.5, float(recovery_spread_step)) self.separation_correction_gain = max(0.0, float(separation_correction_gain)) self.separation_correction_max = max(0.0, float(separation_correction_max)) self.direction_smooth_alpha = min(1.0, max(0.05, float(direction_smooth_alpha))) self.max_lateral_ratio = min(1.0, max(0.05, float(max_lateral_ratio))) self.layer_lane_spacing = max(0.5, float(layer_lane_spacing)) self.layer_avoid_distance = max(self.safe_separation, float(layer_avoid_distance)) self.layer_lane_fade_distance = max(1.0, float(layer_lane_fade_distance)) self.heading_smooth_alpha = min(1.0, max(0.02, float(heading_smooth_alpha))) self.encircle_tolerance = float(encircle_tolerance) self.reassign_improvement = min(0.9, max(0.0, float(reassign_improvement))) self.reassign_cooldown_sec = float(reassign_cooldown_sec) self.target_lost_timeout_sec = float(target_lost_timeout_sec) self.crossing_penalty = float(crossing_penalty) self.separation_penalty = float(separation_penalty) self.slot_switch_penalty = float(slot_switch_penalty) self.phase = MissionPhase.IDLE self.a_drone_ids = [] self.b_drone_ids = [] self.enemy_id = None self._detect_since = 0.0 # 確認後的角色無關友機集合 self._friendly_ids = [] self._merged_ex = None # assignment: {drone_id: slot_index} self._assignment = {} self._last_assignment_change_at = 0.0 self._last_assignment_had_conflict = False # 敵機運動估計 self._enemy_hist = [] self._enemy_vel = (0.0, 0.0) # east, north m/s self._enemy_alt = None # 敵機即時相對高度；缺失時才用 formation_alt self._enemy_heading_deg = None # 0°=North, clockwise；X 陣形以此為基準 self._last_enemy_seen_at = 0.0 # replan / command state self._last_replan_at = 0.0 self._last_plan_enemy = None self._last_predicted_center = None self._last_slots = None self._smoothed_center = None # 每架持續前進方向；避免每次 replan 重新生成 carrot 時航向跳動/煞停 self._motion_dirs = {} self._slot_groups = {} # {slot_index: "A"/"B"} self._vertical_hold_ids = set() # ------------------------------------------------------------------ 設定 def configure(self, a_drone_ids, b_drone_ids, enemy_id): """ WATCH/DETECTED 階段可更新 A/B 與敵機。 一旦進入 CONVERGE/ENCIRCLE，角色集合鎖定，避免 GUI 下拉變動破壞任務。 """ if self.phase in ( MissionPhase.CONVERGE, MissionPhase.ENCIRCLE, MissionPhase.TARGET_LOST, ): 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 self._enemy_hist = [] self._enemy_vel = (0.0, 0.0) self._enemy_alt = None self._enemy_heading_deg = None self.enemy_id = new_enemy return True def _configured(self): friendly = set(self.a_drone_ids) | set(self.b_drone_ids) return ( bool(self.enemy_id) and bool(self.a_drone_ids) and self.enemy_id not in friendly and not (set(self.a_drone_ids) & set(self.b_drone_ids)) ) # ------------------------------------------------------------------ 操作員確認 / 停止 def confirm_detection(self): """ DETECTED -> CONVERGE。 1. 停止既有 A/B 群組任務。 2. A/B 身分解除，合併為單一 friendly 集合。 3. 計算預測敵機中心與圓形 slots。 4. 做全域 slot assignment。 5. 產生 Bezier lookahead point，啟動合併 executor。 """ if self.phase != MissionPhase.DETECTED: self.status_message.emit("目前不在待確認狀態") return gps = self._gps_provider() epos = gps.get(self.enemy_id) 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: self.status_message.emit("沒有可用友機") return if self._positions_for(friendly_ids, gps) is None: self.status_message.emit("有友機尚未定位，無法開始包圍") return if self._stop_groups_fn: try: self._stop_groups_fn() except Exception as e: _log("WARN", f"[orchestrator] 停止群組任務失敗: {e}") self._friendly_ids = friendly_ids self._assignment = {} self._last_assignment_change_at = 0.0 self._last_assignment_had_conflict = False self._enemy_hist = [] self._enemy_vel = (0.0, 0.0) self._enemy_alt = None self._enemy_heading_deg = None self._smoothed_center = None self._motion_dirs = {} self._slot_groups = {} # {slot_index: "A"/"B"} self._vertical_hold_ids = set() self._last_enemy_seen_at = self._position_received_at(epos, now) self._update_enemy_velocity(epos) center = self._planning_center(gps, epos, reset=True) slots = self._circle_slots_for_center(center, len(self._friendly_ids)) assignment, info = self._choose_assignment(gps, center, slots, force=True) self._merged_ex = self._make_executor() if not assignment: # 找不到完全滿足間距的方案時，不再先停/預分散。 # 改採 0-crossing 的最低風險 provisional assignment， # 一邊朝 slot 前進、一邊疊加 separation correction 拉開隊形。 assignment, info = self._choose_progress_assignment(gps, center, slots) if not assignment: self.status_message.emit("目前找不到 0-crossing 路徑，暫不開始包圍") self._merged_ex = None return self._assignment = assignment self._last_assignment_change_at = time.monotonic() self._last_assignment_had_conflict = True targets = self._moving_converge_targets(gps, center, slots, assignment) if targets is None: self.status_message.emit("無法建立移動中隊形調整目標") self._merged_ex = None return self._merged_ex.start(self._targets_to_plan(targets)) self.status_message.emit( "起始間距不足：保持前進，同時在移動中逐步拉開隊形" ) else: self._assignment = assignment self._last_assignment_change_at = time.monotonic() self._last_assignment_had_conflict = info['conflict'] targets = self._moving_converge_targets(gps, center, slots, assignment) self._merged_ex.start(self._targets_to_plan(targets)) self._last_predicted_center = center self._last_slots = slots self._mark_replanned(epos) self._set_phase(MissionPhase.CONVERGE) if assignment: self.status_message.emit("開始動態包圍：slot 鎖定，移動中同步展開隊形") def stop(self): """停止任務並回 IDLE。""" if self._merged_ex is not None: try: self._merged_ex.stop() except Exception: pass self._merged_ex = None self._friendly_ids = [] self._assignment = {} self._detect_since = 0.0 self._enemy_hist = [] self._enemy_vel = (0.0, 0.0) self._enemy_alt = None self._enemy_heading_deg = None self._last_enemy_seen_at = 0.0 self._last_predicted_center = None self._last_slots = None self._smoothed_center = None self._motion_dirs = {} self._slot_groups = {} # {slot_index: "A"/"B"} self._vertical_hold_ids = set() self._set_phase(MissionPhase.IDLE) self.status_message.emit("任務已停止") # ------------------------------------------------------------------ tick def tick(self): """由 GUI 週期呼叫，建議 5~10 Hz。""" 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.CONVERGE: self._tick_converge() elif self.phase == MissionPhase.ENCIRCLE: self._tick_encircle() elif self.phase == MissionPhase.TARGET_LOST: self._tick_target_lost() def _tick_watch(self): gps = self._gps_provider() epos = gps.get(self.enemy_id) now = time.monotonic() if not epos or not self._position_is_fresh(epos, now): self._detect_since = 0.0 return nearest_id = None nearest_d = None for did in self.a_drone_ids: pos = gps.get(did) 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 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) self.detection_prompt.emit( f"偵測到敵機（{nearest_id} 距 {nearest_d:.1f} m）— 請確認動態包圍" ) else: self._detect_since = 0.0 def _tick_converge(self): gps = self._gps_provider() epos = gps.get(self.enemy_id) now = time.monotonic() 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 = self._position_received_at(epos, now) self._update_enemy_velocity(epos) if self._positions_for(self._friendly_ids, gps) is None: return if self._all_in_slots(gps, epos): self._set_phase(MissionPhase.ENCIRCLE) self.status_message.emit("所有友機已進入各自包圍 slot") # 進 ENCIRCLE 當下仍送一次精確 slot self._update_encircle_command(gps, epos, force=True) return if self._should_replan(now, epos): self._update_converge_command(gps, epos) def _tick_encircle(self): gps = self._gps_provider() epos = gps.get(self.enemy_id) now = time.monotonic() 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 = self._position_received_at(epos, now) self._update_enemy_velocity(epos) if self._positions_for(self._friendly_ids, gps) is None: return # 敵機持續移動，圓心跟著預測位置移動。 # 若任何友機明顯脫離自己的 slot，回 CONVERGE 重新做曲線收斂。 if not self._all_in_slots(gps, epos, tolerance=self.encircle_tolerance * 1.8): self._set_phase(MissionPhase.CONVERGE) self.status_message.emit("包圍幾何失配，重新收斂") self._update_converge_command(gps, epos, force_assignment=False) return if self._should_replan(now, epos): self._update_encircle_command(gps, epos) def _tick_target_lost(self): gps = self._gps_provider() epos = gps.get(self.enemy_id) 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 = self._position_received_at(epos, now) self._update_enemy_velocity(epos) self._set_phase(MissionPhase.CONVERGE) self.status_message.emit("敵機定位恢復，重新建立包圍軌跡") self._update_converge_command(gps, epos, force_assignment=True) # ------------------------------------------------------------------ 敵機遺失 / hold def _handle_enemy_missing(self, gps, now): if self._last_enemy_seen_at <= 0.0: self._last_enemy_seen_at = now return if now - self._last_enemy_seen_at < self.target_lost_timeout_sec: return self._hold_current_positions(gps) self._set_phase(MissionPhase.TARGET_LOST) self.status_message.emit("敵機定位逾時：友機保持目前位置") def _hold_current_positions(self, gps): if self._merged_ex is None: return targets = {} now = time.monotonic() for did in self._friendly_ids: p = gps.get(did) 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, current_alt), ) if len(targets) == len(self._friendly_ids): self._merged_ex.update_plan(self._targets_to_plan(targets)) # ------------------------------------------------------------------ 敵機速度 / 預測中心 def _set_enemy_heading(self, heading_deg): """以最短角度差低通更新敵機 heading，正確處理 359°/0° 邊界。""" heading_deg = float(heading_deg) % 360.0 if self._enemy_heading_deg is None: self._enemy_heading_deg = heading_deg return err = ((heading_deg - self._enemy_heading_deg + 180.0) % 360.0) - 180.0 self._enemy_heading_deg = ( self._enemy_heading_deg + self.heading_smooth_alpha * err ) % 360.0 def _update_enemy_velocity(self, epos): """更新敵機高度/heading，並以 GPS 差分估水平速度 (east, north) m/s。""" try: enemy_alt = float(epos.get('alt')) except (TypeError, ValueError): enemy_alt = None if enemy_alt is not None and math.isfinite(enemy_alt): self._enemy_alt = enemy_alt # heading convention：0°=North、90°=East、順時針增加。 # 優先讀 heading/hdg/course；yaw 若像 radians 就自動轉 degree。 raw_heading = None for key in ('heading', 'hdg', 'course'): try: v = epos.get(key) if v is not None and math.isfinite(float(v)): v = float(v) if key == 'hdg' and abs(v) > 360.0: v *= 0.01 raw_heading = v % 360.0 break except (TypeError, ValueError): pass if raw_heading is None: try: v = epos.get('yaw') if v is not None and math.isfinite(float(v)): v = float(v) raw_heading = (math.degrees(v) if abs(v) <= 2.0 * math.pi + 0.2 else v) % 360.0 except (TypeError, ValueError): pass if raw_heading is not None: self._set_enemy_heading(raw_heading) 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) self._enemy_vel = (dx / dt, dy / dt) # 若 provider 沒有 heading，以每次更新的水平航跡方向作持續備援； # 不能只在第一次設定，否則敵機轉彎後 X 陣形不會跟著旋轉。 if raw_heading is None: evx, evy = self._enemy_vel if math.hypot(evx, evy) > 0.3: course_deg = math.degrees(math.atan2(evx, evy)) % 360.0 self._set_enemy_heading(course_deg) def enemy_speed(self): return math.hypot(*self._enemy_vel) def _predict_enemy_center(self, gps, epos): """ 預測「包圍圓未來中心」，而不是給 leader 一個追擊點。 lead time 取友機到敵機平均距離 / friendly_speed，並封頂。 """ gaps = [] for did in self._friendly_ids: p = gps.get(did) if p: gaps.append(_haversine(p['lat'], p['lon'], epos['lat'], epos['lon'])) avg_gap = sum(gaps) / len(gaps) if gaps else 0.0 raw_lead_t = min( self.lead_time_cap_sec, avg_gap / max(0.1, self.friendly_speed), ) # 遠距離保留前置攔截；接近包圍圈時逐步把前置量淡出。 # 這樣最終包圍圓會貼著敵機當下位置，不會因敵機速度 × lead time # 讓視覺上/實際上看起來像半徑被拉到 20~30 m。 if self.lead_fade_start <= self.lead_fade_end + 1e-6: lead_scale = 0.0 if avg_gap <= self.lead_fade_end else 1.0 elif avg_gap >= self.lead_fade_start: lead_scale = 1.0 elif avg_gap <= self.lead_fade_end: lead_scale = 0.0 else: lead_scale = (avg_gap - self.lead_fade_end) / ( self.lead_fade_start - self.lead_fade_end ) lead_t = raw_lead_t * lead_scale evx, evy = self._enemy_vel plat, plon = _m_to_ll(evx * lead_t, evy * lead_t, epos['lat'], epos['lon']) return plat, plon def _planning_center(self, gps, epos, reset=False): """ 對預測圓心做低通。只在真正產生命令/初始規劃時呼叫，避免同一 tick 多次查詢時重複濾波。這能明顯降低 moving target/GPS 抖動造成的曲線折感。 """ raw = self._predict_enemy_center(gps, epos) if reset or self._smoothed_center is None: self._smoothed_center = raw return raw plat, plon = self._smoothed_center dx, dy = _ll_to_m(raw[0], raw[1], plat, plon) a = self.center_smooth_alpha filt = _m_to_ll(dx * a, dy * a, plat, plon) self._smoothed_center = filt return filt # ------------------------------------------------------------------ slot 生成 / 高度 def _group_of(self, did): if did in self.a_drone_ids: return "A" if did in self.b_drone_ids: return "B" return None def _group_ids(self, group): src = self.a_drone_ids if group == "A" else self.b_drone_ids friendly = set(self._friendly_ids) return [d for d in src if d in friendly] def _layer_alt(self, group): """以敵機即時高度為中心：A 在上層、B 在下層。""" base_alt = ( self._enemy_alt if self._enemy_alt is not None else self.formation_alt ) half = self.alt_layer_gap / 2.0 lower = max(self.min_alt_floor, base_alt - half) # 敵機高度過低而觸發最低高度限制時，仍保留完整 A/B 垂直間隔。 upper = max(self.min_alt_floor, base_alt + half, lower + self.alt_layer_gap) return upper if group == "A" else lower @staticmethod def _position_alt(pos): """讀取有效相對高度；缺值或非有限值回傳 None。""" if not pos: return None try: alt = float(pos.get('alt')) except (TypeError, ValueError): 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 友機。 只有 A/B 水平距離小於 cross_layer_hold_distance 時才介入；待實際 高度符合 A 在 B 上方，且垂直差接近規劃層差後，才解除水平鎖定。 """ hold = set() a_ids = [d for d in self._group_ids("A") if gps.get(d)] b_ids = [d for d in self._group_ids("B") if gps.get(d)] required_gap = min( self._layer_alt("A") - self._layer_alt("B"), self.cross_layer_min_vertical_gap, ) for aid in a_ids: apos = gps[aid] for bid in b_ids: bpos = gps[bid] horizontal = _haversine( apos['lat'], apos['lon'], bpos['lat'], bpos['lon'] ) if horizontal >= self.cross_layer_hold_distance: continue a_alt = self._position_alt(apos) b_alt = self._position_alt(bpos) # 高度缺失時採保守策略：不允許近距離跨層水平接近。 if a_alt is None or b_alt is None or a_alt - b_alt < required_gap: hold.update((aid, bid)) return hold def _apply_cross_layer_vertical_gate(self, targets, gps): """把跨層近距離且未完成垂直分離的水平目標鎖在當前位置。""" hold = self._cross_layer_vertical_hold_ids(gps) for did in hold: p = gps.get(did) if not p or did not in targets: continue targets[did] = (p['lat'], p['lon'], self._layer_alt(self._group_of(did))) self._motion_dirs.pop(did, None) if hold != self._vertical_hold_ids: if hold: labels = ", ".join(sorted(hold)) self.status_message.emit( f"A/B 近距離：{labels} 先垂直分層，再恢復水平移動" ) elif self._vertical_hold_ids: self.status_message.emit("A/B 垂直分層完成，恢復水平移動") self._vertical_hold_ids = hold return targets def _slot_alt(self, slot_index): group = self._slot_groups.get(slot_index, "A") return self._layer_alt(group) def _circle_slots_for_center(self, center, n): """ 以敵機機體座標系建立真正的 X 形包圍。 body-frame： forward = 敵機 heading 0° 軸 right = 敵機右側 90° 軸 四機、A/B 各兩機： A 層：front-right、rear-left B 層：front-left、rear-right heading=0° 時，四個 slot 固定落在 NE / NW / SW / SE 四個對角方向。 """ clat, clon = center slots = {} self._slot_groups = {} heading_deg = self._enemy_heading_deg if self._enemy_heading_deg is not None else 0.0 h = math.radians(heading_deg) # Local map frame: x=East, y=North # heading: 0°=North, 90°=East, clockwise fx, fy = math.sin(h), math.cos(h) rx, ry = math.cos(h), -math.sin(h) s = 1.0 / math.sqrt(2.0) body_dirs = { "front_right": ((fx + rx) * s, (fy + ry) * s), "front_left": ((fx - rx) * s, (fy - ry) * s), "rear_left": ((-fx - rx) * s, (-fy - ry) * s), "rear_right": ((-fx + rx) * s, (-fy + ry) * s), } group_names = { "A": ("front_right", "rear_left"), "B": ("front_left", "rear_right"), } idx = 0 for group in ("A", "B"): gids = self._group_ids(group) m = len(gids) if m <= 0: continue if m == 2: dirs = [body_dirs[name] for name in group_names[group]] else: base_deg = 45.0 if group == "A" else -45.0 dirs = [] for j in range(m): off = math.radians(base_deg + 360.0 * j / m) bx = math.sin(off) by = math.cos(off) dx = bx * rx + by * fx dy = bx * ry + by * fy dirs.append((dx, dy)) for dx, dy in dirs: x = self.circle_radius * dx y = self.circle_radius * dy lat, lon = _m_to_ll(x, y, clat, clon) slots[idx] = (lat, lon, self._layer_alt(group)) self._slot_groups[idx] = group idx += 1 return slots def _lane_offset_for(self, did): """同一層內給每架固定的同心圓半徑車道，避免車道在飛行中交換。""" group = self._group_of(did) gids = self._group_ids(group) if did not in gids or len(gids) <= 1: return 0.0 i = gids.index(did) return (i - (len(gids) - 1) / 2.0) * self.layer_lane_spacing # ------------------------------------------------------------------ Bezier 路徑 def _assignment_target_angles(self, assignment, gps, center, slots): """ 把 slot 角度展開成與友機目前角度相同的連續角域。 當 assignment 保持環形順序時，所有友機的角度插值全程保持順序， 不會因為跨過 -pi/pi 邊界而突然反向或互相超車。 """ rows = [] for did in self._friendly_ids: p = gps.get(did) if not p or did not in assignment: return None x, y = _ll_to_m(p['lat'], p['lon'], center[0], center[1]) rows.append((math.atan2(y, x), did)) rows.sort(key=lambda z: z[0]) src = [r[0] for r in rows] raw_targets = [] prev = None for _, did in rows: slot = slots[assignment[did]] sx, sy = _ll_to_m(slot[0], slot[1], center[0], center[1]) th = math.atan2(sy, sx) if prev is not None: while th <= prev: th += 2.0 * math.pi raw_targets.append(th) prev = th # 全體只加同一個 2pi 倍數，讓總旋轉量最小，同時保持拓樸順序。 best = None best_err = float('inf') for k in range(-2, 3): cand = [th + k * 2.0 * math.pi for th in raw_targets] err = sum((cand[i] - src[i]) ** 2 for i in range(len(src))) if err < best_err: best_err, best = err, cand return {rows[i][1]: best[i] for i in range(len(rows))} def _trajectory_for(self, did, slot_index, gps, center, slots, assignment=None): """ C2 平滑、近似最短路徑的 Cartesian 收斂軌跡。 舊版使用極座標 theta/r 插值，當起點與 slot 的方位角差較大時， 即使沒有交叉，也可能沿敵機外圍繞出很大的弧。 新版改成： base(t) = P0 -> P3 的 quintic smoothstep offset(t) = 小幅「向外」bump 因此主體接近兩點間的直接路徑，只保留少量發散來增加機間分離； crossing / safe separation 仍由全局 assignment 的硬限制負責。 quintic smoothstep 使起終點的一、二階導數為 0，保持平滑。 """ p = gps.get(did) if not p: return None if assignment is None: assignment = self._assignment clat, clon = center p0 = _ll_to_m(p['lat'], p['lon'], clat, clon) slot = slots[slot_index] p3 = _ll_to_m(slot[0], slot[1], clat, clon) dx = p3[0] - p0[0] dy = p3[1] - p0[1] dist_to_slot = math.hypot(dx, dy) # 外擴方向：以敵機/包圍中心為原點，由目前位置向外。 # 若目前位置非常接近中心，退化成與直達方向垂直的方向。 ox, oy = _normalize(p0[0], p0[1], fallback=_normalize(-dy, dx)) # 不再隨距離大幅外擴。 # 最多 spread_distance（預設 3m），近距離時再自動縮小。 bump = min( self.spread_distance, max(0.0, 0.08 * dist_to_slot), ) points = [] for i in range(self.path_samples): t = i / (self.path_samples - 1) q = _smoothstep5(t) # 近似直達的 C2 smoothstep 主路徑。 bx = p0[0] + dx * q by = p0[1] + dy * q # 小幅外擴：首尾為 0，中間最大。 # 使用 q^2(1-q)^2 讓 offset 在兩端也非常柔順。 shape = 16.0 * q * q * (1.0 - q) * (1.0 - q) x = bx + ox * bump * shape y = by + oy * bump * shape points.append((x, y)) return { 'p0': points[0], 'p3': points[-1], 'points': points, 'length': _polyline_length(points), } def _lookahead_target(self, did, slot_index, gps, center, slots): traj = self._trajectory_for( did, slot_index, gps, center, slots, assignment=self._assignment ) if traj is None: return None dist_to_slot = math.hypot( traj['p3'][0] - traj['p0'][0], traj['p3'][1] - traj['p0'][1], ) if dist_to_slot <= self.final_direct_distance: slot = slots[slot_index] return slot[0], slot[1], slot[2] # 固定「公尺」前視，比固定參數 t 更不容易因曲線長短改變而突然加減速。 want = min(self.lookahead_distance, max(6.0, 0.70 * dist_to_slot)) acc = 0.0 chosen = traj['points'][-1] for a, b in zip(traj['points'], traj['points'][1:]): seg = math.hypot(b[0] - a[0], b[1] - a[1]) if acc + seg >= want and seg > 1e-9: u = (want - acc) / seg chosen = (a[0] + u * (b[0] - a[0]), a[1] + u * (b[1] - a[1])) break acc += seg lat, lon = _m_to_ll(chosen[0], chosen[1], center[0], center[1]) 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'])) q = _smoothstep5(frac) alt = cur_alt + q * (target_alt - cur_alt) return lat, lon, max(self.min_alt_floor, alt) def _converge_targets(self, gps, center, slots, assignment): targets = {} for did in self._friendly_ids: sidx = assignment.get(did) if sidx is None: return None target = self._lookahead_target(did, sidx, gps, center, slots) if target is None: return None targets[did] = target return targets def _separation_correction_xy(self, did, gps, ref_lat, ref_lon): """目前太靠近其他友機時，計算小幅排斥修正；不取代前進目標，只疊加在其上。""" p = gps.get(did) if not p: return 0.0, 0.0 x, y = _ll_to_m(p['lat'], p['lon'], ref_lat, ref_lon) cx = cy = 0.0 my_group = self._group_of(did) for j, oid in enumerate(self._friendly_ids): if oid == did or self._group_of(oid) != my_group: continue op = gps.get(oid) if not op: continue ox, oy = _ll_to_m(op['lat'], op['lon'], ref_lat, ref_lon) dx, dy = x - ox, y - oy d = math.hypot(dx, dy) # 8m 是硬安全門檻，但隊形不等到 8m 才開始整理； # 在 formation_spacing（預設 12m）內就逐步往外展開。 if d >= self.formation_spacing: continue if d < 1e-6: # 完全重合時用固定但每機不同的方向，避免除零。 idx = self._friendly_ids.index(did) th = 2.0 * math.pi * idx / max(1, len(self._friendly_ids)) ux, uy = math.cos(th), math.sin(th) d = 0.0 else: ux, uy = dx / d, dy / d # 以「隊形希望間距」而非硬安全距離算修正，因此會更早開始展開。 deficit = self.formation_spacing - d closeness = deficit / max(0.1, self.formation_spacing) boost = 1.0 + 0.7 * closeness mag = 0.5 * self.separation_correction_gain * deficit * boost cx += ux * mag cy += uy * mag n = math.hypot(cx, cy) if n > self.separation_correction_max > 0.0: k = self.separation_correction_max / n cx *= k cy *= k return cx, cy def _segments_cross_for_targets(self, gps, targets, ref_lat, ref_lon): """只檢查『當前位置 -> 下一個 lookahead』短線段是否彼此相交。""" ids = [d for d in self._friendly_ids if d in targets and gps.get(d)] segs = {} for did in ids: p = gps[did] a = _ll_to_m(p['lat'], p['lon'], ref_lat, ref_lon) t = targets[did] b = _ll_to_m(t[0], t[1], ref_lat, ref_lon) segs[did] = (a, b) for i in range(len(ids)): for j in range(i + 1, len(ids)): if self._group_of(ids[i]) != self._group_of(ids[j]): continue if _segments_intersect(segs[ids[i]][0], segs[ids[i]][1], segs[ids[j]][0], segs[ids[j]][1]): return True return False def _layer_conflict_flags(self, gps, center, slots, assignment): """判斷各高度層是否需要啟用同心圓車道。跨層完全不互相觸發。""" flags = {"A": False, "B": False} ref_lat, ref_lon = center for group in ("A", "B"): gids = [d for d in self._group_ids(group) if d in assignment and gps.get(d)] for i in range(len(gids)): di = gids[i] pi = gps[di] aix = _ll_to_m(pi['lat'], pi['lon'], ref_lat, ref_lon) si = slots[assignment[di]] bix = _ll_to_m(si[0], si[1], ref_lat, ref_lon) for j in range(i + 1, len(gids)): dj = gids[j] pj = gps[dj] ajx = _ll_to_m(pj['lat'], pj['lon'], ref_lat, ref_lon) sj = slots[assignment[dj]] bjx = _ll_to_m(sj[0], sj[1], ref_lat, ref_lon) d = math.hypot(aix[0] - ajx[0], aix[1] - ajx[1]) if d < self.layer_avoid_distance or _segments_intersect(aix, bix, ajx, bjx): flags[group] = True break if flags[group]: break return flags def _concentric_lane_carrot(self, did, p, slot, center, dist_to_slot): """ 同層避讓用的同心圓 carrot。 - 每架固定一條暫時半徑車道。 - 沿角向前進，同時平滑靠近該車道半徑。 - 接近最終 slot 時 lane offset 淡出為 0，回到共同包圍半徑。 """ ref_lat, ref_lon = center px, py = _ll_to_m(p['lat'], p['lon'], ref_lat, ref_lon) sx, sy = _ll_to_m(slot[0], slot[1], ref_lat, ref_lon) r = max(0.5, math.hypot(px, py)) th = math.atan2(py, px) sth = math.atan2(sy, sx) dth = _wrap_pi(sth - th) # 遠處完整使用分層車道；進入 slot 前逐步收回共同半徑。 fade = min(1.0, max(0.0, (dist_to_slot - self.final_direct_distance) / max(1.0, self.layer_lane_fade_distance - self.final_direct_distance) )) lane_r = max(2.0, self.circle_radius + self._lane_offset_for(did) * fade) horizon = min(self.lookahead_distance, max(4.0, dist_to_slot)) # 約 70% 用於角向前進、30% 用於半徑收斂，避免突然徑向橫切。 arc_budget = 0.72 * horizon radial_budget = 0.35 * horizon th_step = min(abs(dth), arc_budget / max(2.0, r)) nth = th + (1.0 if dth >= 0.0 else -1.0) * th_step dr = max(-radial_budget, min(radial_budget, lane_r - r)) nr = max(2.0, r + dr) tx = nr * math.cos(nth) ty = nr * math.sin(nth) return tx, ty def _moving_converge_targets(self, gps, center, slots, assignment): """ A/B 分層持續移動控制： - A/B 高度層跟隨敵機即時高度。 - 跨層近距離且垂直間隔不足時，先鎖住水平位置完成分層。 - 同層若過近或直達線可能交叉，切換到不同半徑的同心圓車道。 - 非跨層垂直分離狀態時，持續保持前方 carrot。 """ ref_lat, ref_lon = center layer_avoid = self._layer_conflict_flags(gps, center, slots, assignment) raw = {} for did in self._friendly_ids: p = gps.get(did) sidx = assignment.get(did) if not p or sidx is None: return None slot = slots[sidx] px, py = _ll_to_m(p['lat'], p['lon'], ref_lat, ref_lon) sx, sy = _ll_to_m(slot[0], slot[1], ref_lat, ref_lon) vx, vy = sx - px, sy - py dist = math.hypot(vx, vy) if dist <= self.final_direct_distance: raw[did] = (slot[0], slot[1], self._layer_alt(self._group_of(did))) continue group = self._group_of(did) if layer_avoid.get(group, False): tx, ty = self._concentric_lane_carrot(did, p, slot, center, dist) dux, duy = _normalize(tx - px, ty - py) else: dux, duy = _normalize(vx, vy) horizon = min(self.lookahead_distance, dist) tx, ty = px + dux * horizon, py + duy * horizon # 同層近距離排斥仍保留，但只取側向，不會抵消前進分量。 rx, ry = self._separation_correction_xy(did, gps, ref_lat, ref_lon) dot = rx * dux + ry * duy lx, ly = rx - dot * dux, ry - dot * duy ln = math.hypot(lx, ly) cap = min(self.separation_correction_max, self.max_lateral_ratio * self.lookahead_distance) if ln > cap > 0.0: k = cap / ln lx *= k; ly *= k tx += lx ty += ly ndx, ndy = _normalize(tx - px, ty - py, fallback=(dux, duy)) prev = self._motion_dirs.get(did) if prev is not None: a = self.direction_smooth_alpha ndx, ndy = _normalize(prev[0]*(1-a)+ndx*a, prev[1]*(1-a)+ndy*a, fallback=(ndx, ndy)) h = min(self.lookahead_distance, max(4.0, dist)) tx, ty = px + ndx*h, py + ndy*h self._motion_dirs[did] = (ndx, ndy) lat, lon = _m_to_ll(tx, ty, ref_lat, ref_lon) target_alt = self._layer_alt(group) 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)) return self._apply_cross_layer_vertical_gate(raw, gps) def _recovery_spread_targets(self, gps): """ 找不到安全 slot assignment 時的短距離預分散。 每架只沿「友軍形心 -> 自己」方向往外移 recovery_spread_step； 不繞敵機、不做大圓，只把原本過密的起始幾何稍微拉開。 """ pts = [] for did in self._friendly_ids: p = gps.get(did) if not p: return None pts.append((p['lat'], p['lon'])) if not pts: return None ref_lat = sum(p[0] for p in pts) / len(pts) ref_lon = sum(p[1] for p in pts) / len(pts) xy = {} for did in self._friendly_ids: p = gps[did] xy[did] = _ll_to_m(p['lat'], p['lon'], ref_lat, ref_lon) cx = sum(v[0] for v in xy.values()) / len(xy) cy = sum(v[1] for v in xy.values()) / len(xy) targets = {} n = len(self._friendly_ids) for idx, did in enumerate(self._friendly_ids): x, y = xy[did] ox, oy = x - cx, y - cy if math.hypot(ox, oy) < 1e-6: th = 2.0 * math.pi * idx / max(1, n) ox, oy = math.cos(th), math.sin(th) ox, oy = _normalize(ox, oy) 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) 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 # ------------------------------------------------------------------ assignment / crossing / separation def _assignment_cost(self, assignment, gps, center, slots): """ 評估一個完整 assignment。 crossing 仍是硬限制。水平間距改成「漸進安全」： - 若兩機起始已 >= safe_separation，後續不得低於 safe_separation。 - 若起始本來 < safe_separation，不要求瞬間跳到 8m；只要求路徑不要 比起始距離再惡化超過 initial_sep_tolerance，並且永遠不得低於 emergency_separation。 這避免四機原本只隔 4~7m 時，所有 assignment 在第一個 sample 就被 8m 硬門檻全部淘汰。 """ trajectories = {} total = 0.0 for did in self._friendly_ids: sidx = assignment[did] traj = self._trajectory_for(did, sidx, gps, center, slots, assignment=assignment) if traj is None: return float('inf'), True, 0, 0.0 trajectories[did] = traj total += traj['length'] old = self._assignment.get(did) if old is not None and old != sidx: total += self.slot_switch_penalty crossing_count = 0 separation_violation = False min_sep = float('inf') ids = self._friendly_ids for i in range(len(ids)): for j in range(i + 1, len(ids)): # A/B 高度層不同：只對同層做水平 crossing / separation 約束。 if self._group_of(ids[i]) != self._group_of(ids[j]): continue a = trajectories[ids[i]]['points'] b = trajectories[ids[j]]['points'] # 幾何 crossing：從頭到尾都不允許真正穿越。 crossed = False for a0, a1 in zip(a, a[1:]): if crossed: break for b0, b1 in zip(b, b[1:]): if _segments_intersect(a0, a1, b0, b1): crossed = True break if crossed: crossing_count += 1 total += self.crossing_penalty # 起始實際水平距離。 initial_sep = math.hypot(a[0][0] - b[0][0], a[0][1] - b[0][1]) # 已經足夠遠：維持完整 8m；本來較近：不能再明顯靠得更近。 if initial_sep >= self.safe_separation: required_sep = self.safe_separation else: required_sep = max( self.emergency_separation, initial_sep - self.initial_sep_tolerance, ) # 同 normalized t 的預測水平距離。略過 t=0，因為那是既成狀態。 pair_min = float('inf') for k in range(1, min(len(a), len(b))): d = math.hypot(a[k][0] - b[k][0], a[k][1] - b[k][1]) pair_min = min(pair_min, d) min_sep = min(min_sep, pair_min) if pair_min < required_sep: separation_violation = True ratio = (required_sep - pair_min) / max(0.1, required_sep) total += self.separation_penalty * (1.0 + ratio) if min_sep == float('inf'): min_sep = 9999.0 conflict = crossing_count > 0 or separation_violation return total, conflict, crossing_count, min_sep def _cyclic_assignments(self, gps, center, slots): """A/B 各自在自己的高度層做 cyclic slot rotation，再取笛卡兒積組合。""" per_group = [] for group in ("A", "B"): ids = self._group_ids(group) if not ids: continue rows = [] for did in ids: p = gps.get(did) if not p: return [] x, y = _ll_to_m(p['lat'], p['lon'], center[0], center[1]) rows.append((math.atan2(y, x), did)) rows.sort(key=lambda z: z[0]) ordered_ids = [d for _, d in rows] slot_rows = [] for sidx, sp in slots.items(): if self._slot_groups.get(sidx) != group: continue x, y = _ll_to_m(sp[0], sp[1], center[0], center[1]) slot_rows.append((math.atan2(y, x), sidx)) slot_rows.sort(key=lambda z: z[0]) ordered_slots = [s for _, s in slot_rows] if len(ordered_slots) != len(ordered_ids): return [] group_candidates = [] m = len(ordered_ids) for shift in range(m): group_candidates.append({ ordered_ids[i]: ordered_slots[(i + shift) % m] for i in range(m) }) per_group.append(group_candidates) if not per_group: return [] out = [] for combo in product(*per_group): merged = {} for part in combo: merged.update(part) out.append(merged) return out def _group_permutation_assignments(self, slots): """只允許 A->A slots、B->B slots；小隊內可做完整 permutation 保底。""" per_group = [] for group in ("A", "B"): ids = self._group_ids(group) if not ids: continue sidxs = [s for s in slots if self._slot_groups.get(s) == group] if len(ids) != len(sidxs): return [] if len(ids) > 6: return [] gc = [] for perm in permutations(sidxs): gc.append({did: perm[i] for i, did in enumerate(ids)}) per_group.append(gc) if not per_group: return [] out=[] for combo in product(*per_group): d={} for part in combo: d.update(part) out.append(d) return out def _choose_assignment(self, gps, center, slots, force=False): """ Crossing 是硬限制，不再只是 penalty。 優先搜尋保持環形順序的 n 個 rotation；只有找不到安全解時才擴大到全部 permutation。任何 crossings > 0 或違反『漸進安全間距』的方案都不採用。 若目前 assignment 仍安全就鎖住，不為了省幾公尺頻繁換位。 """ ids = list(self._friendly_ids) n = len(ids) bad = {'conflict': True, 'crossings': 999, 'min_sep': 0.0, 'cost': float('inf')} if n == 0 or len(slots) != n: return None, bad # 一旦開始移動，只要目前 assignment 的「幾何路徑沒有交叉」就鎖定。 # 暫時的間距不足交給 moving separation correction 處理，不因此突然換 slot。 # 這可避免飛行途中目標位置跳到另一側而先煞停再重新排隊。 if self._assignment and not force: cost, conflict, crossings, min_sep = self._assignment_cost( self._assignment, gps, center, slots ) if crossings == 0: return dict(self._assignment), { 'cost': cost, 'conflict': conflict, 'crossings': 0, 'min_sep': min_sep, } candidates = self._cyclic_assignments(gps, center, slots) # cyclic 不足時只擴大到「各層內」的 permutation，不允許 A/B 交換高度層。 all_perm = self._group_permutation_assignments(slots) seen = {tuple(c.get(d) for d in ids) for c in candidates} candidates += [c for c in all_perm if tuple(c.get(d) for d in ids) not in seen] valid = [] for assignment in candidates: cost, conflict, crossings, min_sep = self._assignment_cost( assignment, gps, center, slots ) # 幾何不交叉 + 漸進安全間距，兩者都是硬條件。 if crossings == 0 and not conflict: valid.append((cost, -min_sep, assignment, { 'cost': cost, 'conflict': False, 'crossings': 0, 'min_sep': min_sep, })) if not valid: return None, bad valid.sort(key=lambda x: (x[0], x[1])) return valid[0][2], valid[0][3] def _choose_progress_assignment(self, gps, center, slots): """ 找不到完整安全間距方案時的移動中保底。 crossing 仍是硬限制：只從 0-crossing assignment 中選最低風險方案。 允許暫時低於 safe_separation，之後由 moving separation correction 在持續前進的同時把距離拉開。 """ ids = list(self._friendly_ids) n = len(ids) bad = {'conflict': True, 'crossings': 999, 'min_sep': 0.0, 'cost': float('inf')} if n == 0 or len(slots) != n: return None, bad candidates = self._cyclic_assignments(gps, center, slots) all_perm = self._group_permutation_assignments(slots) seen = {tuple(c.get(d) for d in ids) for c in candidates} candidates += [c for c in all_perm if tuple(c.get(d) for d in ids) not in seen] valid = [] for assignment in candidates: cost, conflict, crossings, min_sep = self._assignment_cost( assignment, gps, center, slots ) if crossings != 0: continue # 先偏好較大的預測最小間距，再看路徑成本；不接受幾何交叉。 valid.append((-min_sep, cost, assignment, { 'cost': cost, 'conflict': conflict, 'crossings': 0, 'min_sep': min_sep, })) if not valid: return None, bad valid.sort(key=lambda x: (x[0], x[1])) return valid[0][2], valid[0][3] def _greedy_assignment(self, gps, center, slots): """大於 7 機時的保底 nearest-slot greedy。""" remain = set(slots.keys()) assignment = {} for did in self._friendly_ids: p = gps.get(did) if not p: continue best_s = min( remain, key=lambda s: _haversine( p['lat'], p['lon'], slots[s][0], slots[s][1] ), ) assignment[did] = best_s remain.remove(best_s) return assignment # ------------------------------------------------------------------ command 更新 def _update_converge_command(self, gps, epos, force_assignment=False): if self._merged_ex is None: return center = self._planning_center(gps, epos) slots = self._circle_slots_for_center(center, len(self._friendly_ids)) new_assignment, info = self._choose_assignment( gps, center, slots, force=force_assignment ) if not new_assignment: # 若任務已經在移動中，優先沿用既有 slot，不在半路 hold。 # 只有「一開始尚未建立任何 assignment」時才找 provisional 0-crossing 配置。 if self._assignment: new_assignment = dict(self._assignment) _, conflict, crossings, min_sep = self._assignment_cost( new_assignment, gps, center, slots ) info = { 'cost': 0.0, 'conflict': conflict, 'crossings': crossings, 'min_sep': min_sep, } else: new_assignment, info = self._choose_progress_assignment(gps, center, slots) if not new_assignment: # 尚未開始飛行時若連 0-crossing topology 都沒有，才暫停。 self._hold_current_positions(gps) self.status_message.emit("起始暫停：目前連 0-crossing 路徑都不存在") return changed = new_assignment != self._assignment if changed: self._assignment = new_assignment self._last_assignment_change_at = time.monotonic() msg = ( f"slot 重新分配：cross={info['crossings']}，" f"預測最小間距={info['min_sep']:.1f} m" ) _log("INFO", f"[orchestrator] {msg}") self.status_message.emit(msg) self._last_assignment_had_conflict = info['conflict'] targets = self._moving_converge_targets(gps, center, slots, self._assignment) if targets is None: return self._merged_ex.update_plan(self._targets_to_plan(targets)) self._last_predicted_center = center self._last_slots = slots self._mark_replanned(epos) def _update_encircle_command(self, gps, epos, force=False): if self._merged_ex is None: return now = time.monotonic() if not force and not self._should_replan(now, epos): return center = self._planning_center(gps, epos) slots = self._circle_slots_for_center(center, len(self._friendly_ids)) # ENCIRCLE 內原則上不交換 slot，以免四機繞圈互換造成穿越。 # 只有現有 assignment 已無效才重新建立。 if not self._assignment or any(d not in self._assignment for d in self._friendly_ids): assignment, _ = self._choose_assignment(gps, center, slots, force=True) if assignment: self._assignment = assignment self._last_assignment_change_at = now targets = { did: slots[self._assignment[did]] for did in self._friendly_ids if did in self._assignment } if len(targets) != len(self._friendly_ids): return targets = self._apply_cross_layer_vertical_gate(targets, gps) self._merged_ex.update_plan(self._targets_to_plan(targets)) self._last_predicted_center = center self._last_slots = slots self._mark_replanned(epos) def _targets_to_plan(self, targets): return { 'drone_ids': list(self._friendly_ids), 'waypoints': [[targets[d]] for d in self._friendly_ids], 'rendezvous_indices': [], } # ------------------------------------------------------------------ 包圍完成判斷 def _all_in_slots(self, gps, epos, tolerance=None): if not self._assignment or not self._friendly_ids: return False tolerance = self.encircle_tolerance if tolerance is None else float(tolerance) # 水平 slot 即使已到達，只要近距離 A/B 尚未建立足夠垂直差，仍不可 # 宣告包圍完成或提前解除「先垂直、後水平」保護。 if self._cross_layer_vertical_hold_ids(gps): return False center = self._last_predicted_center or self._predict_enemy_center(gps, epos) slots = self._circle_slots_for_center(center, len(self._friendly_ids)) for did in self._friendly_ids: p = gps.get(did) sidx = self._assignment.get(did) if not p or sidx is None: return False slot = slots[sidx] d = _haversine(p['lat'], p['lon'], slot[0], slot[1]) if d > tolerance: return False current_alt = self._position_alt(p) if (current_alt is None or abs(current_alt - slot[2]) > self.cross_layer_alt_tolerance): return False return True # ------------------------------------------------------------------ replan 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 ) # ------------------------------------------------------------------ helpers def _positions_for(self, drone_ids, gps): out = [] now = time.monotonic() for did in drone_ids: p = gps.get(did) 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((lat, lon, alt)) return out @staticmethod def _unique_keep_order(items): seen = set() out = [] for x in items: if x in seen: continue seen.add(x) out.append(x) return out 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) # --------------------------------------------------------------------------- # Engagement Panel class EngagementPanel(QWidget): """ 交戰控制面板。 - 敵機下拉 + 即時敵機到最近友機距離 - A/B 僅用於偵察前的群組指定 - 偵測確認後由 MissionOrchestrator 合併為無角色 friendly 集合 - 速度比例 UI 保留，方便既有 GUI 發 DO_CHANGE_SPEED """ enemy_changed = pyqtSignal(str) confirm_requested = pyqtSignal() stop_requested = pyqtSignal() groups_changed = pyqtSignal(str, str) apply_speed_requested = pyqtSignal(float, float) def __init__(self, parent=None, r_detect=DEFAULT_R_DETECT, friendly_speed=DEFAULT_FRIENDLY_SPEED): super().__init__(parent) self.r_detect = float(r_detect) self._init_friendly_speed = float(friendly_speed) self._enemy_id = "" self._suppress_enemy = False self._suppress_group = False self._group_sig = None 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) mode_hint = QLabel( "偵測確認後：A/B 跟隨敵機高度分層；近距離先垂直後水平，" "同層以同心圓路徑避讓" ) mode_hint.setWordWrap(True) mode_hint.setStyleSheet("color: #888; font-size: 11px;") root.addWidget(mode_hint) 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) 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) 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)]。""" 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_configuration(self, enemy_id, a_group_id, b_group_id): """由 GUI 程式化選定敵機與 A/B 群組，並發出一次同步訊號。""" enemy_index = self.enemy_combo.findText(enemy_id) a_index = self.a_group_combo.findData(a_group_id) b_index = self.b_group_combo.findData(b_group_id) if min(enemy_index, a_index, b_index) < 0: return False self._suppress_enemy = True self._suppress_group = True try: self.enemy_combo.setCurrentIndex(enemy_index) self.a_group_combo.setCurrentIndex(a_index) self.b_group_combo.setCurrentIndex(b_index) self._enemy_id = enemy_id finally: self._suppress_enemy = False self._suppress_group = False self.enemy_changed.emit(enemy_id) self.groups_changed.emit(a_group_id, b_group_id) return True 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_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 = None nearest_d = 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;" ) # ------------------------------------------------------------------ UI callbacks 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)