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

feat(GUI): 交戰高度分層改「跟著敵機高度 ±offset」浮動分層

Browse Source 
修實飛重編隊後待機組 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 <noreply@anthropic.com>
This commit is contained in:
wenchun
2026-08-24 18:17:48 +08:00
co-authored by Claude Opus 4.8
parent 1db3445e86
commit e205f567e8
2 changed files with 74 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/GUI/gui.py
+13 -1
View File
@@ -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':
src/GUI/mission_orchestrator.py
+61 -14
View File
@@ -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;")