chiyu1468/AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

2.9.0 交戰

Browse Source
This commit is contained in:
ken910606
2026-09-09 13:41:14 +08:00
parent 4d0d1366ac
commit a345436008
5 changed files with 2503 additions and 274 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/GUI/gui.py
+95 -9
View File
@@ -147,7 +147,7 @@ class ToggleSwitch(QWidget):
class ControlStationUI(QMainWindow):
planning_finished = pyqtSignal(object)
VERSION = '2.8.2'
VERSION = '2.9.0'
FONT_SCALE_MIN = 70
FONT_SCALE_MAX = 180
FONT_SCALE_DEFAULT = 100
@@ -246,6 +246,7 @@ class ControlStationUI(QMainWindow):
self.drone_positions = {}
self.drone_headings = {}
self.drone_heading_updated_at = {}
# 初始化地圖
self.drone_map = DroneMap()
self.drone_map.set_socket_colors(self.socket_colors)
@@ -277,7 +278,7 @@ class ControlStationUI(QMainWindow):
# ================================================================================
self.mission_planner = FormationPlanner(
spacing=5.0, # 5 公尺間距
base_altitude=25.0, # 基準高度 25 公尺
base_altitude=30.0, # 基準高度 30 公尺
altitude_diff=2.0 # 高低差 2 公尺
)
# ================================================================================
@@ -297,6 +298,7 @@ class ControlStationUI(QMainWindow):
self.active_group_id = None # 當前 active 的 group
self._group_counter = 0 # 用來產生 group_id
self._pending_box_assign = None # 框選後直接分配到的 group_id
self._auto_engagement_setup_applied = False
# ================================================================================
self.global_mission_defaults = dict(DEFAULT_MISSION_PARAM_VALUES)
self.font_scale = self.FONT_SCALE_DEFAULT / 100.0
@@ -1146,7 +1148,7 @@ class ControlStationUI(QMainWindow):
def handle_takeoff(self, drone_id):
loop = asyncio.get_event_loop()
future = self.monitor.takeoff_drone(drone_id, 25.0)
future = self.monitor.takeoff_drone(drone_id, 30.0)
loop.create_task(self.handle_service_response(future, f"起飛 {drone_id}"))
def handle_setpoint_selected(self):
@@ -1231,7 +1233,7 @@ class ControlStationUI(QMainWindow):
loop = asyncio.get_event_loop()
for drone_id in selected:
future = self.monitor.takeoff_drone(drone_id, 25.0)
future = self.monitor.takeoff_drone(drone_id, 30.0)
loop.create_task(self.handle_service_response(future, f"批次起飛 {drone_id}"))
# ================================================================================
@@ -2313,7 +2315,7 @@ class ControlStationUI(QMainWindow):
_log("INFO", f"地圖點擊: {lat:.6f}, {lon:.6f} -> Group {group.group_id} ({group.mission_type})")
panel = self.group_panels.get(group.group_id)
params = panel.get_mission_params() if panel else {}
base_alt = params.get('base_altitude', params.get('altitude', 25.0))
base_alt = params.get('base_altitude', params.get('altitude', 30.0))
target_gps = (lat, lon, base_alt)
self.statusBar().showMessage(
f"Group {group.group_id}: 正在規劃 {group.mission_type} ({len(selected_drones)} 台)", 2000)
@@ -2365,7 +2367,7 @@ class ControlStationUI(QMainWindow):
panel = self.group_panels.get(group.group_id)
params = panel.get_mission_params() if panel else {}
base_alt = params.get('altitude', 25.0)
base_alt = params.get('altitude', 30.0)
self.statusBar().showMessage(
f"Group {group.group_id}: 正在規劃 Grid Sweep ({len(selected_drones)} 台)", 2000)
drone_gps_positions = self._collect_drone_gps(selected_drones)
@@ -2426,7 +2428,7 @@ class ControlStationUI(QMainWindow):
panel = self.group_panels.get(group.group_id)
params = panel.get_mission_params() if panel else {}
base_alt = params.get('altitude', 25.0)
base_alt = params.get('altitude', 30.0)
self.statusBar().showMessage(
f"Group {group.group_id}: 正在規劃跟隨模式 ({len(selected_drones)} 台)", 2000)
@@ -2632,15 +2634,43 @@ class ControlStationUI(QMainWindow):
MissionPhase.IDLE.value: "待機（尚未設定敵機/A 組）",
MissionPhase.WATCH.value: "監看偵測中",
MissionPhase.DETECTED.value: "已偵測到敵機 — 等待確認重編隊",
MissionPhase.PURSUE.value: "前進中（4 機 leader-follow）",
MissionPhase.CONVERGE.value: "收斂中（動態分配包圍位置）",
MissionPhase.ENCIRCLE.value: "包圍中",
MissionPhase.TARGET_LOST.value: "敵機定位遺失（友機保持位置）",
}
def _engagement_gps_provider(self):
"""
提供交戰編排器完整的定位快照。
GPS 與 VFR_HUD/heading 是不同訊息來源；monitor.drone_gps 原本只有
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)
}
now = time.monotonic()
for drone_id, heading in self.drone_headings.items():
updated_at = self.drone_heading_updated_at.get(drone_id)
heading_is_fresh = (
isinstance(updated_at, (int, float))
and now - updated_at <= 2.0
)
if (drone_id in snapshot
and isinstance(heading, (int, float))
and heading_is_fresh):
snapshot[drone_id]['heading'] = float(heading) % 360.0
return snapshot
def _init_orchestrator(self):
"""建立 MissionOrchestrator，用 callback 與 GUI 解耦。"""
self.orchestrator = MissionOrchestrator(
make_executor=self._make_orchestrator_executor,
gps_provider=lambda: self.monitor.drone_gps,
gps_provider=self._engagement_gps_provider,
stop_groups_fn=self._stop_engagement_group_missions,
)
self._engagement_a_group = None
@@ -2776,6 +2806,61 @@ class ControlStationUI(QMainWindow):
# 同步敵機下拉選單（保留當前選擇）
if hasattr(self, 'engagement_panel'):
self.engagement_panel.refresh_drone_list(list(self.drones.keys()))
self._try_auto_engagement_setup()
def _try_auto_engagement_setup(self):
"""sysid 11~15 全部出現時，自動建立 ABC 編組並設定交戰面板。"""
if self._auto_engagement_setup_applied:
return
drones_by_sysid = {}
for did in sorted(self.drones):
match = re.fullmatch(r's\d+_(\d+)', did)
if match:
drones_by_sysid.setdefault(int(match.group(1)), did)
required_sysids = {11, 12, 13, 14, 15}
if not required_sysids.issubset(drones_by_sysid):
return
# 正常啟動時已有 A；補建 B、C。
while self._group_counter < 3:
self._add_mission_group()
if not {'A', 'B', 'C'}.issubset(self.mission_groups):
_log("WARN", "自動交戰編組失敗：找不到完整的 Group A/B/C")
return
assignments = {
'A': {drones_by_sysid[11], drones_by_sysid[12]},
'B': {drones_by_sysid[14], drones_by_sysid[15]},
'C': {drones_by_sysid[13]},
}
for gid, members in assignments.items():
group = self.mission_groups[gid]
if group.executor:
group.executor.stop()
group.selected_drone_ids = set(members)
group.planned_waypoints = None
group.mission_target = None
panel = self.group_panels.get(gid)
if panel:
panel.update_drone_list()
panel.update_status()
panel.clear_mission_info()
self._auto_engagement_setup_applied = True
self.refresh_selection_ui()
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]
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）",
6000)
def reorganize_socket_groups(self):
while self.drone_panel_layout.count():
@@ -2986,6 +3071,7 @@ class ControlStationUI(QMainWindow):
heading = hud_data.get('heading')
if isinstance(heading, (int, float)):
self.drone_headings[drone_id] = heading
self.drone_heading_updated_at[drone_id] = time.monotonic()
self._map_dirty_drones.add(drone_id)
groundspeed = hud_data.get('groundspeed')
airspeed = hud_data.get('airspeed')
src/GUI/mission_group.py
+5 -5
View File
@@ -24,10 +24,10 @@ GROUP_COLORS = [
DEFAULT_MISSION_PARAM_VALUES = {
'spacing': '5.0',
'base_altitude': '25.0',
'base_altitude': '30.0',
'altitude_diff': '2.0',
'radius': '10.0',
'altitude': '25.0',
'altitude': '30.0',
'start_angle': '0',
'lateral_offset': '3.0',
'longitudinal_spacing': '5.0',
@@ -249,8 +249,8 @@ class GroupPanel(QWidget):
takeoff_row.setSpacing(3)
self.alt_input = QComboBox()
self.alt_input.setEditable(True)
self.alt_input.addItems(["5", "10", "15", "20", "25"])
self.alt_input.setCurrentText("25")
self.alt_input.addItems(["5", "10", "15", "20", "25", "30"])
self.alt_input.setCurrentText("30")
self.alt_input.setStyleSheet(COMBO)
alt_lbl = QLabel("m")
alt_lbl.setStyleSheet(LBL)
@@ -679,5 +679,5 @@ class GroupPanel(QWidget):
try:
alt = float(self.alt_input.currentText())
except ValueError:
alt = 25.0
alt = 30.0
self.takeoff_requested.emit(self.group.group_id, alt)
src/GUI/mission_orchestrator.py
+1691 -258
View File
File diff suppressed because it is too large Load Diff
src/GUI/mission_orchestrator1.py
+710
View File
@@ -0,0 +1,710 @@
#!/usr/bin/env python3
"""
任務編排器 (MissionOrchestrator) —— ABC 對抗任務的階段狀態機。
角色：坐在既有 MissionExecutor 之上的「任務大腦」。偵察飛行由操作員用既有的
群組執行按鈕控制（A 組 GRID_SWEEP、B 組待機）；orchestrator 只做兩件事：
1. 被動監看「A↔敵機」距離，達 R_detect 持續 dwell → latch，提示操作員。
2. 操作員按「重編隊」→ 接管 A+B 四機，用「瞬時隊形」追移動敵機、足夠近時展開包圍。
追蹤採「瞬時隊形」而非路徑跟隨：每 tick 給每台一個目標點（此刻該站的隊形位置），
敵機移動就更新。避免了路徑跟隨在移動目標下 index 歸零/往後外推造成的往返振盪。
階段：IDLE → WATCH（監看偵測）→ DETECTED（latch，等確認）
→ PURSUE（4 機瞬時 leader-follow 朝敵，B 在後方 slot 自然追上會合）
→ ENCIRCLE（形心距敵 ≤ R_engage 展開包圍，圓心追敵；逃出 R_engage_exit 退回）
設計定案見 memory: design_mission_orchestrator。修改限定 src/GUI/（scope_gui_only）。
"""
import math
import time
from enum import Enum
from PyQt6.QtWidgets import (
QWidget, QVBoxLayout, QHBoxLayout, QLabel, QComboBox, QFrame, QPushButton,
QDoubleSpinBox
)
from PyQt6.QtCore import Qt, QObject, pyqtSignal
def _log(level, message):
print(f"[{level}] {message}", flush=True)
def _haversine(lat1, lon1, lat2, lon2):
"""兩經緯度間的水平地面距離 (m)。"""
R = 6371000.0
p1 = math.radians(lat1)
p2 = math.radians(lat2)
dphi = math.radians(lat2 - lat1)
dlam = math.radians(lon2 - lon1)
a = (math.sin(dphi / 2) ** 2
+ math.cos(p1) * math.cos(p2) * math.sin(dlam / 2) ** 2)
return 2 * 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) * 6371000.0 * math.cos(math.radians(ref_lat))
y = math.radians(lat - ref_lat) * 6371000.0
return x, y
def _m_to_ll(x, y, ref_lat, ref_lon):
"""本地平面公尺 → 經緯度。"""
lat = ref_lat + math.degrees(y / 6371000.0)
lon = ref_lon + math.degrees(x / (6371000.0 * math.cos(math.radians(ref_lat))))
return lat, lon
# 預設參數
DEFAULT_R_DETECT = 40.0 # 偵測範圍 (m)
DEFAULT_DETECT_DWELL = 1.0 # 偵測 dwell (s)
DEFAULT_R_ENGAGE = 25.0 # 接戰半徑 (m)：形心↔敵 ≤ 此值 → 切包圍
DEFAULT_R_ENGAGE_EXIT = 35.0 # 包圍退出半徑 (m)：> 此值 → 退回前進（hysteresis）
DEFAULT_REPLAN_MOVE_M = 2.0 # 敵機位移 ≥ 此值即重算（moving target）
DEFAULT_REPLAN_CAP_SEC = 1.0 # 重算週期上限 (s)
DEFAULT_CIRCLE_RADIUS = 15.0 # 包圍半徑 (m)
DEFAULT_LON_SPACING = 8.0 # 隊形縱向間距 (m)
DEFAULT_LAT_OFFSET = 5.0 # 隊形橫向錯開 (m)
DEFAULT_FORMATION_ALT = 30.0 # 隊形基準高度 (m，相對 home)
DEFAULT_ALT_LAYER_GAP = 10.0 # A/B 上下層垂直間隔 (m)：A=基準+gap/2、B=基準-gap/2
DEFAULT_MIN_ALT_FLOOR = 10.0 # 相對 home 的最低安全高度 (m)：B 下層不得低於此值
DEFAULT_LEAD_TIME_CAP = 4.0 # 前置攔截 lead_time 上限 (s)
DEFAULT_FRIENDLY_SPEED = 5.0 # 友機假設水平速度 (m/s)：估 lead_time = gap/此值
DEFAULT_ENEMY_VEL_WINDOW = 1.0 # 敵速估計的 GPS 差分視窗 (s)
_NONE_LABEL = "（未選擇）"
class MissionPhase(Enum):
IDLE = "idle" # 未設定敵機/A 組
WATCH = "watch" # 監看偵測（偵察飛行由操作員的群組任務負責）
DETECTED = "detected" # 偵測 latch，等操作員確認重編隊
PURSUE = "pursue" # 4 機瞬時 leader-follow 朝敵機
ENCIRCLE = "encircle" # 4 機包圍敵機
class MissionOrchestrator(QObject):
"""
階段狀態機。透過注入的 callback 與 GUI 解耦（便於 headless 測試）：
make_executor() -> MissionExecutor（complete_stops=False，hover-hold）
gps_provider() -> {drone_id: {'lat','lon','alt'}}
stop_groups_fn() -> 停掉操作員的 A/B 群組任務（重編隊時避免雙重送點）
"""
phase_changed = pyqtSignal(str) # MissionPhase.value
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,
parent=None):
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)
self.r_engage = float(r_engage)
self.r_engage_exit = float(r_engage_exit)
self.replan_move_m = float(replan_move_m)
self.replan_cap_sec = float(replan_cap_sec)
self.circle_radius = float(circle_radius)
self.lon_spacing = float(lon_spacing)
self.lat_offset = float(lat_offset)
self.formation_alt = float(formation_alt)
self.alt_layer_gap = float(alt_layer_gap)
self.min_alt_floor = float(min_alt_floor)
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.phase = MissionPhase.IDLE
self.a_drone_ids = [] # 偵察組
self.b_drone_ids = [] # 待機組
self.enemy_id = None
self._detect_since = 0.0
self._merged_ids = [] # PURSUE/ENCIRCLE 的 4 機（領隊優先，固定順序）
self._merged_ex = None # 合併 executor（hover-hold）
self._last_replan_at = 0.0
self._last_plan_enemy = None # 上次重算用的敵機 (lat, lon)
self._last_heading = (0.0, 1.0) # 上次隊形朝向（敵機太近時沿用，避免抖動）
self._enemy_hist = [] # [(t, lat, lon)] 敵機軌跡（估速用）
self._enemy_vel = (0.0, 0.0) # 估得的敵速 (east, north) m/s
# ------------------------------------------------------------- 設定（可連續呼叫）
def configure(self, a_drone_ids, b_drone_ids, enemy_id):
"""設定 A/B 組與敵機。監看/待機階段可隨時更新；進入 PURSUE 後鎖定不受影響。"""
if self.phase in (MissionPhase.PURSUE, MissionPhase.ENCIRCLE):
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 # 換敵機重置 dwell
self.enemy_id = new_enemy
return True
def _configured(self):
return bool(self.enemy_id) and bool(self.a_drone_ids) \
and self.enemy_id not in set(self.a_drone_ids) | set(self.b_drone_ids) \
and not (set(self.a_drone_ids) & set(self.b_drone_ids))
# ------------------------------------------------------------- 重編隊 / 停止
def confirm_detection(self):
"""DETECTED → PURSUE：操作員手動確認。停掉群組掃描，接管 4 機瞬時追蹤。"""
if self.phase != MissionPhase.DETECTED:
self.status_message.emit("目前不在待確認狀態")
return
gps = self._gps_provider()
epos = gps.get(self.enemy_id)
if not epos:
self.status_message.emit("敵機定位遺失，無法重編隊")
return
merged = self._merged_ids_leader_first(gps, epos)
if self._positions_for(merged, gps) is None:
self.status_message.emit("有無人機尚未定位，無法重編隊")
return
# 先停掉操作員的 A/B 群組任務，避免與合併 executor 雙重送點塞爆 adapter
if self._stop_groups_fn:
try:
self._stop_groups_fn()
except Exception as e:
_log("WARN", f"[orchestrator] 停止群組任務失敗: {e}")
self._merged_ids = merged
self._last_heading = (0.0, 1.0)
self._enemy_hist = []
self._enemy_vel = (0.0, 0.0)
self._update_enemy_velocity(epos)
slots = self._pursue_slots(gps, epos)
self._merged_ex = self._make_executor()
self._merged_ex.start(self._slots_to_plan(slots))
self._mark_replanned(epos)
self._set_phase(MissionPhase.PURSUE)
self.status_message.emit("重編隊確認：4 機前進中（B 追上會合）")
def stop(self):
"""全體停止，回 IDLE。"""
if self._merged_ex is not None:
try:
self._merged_ex.stop()
except Exception:
pass
self._merged_ex = None
self._merged_ids = []
self._detect_since = 0.0
self._enemy_hist = []
self._enemy_vel = (0.0, 0.0)
self._set_phase(MissionPhase.IDLE)
self.status_message.emit("任務已停止")
# ------------------------------------------------------------- tick
def tick(self):
"""GUI 週期呼叫。"""
# 自動進出監看（偵察飛行由操作員群組任務負責，這裡只監看偵測）
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.PURSUE:
self._tick_pursue()
elif self.phase == MissionPhase.ENCIRCLE:
self._tick_encircle()
def _tick_watch(self):
gps = self._gps_provider()
epos = gps.get(self.enemy_id)
if not epos:
self._detect_since = 0.0
return
nearest_id, nearest_d = None, None
for did in self.a_drone_ids:
pos = gps.get(did)
if not pos:
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
elif now - self._detect_since >= self.detect_dwell_sec:
self._set_phase(MissionPhase.DETECTED) # latch，等操作員手動確認
self.detection_prompt.emit(
f"偵測到敵機（{nearest_id} 距 {nearest_d:.1f} m）— 請確認重編隊")
else:
self._detect_since = 0.0
def _tick_pursue(self):
gps = self._gps_provider()
epos = gps.get(self.enemy_id)
if not epos:
return # 敵機定位遺失（S4 加 stale-timeout hold）
self._update_enemy_velocity(epos)
d = self._centroid_dist(gps, epos)
if d is None:
return
if d <= self.r_engage:
self._apply_slots(self._circle_slots(gps, epos), epos)
self._set_phase(MissionPhase.ENCIRCLE)
self.status_message.emit("進入包圍")
return
if self._should_replan(time.monotonic(), epos):
self._apply_slots(self._pursue_slots(gps, epos), epos)
def _tick_encircle(self):
gps = self._gps_provider()
epos = gps.get(self.enemy_id)
if not epos:
return
self._update_enemy_velocity(epos)
d = self._centroid_dist(gps, epos)
if d is None:
return
if d > self.r_engage_exit:
self._apply_slots(self._pursue_slots(gps, epos), epos)
self._set_phase(MissionPhase.PURSUE)
self.status_message.emit("敵機逃離，退回前進")
return
if self._should_replan(time.monotonic(), epos):
self._apply_slots(self._circle_slots(gps, epos), epos)
# ------------------------------------------------------------- 高度分層
def _alt_for(self, did):
"""S3 高度分層：A 組（含領隊）走上層、B 組走下層（不低於地面下限）。
同高度繞圈/追擊會交叉撞機，分層後 A/B 垂直錯開自然避開。"""
if did in set(self.b_drone_ids):
return max(self.min_alt_floor,
self.formation_alt - self.alt_layer_gap / 2.0)
return self.formation_alt + self.alt_layer_gap / 2.0
# ------------------------------------------------------------- 前置攔截（估敵速）
def _update_enemy_velocity(self, epos):
"""用敵機 GPS 差分估水平速度 (east, north) m/s，視窗 enemy_vel_window_sec。"""
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) # sample0→sample1 位移 (m)
self._enemy_vel = (dx / dt, dy / dt)
def enemy_speed(self):
"""估得的敵機水平速率 (m/s)。"""
return math.hypot(*self._enemy_vel)
def _aim_point(self, gps, epos):
"""前置攔截點：敵位 + 敵速×lead_time。lead_time = gap/friendly_speed（封頂
lead_time_cap_sec）。敵速尚未估到時退化為瞄當下位置（等同原純追尾）。"""
evx, evy = self._enemy_vel
lp = gps.get(self._merged_ids[0]) if self._merged_ids else None
gap = (_haversine(lp['lat'], lp['lon'], epos['lat'], epos['lon'])
if lp else 0.0)
lead_t = min(self.lead_time_cap_sec, gap / max(self.friendly_speed, 0.1))
return _m_to_ll(evx * lead_t, evy * lead_t, epos['lat'], epos['lon'])
# ------------------------------------------------------------- 瞬時隊形
def _pursue_slots(self, gps, epos):
"""朝『前置攔截點』的瞬時 leader-follow：領隊朝攔截點，僚機沿隊軸後方 rank×間距、左右錯開。"""
ids = self._merged_ids
tlat, tlon = self._aim_point(gps, epos) # 前置攔截點取代敵機當下位置
lp = gps.get(ids[0])
if lp is None:
return None
lx, ly = _ll_to_m(lp['lat'], lp['lon'], tlat, tlon) # 領隊相對攔截點
hx, hy = -lx, -ly # 領隊 → 攔截點
n = math.hypot(hx, hy)
if n < 1.0:
h = self._last_heading # 太近，沿用上次朝向避免抖動
else:
h = (hx / n, hy / n)
self._last_heading = h
px, py = -h[1], h[0] # 左垂直
slots = {}
for rank, d in enumerate(ids):
lat_sign = -1 if rank % 2 == 0 else 1
# 領隊(rank0)目標≈攔截點；僚機沿 -h 後退 rank×間距，並左右錯開
sx = -h[0] * (rank * self.lon_spacing) + px * (lat_sign * self.lat_offset)
sy = -h[1] * (rank * self.lon_spacing) + py * (lat_sign * self.lat_offset)
lat, lon = _m_to_ll(sx, sy, tlat, tlon)
slots[d] = (lat, lon, self._alt_for(d))
return slots
def _circle_slots(self, gps, epos):
"""繞敵機的瞬時包圍：N 機均分在半徑 circle_radius 的圓周上。"""
ids = self._merged_ids
elat, elon = epos['lat'], epos['lon']
n = len(ids)
slots = {}
for i, d in enumerate(ids):
theta = 2.0 * math.pi * i / max(1, n)
x = self.circle_radius * math.cos(theta)
y = self.circle_radius * math.sin(theta)
lat, lon = _m_to_ll(x, y, elat, elon)
slots[d] = (lat, lon, self._alt_for(d))
return slots
def _apply_slots(self, slots, epos):
"""把瞬時 slot 熱替換給合併 executor 並更新重算基準。"""
if slots is None or self._merged_ex is None:
return
self._merged_ex.update_plan(self._slots_to_plan(slots))
self._mark_replanned(epos)
def _slots_to_plan(self, slots):
return {
'drone_ids': list(self._merged_ids),
'waypoints': [[slots[d]] for d in self._merged_ids],
'rendezvous_indices': [],
}
# ------------------------------------------------------------- 內部
def _centroid_dist(self, gps, epos):
m_pos = self._positions_for(self._merged_ids, gps)
if m_pos is None:
return None
cen_lat = sum(p[0] for p in m_pos) / len(m_pos)
cen_lon = sum(p[1] for p in m_pos) / len(m_pos)
return _haversine(cen_lat, cen_lon, epos['lat'], epos['lon'])
def _positions_for(self, drone_ids, gps):
out = []
for did in drone_ids:
p = gps.get(did)
if not p:
return None
out.append((p['lat'], p['lon'], p.get('alt', 0.0)))
return out
def _merged_ids_leader_first(self, gps, epos):
"""A+B 合併；A 中離敵最近者當領隊(rank0)，其餘 A 維持原序，再接 B。"""
def dist(did):
p = gps.get(did)
return float('inf') if not p else _haversine(
epos['lat'], epos['lon'], p['lat'], p['lon'])
if not self.a_drone_ids:
return list(self.b_drone_ids)
leader = min(self.a_drone_ids, key=dist)
rest_a = [d for d in self.a_drone_ids if d != leader]
return [leader] + rest_a + list(self.b_drone_ids)
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)
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)
class EngagementPanel(QWidget):
"""
交戰控制面板。
- 敵機下拉 + 即時「敵機↔最近友機」距離
- A/B 組指派下拉
- 階段顯示、重編隊 / 停止 按鈕
偵察飛行用既有的群組執行按鈕控制，這裡不含「開始」。面板只發訊號，邏輯在 GUI/orchestrator。
"""
enemy_changed = pyqtSignal(str) # drone_id（未選擇=空字串）
confirm_requested = pyqtSignal() # 重編隊
stop_requested = pyqtSignal()
groups_changed = pyqtSignal(str, str) # (a_group_id, b_group_id)
apply_speed_requested = pyqtSignal(float, float) # (friendly_speed, enemy_ratio)
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 # 目前下拉內容的簽章，變了才重建（避免 5Hz 狂刷）
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)
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)
# 速度比例限速：友方 V、敵方 r×V，按鈕一次套用（DO_CHANGE_SPEED）
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)]。只有內容變動才重建，避免 5Hz 狂刷。"""
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_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_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, nearest_d = None, 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;")
# ----------------------------------------------------------------- 內部
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)
src/GUI/mission_planner.py
+2 -2
View File
@@ -58,7 +58,7 @@ class FormationPlanner:
"""隊形規劃器"""
def __init__(self, spacing: float = 5.0,
base_altitude: float = 25.0,
base_altitude: float = 30.0,
altitude_diff: float = 2.0):
self.spacing = spacing
self.base_altitude = base_altitude
@@ -179,7 +179,7 @@ class FormationPlanner:
params = params or {}
N = len(drone_positions)
radius = params.get('radius', 10.0)
altitude = params.get('altitude', 25.0)
altitude = params.get('altitude', 30.0)
start_angle = params.get('start_angle', 0.0)
center_x, center_y, center_z = target_point