From e205f567e8824dd70bf181f414ea9a375a7895d7 Mon Sep 17 00:00:00 2001 From: wenchun Date: Mon, 24 Aug 2026 18:17:48 +0800 Subject: [PATCH] =?UTF-8?q?feat(GUI):=20=E4=BA=A4=E6=88=B0=E9=AB=98?= =?UTF-8?q?=E5=BA=A6=E5=88=86=E5=B1=A4=E6=94=B9=E3=80=8C=E8=B7=9F=E8=91=97?= =?UTF-8?q?=E6=95=B5=E6=A9=9F=E9=AB=98=E5=BA=A6=20=C2=B1offset=E3=80=8D?= =?UTF-8?q?=E6=B5=AE=E5=8B=95=E5=88=86=E5=B1=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 修實飛重編隊後待機組 B 掉高度的 bug（原 _alt_for 寫死 formation_alt 沒讀敵機高度）。高度來源改用 local_pose z（相對 home，非 AMSL gps alt）， 新增 rel_alt_provider / _enemy_base_alt（None 沿用上次有效值）， A=base+offset、B=base−offset；EngagementPanel 加「高度分層 ±」欄位即時生效。 SITL 驗證通過。 Co-Authored-By: Claude Opus 4.8 --- src/GUI/gui.py | 14 +++++- src/GUI/mission_orchestrator.py | 75 +++++++++++++++++++++++++++------ 2 files changed, 74 insertions(+), 15 deletions(-) diff --git a/src/GUI/gui.py b/src/GUI/gui.py index 1a5896e..9446868 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -364,6 +364,7 @@ class ControlStationUI(QMainWindow): self.engagement_panel.stop_requested.connect(self._on_engagement_stop) self.engagement_panel.groups_changed.connect(self._on_engagement_groups_changed) self.engagement_panel.apply_speed_requested.connect(self._on_engagement_apply_speed) + self.engagement_panel.alt_offset_changed.connect(self._on_engagement_alt_offset) self.left_tab.addTab(self.engagement_panel, "交戰") self._init_orchestrator() @@ -2503,6 +2504,9 @@ class ControlStationUI(QMainWindow): self.orchestrator = MissionOrchestrator( make_executor=self._make_orchestrator_executor, gps_provider=lambda: self.monitor.drone_gps, + # 敵機相對 home 高度（local_pose z，與 setpoint 同座標系）；勿用 drone_gps.alt（AMSL） + rel_alt_provider=lambda did: ( + getattr(self.monitor, 'drone_local', {}).get(did) or {}).get('z'), stop_groups_fn=self._stop_engagement_group_missions, ) self._engagement_a_group = None @@ -2605,6 +2609,13 @@ class ControlStationUI(QMainWindow): f"套用速度：友機 {friendly_speed:.1f} m/s ({len(friendly_ids)} 台)、" f"敵機 {enemy_speed:.1f} m/s", 4000) + def _on_engagement_alt_offset(self, offset): + """高度分層偏移即時生效：A=敵機高度+offset、B=敵機高度−offset。 + 下個 tick 的瞬時 slot 就會套用（PURSUE/ENCIRCLE 中調整也立即反映）。""" + if hasattr(self, 'orchestrator'): + self.orchestrator.alt_layer_offset = float(offset) + self.statusBar().showMessage(f"高度分層 ±{offset:.1f} m（A 上層 / B 下層）", 3000) + def _on_engagement_confirm(self): if hasattr(self, 'orchestrator'): self.orchestrator.confirm_detection() @@ -2735,9 +2746,10 @@ class ControlStationUI(QMainWindow): elif msg_type == 'local_pose': x, y = data.get('x', 0), data.get('y', 0) + z = data.get('z', 0) # 相對 home 高度（正向朝上），供交戰高度分層用 if not hasattr(self.monitor, 'drone_local'): self.monitor.drone_local = {} - self.monitor.drone_local[drone_id] = {'x': x, 'y': y} + self.monitor.drone_local[drone_id] = {'x': x, 'y': y, 'z': z} self.queue_overview_update(drone_id, 'local', f"{x:.1f}, {y:.1f}") elif msg_type == 'loss_rate': diff --git a/src/GUI/mission_orchestrator.py b/src/GUI/mission_orchestrator.py index 17e5a7d..73ee5ee 100644 --- a/src/GUI/mission_orchestrator.py +++ b/src/GUI/mission_orchestrator.py @@ -67,8 +67,8 @@ DEFAULT_REPLAN_CAP_SEC = 1.0 # 重算週期上限 (s) DEFAULT_CIRCLE_RADIUS = 10.0 # 包圍半徑 (m) DEFAULT_LON_SPACING = 5.0 # 隊形縱向間距 (m) DEFAULT_LAT_OFFSET = 3.0 # 隊形橫向錯開 (m) -DEFAULT_FORMATION_ALT = 10.0 # 隊形基準高度 (m，相對 home) -DEFAULT_ALT_LAYER_GAP = 6.0 # A/B 上下層垂直間隔 (m)：A=基準+gap/2、B=基準-gap/2 +DEFAULT_FORMATION_ALT = 10.0 # fallback 隊形基準高度 (m，相對 home)：敵機相對高度都讀不到時才用 +DEFAULT_ALT_LAYER_OFFSET = 5.0 # A/B 相對敵機高度的上下偏移 (m)：A=敵高+offset、B=敵高−offset DEFAULT_MIN_ALT_FLOOR = 5.0 # 相對 home 的最低安全高度 (m)：B 下層不得低於此值 DEFAULT_LEAD_TIME_CAP = 4.0 # 前置攔截 lead_time 上限 (s) DEFAULT_FRIENDLY_SPEED = 5.0 # 友機假設水平速度 (m/s)：估 lead_time = gap/此值 @@ -89,7 +89,8 @@ class MissionOrchestrator(QObject): """ 階段狀態機。透過注入的 callback 與 GUI 解耦（便於 headless 測試）： make_executor() -> MissionExecutor（complete_stops=False，hover-hold） - gps_provider() -> {drone_id: {'lat','lon','alt'}} + gps_provider() -> {drone_id: {'lat','lon','alt'}}（alt 是 AMSL，勿當相對高度用） + rel_alt_provider(drone_id) -> 該機相對 home 的高度 z（local_pose，正向朝上）或 None stop_groups_fn() -> 停掉操作員的 A/B 群組任務（重編隊時避免雙重送點） """ @@ -98,6 +99,7 @@ class MissionOrchestrator(QObject): status_message = pyqtSignal(str) def __init__(self, make_executor, gps_provider, stop_groups_fn=None, + rel_alt_provider=None, r_detect=DEFAULT_R_DETECT, detect_dwell_sec=DEFAULT_DETECT_DWELL, r_engage=DEFAULT_R_ENGAGE, @@ -108,7 +110,7 @@ class MissionOrchestrator(QObject): lon_spacing=DEFAULT_LON_SPACING, lat_offset=DEFAULT_LAT_OFFSET, formation_alt=DEFAULT_FORMATION_ALT, - alt_layer_gap=DEFAULT_ALT_LAYER_GAP, + alt_layer_offset=DEFAULT_ALT_LAYER_OFFSET, min_alt_floor=DEFAULT_MIN_ALT_FLOOR, lead_time_cap_sec=DEFAULT_LEAD_TIME_CAP, friendly_speed=DEFAULT_FRIENDLY_SPEED, @@ -117,6 +119,7 @@ class MissionOrchestrator(QObject): super().__init__(parent) self._make_executor = make_executor self._gps_provider = gps_provider + self._rel_alt_provider = rel_alt_provider self._stop_groups_fn = stop_groups_fn self.r_detect = float(r_detect) self.detect_dwell_sec = float(detect_dwell_sec) @@ -128,7 +131,7 @@ class MissionOrchestrator(QObject): 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.alt_layer_offset = float(alt_layer_offset) self.min_alt_floor = float(min_alt_floor) self.lead_time_cap_sec = float(lead_time_cap_sec) self.friendly_speed = float(friendly_speed) @@ -147,6 +150,7 @@ class MissionOrchestrator(QObject): self._last_heading = (0.0, 1.0) # 上次隊形朝向（敵機太近時沿用，避免抖動） self._enemy_hist = [] # [(t, lat, lon)] 敵機軌跡（估速用） self._enemy_vel = (0.0, 0.0) # 估得的敵速 (east, north) m/s + self._last_enemy_rel_z = None # 上次有效的敵機相對高度（provider 暫讀不到時沿用） # ------------------------------------------------------------- 設定（可連續呼叫） def configure(self, a_drone_ids, b_drone_ids, enemy_id): @@ -294,13 +298,29 @@ class MissionOrchestrator(QObject): self._apply_slots(self._circle_slots(gps, epos), epos) # ------------------------------------------------------------- 高度分層 - def _alt_for(self, did): - """S3 高度分層：A 組（含領隊）走上層、B 組走下層（不低於地面下限）。 - 同高度繞圈/追擊會交叉撞機，分層後 A/B 垂直錯開自然避開。""" + def _enemy_base_alt(self): + """分層基準 = 敵機當下『相對 home』高度（來自 local_pose z，與 setpoint 同座標系）。 + provider 暫讀不到（None）時沿用上一次有效值；連一次都沒有才退回 formation_alt。 + 注意：不可用 gps_provider 的 alt（那是 AMSL 海拔，座標系不同會炸高）。""" + z = None + if self._rel_alt_provider is not None and self.enemy_id is not None: + try: + z = self._rel_alt_provider(self.enemy_id) + except Exception: + z = None + if z is not None: + self._last_enemy_rel_z = float(z) + return float(z) + if self._last_enemy_rel_z is not None: + return self._last_enemy_rel_z + return self.formation_alt + + def _alt_for(self, did, base): + """S3 高度分層：以敵機高度 base 為中心，A 組（含領隊）走上層 base+offset、 + B 組走下層 base−offset（不低於地面下限）。分層後 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 + return max(self.min_alt_floor, base - self.alt_layer_offset) + return base + self.alt_layer_offset # ------------------------------------------------------------- 前置攔截（估敵速） def _update_enemy_velocity(self, epos): @@ -349,6 +369,7 @@ class MissionOrchestrator(QObject): h = (hx / n, hy / n) self._last_heading = h px, py = -h[1], h[0] # 左垂直 + base = self._enemy_base_alt() # 高度分層基準：敵機相對高度（每 tick 取一次） slots = {} for rank, d in enumerate(ids): lat_sign = -1 if rank % 2 == 0 else 1 @@ -356,7 +377,7 @@ class MissionOrchestrator(QObject): 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)) + slots[d] = (lat, lon, self._alt_for(d, base)) return slots def _circle_slots(self, gps, epos): @@ -364,13 +385,14 @@ class MissionOrchestrator(QObject): ids = self._merged_ids elat, elon = epos['lat'], epos['lon'] n = len(ids) + base = self._enemy_base_alt() # 高度分層基準：敵機相對高度（每 tick 取一次） 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)) + slots[d] = (lat, lon, self._alt_for(d, base)) return slots def _apply_slots(self, slots, epos): @@ -453,12 +475,15 @@ class EngagementPanel(QWidget): stop_requested = pyqtSignal() groups_changed = pyqtSignal(str, str) # (a_group_id, b_group_id) apply_speed_requested = pyqtSignal(float, float) # (friendly_speed, enemy_ratio) + alt_offset_changed = pyqtSignal(float) # A/B 相對敵機高度的上下偏移 (m) def __init__(self, parent=None, r_detect=DEFAULT_R_DETECT, - friendly_speed=DEFAULT_FRIENDLY_SPEED): + friendly_speed=DEFAULT_FRIENDLY_SPEED, + alt_offset=DEFAULT_ALT_LAYER_OFFSET): super().__init__(parent) self.r_detect = float(r_detect) self._init_friendly_speed = float(friendly_speed) + self._init_alt_offset = float(alt_offset) self._enemy_id = "" self._suppress_enemy = False self._suppress_group = False @@ -568,6 +593,28 @@ class EngagementPanel(QWidget): apply_speed_row.addStretch() root.addLayout(apply_speed_row) + # 高度分層：A=敵機高度+offset、B=敵機高度−offset（即時生效，不需重編隊） + alt_row = QHBoxLayout() + alt_row.setSpacing(8) + altl = QLabel("高度分層 ±：") + altl.setStyleSheet("color: #CCC; font-size: 12px;") + self.alt_offset_spin = QDoubleSpinBox() + self.alt_offset_spin.setStyleSheet(spin_css) + self.alt_offset_spin.setRange(0.0, 30.0) + self.alt_offset_spin.setSingleStep(0.5) + self.alt_offset_spin.setDecimals(1) + self.alt_offset_spin.setSuffix(" m") + self.alt_offset_spin.setValue(self._init_alt_offset) + self.alt_offset_spin.valueChanged.connect( + lambda v: self.alt_offset_changed.emit(float(v))) + alt_hint = QLabel("（A 走上層 / B 走下層，繞敵機高度）") + alt_hint.setStyleSheet("color: #777; font-size: 11px;") + alt_row.addWidget(altl) + alt_row.addWidget(self.alt_offset_spin) + alt_row.addWidget(alt_hint) + alt_row.addStretch() + root.addLayout(alt_row) + sep = QFrame() sep.setFrameShape(QFrame.Shape.HLine) sep.setStyleSheet("color: #444;")