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

Compare commits

merge into: chiyu1468/AirTrapMine:master
Branches Tags
chiyu1468/AirTrapMine:master chiyu1468/AirTrapMine:wenchun chiyu1468/AirTrapMine:chiyu chiyu1468/AirTrapMine:chiyuTemp chiyu1468/AirTrapMine:lunu chiyu1468/AirTrapMine:ken
..
pull from: chiyu1468/AirTrapMine:wenchun
Branches Tags
chiyu1468/AirTrapMine:master chiyu1468/AirTrapMine:wenchun chiyu1468/AirTrapMine:chiyu chiyu1468/AirTrapMine:chiyuTemp chiyu1468/AirTrapMine:lunu chiyu1468/AirTrapMine:ken
1 Commits
master .. wenchun
Author SHA1 Message Date
wenchunandClaude Opus 4.8 e205f567e8 feat(GUI): 交戰高度分層改「跟著敵機高度 ±offset」浮動分層 
修實飛重編隊後待機組 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>
 2026-08-24 18:17:48 +08:00
13 changed files with 480 additions and 3885 deletions
Expand all files Collapse all files
src/GUI/communication.py
+15 -257
View File
@@ -11,7 +11,6 @@ import json
import socket
import sys
import os
import time
import traceback
try:
import serial
@@ -30,28 +29,6 @@ def _log(level, message):
print(f"[{level}] {message}")
def build_xbee_json_command(system_id, command, position=None):
"""Build the shared newline-free XBee command used by Serial and WebSocket."""
if isinstance(system_id, bool) or not isinstance(system_id, int):
raise ValueError("XBee system_id 必須為整數")
if not isinstance(command, str):
raise ValueError("XBee 指令必須為大寫字串")
if command not in {'START', 'STOP', 'GOTO'}:
raise ValueError(f"XBee 指令必須為大寫 START、STOP 或 GOTO: {command}")
payload_data = {
's': system_id,
'c': command,
}
if command == 'GOTO':
if not isinstance(position, (list, tuple)) or len(position) < 2:
raise ValueError("GOTO 缺少有效的經緯度")
payload_data['p'] = [float(position[0]), float(position[1])]
return json.dumps(
payload_data, ensure_ascii=False, separators=(',', ':'))
# 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入）
_src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
if _src_path not in sys.path:
@@ -118,9 +95,7 @@ class JsonTelemetryProcessor:
"position": {"lat": 24.0, "lon": 120.0},
"heading": 90
}
Serial and WebSocket JSON also accept compact UAV status shapes:
{"s": 1, "m": "GUIDED", "b": 85, "p": [24.0, 120.0], "y": 90.0}
or the full shape:
Serial JSON also accepts the compact UAV.py shape:
{"s": 1, "m": "GUIDED", "a": 1, "b": 85, "h": 10.0,
"v": 4.2, "p": [24.0, 120.0], "ypr": [90.0, 0.0, 0.0],
"g": 3, "d": [0.8, 1.2]}
@@ -143,20 +118,6 @@ class JsonTelemetryProcessor:
if not isinstance(data, dict):
return
# XBee command acknowledgement does not carry a UAV system id.
# Route it to the GUI log/status area instead of silently dropping it.
if all(key in data for key in ('result', 'command', 'message')):
self.signals.update_signal.emit(
'xbee_ack',
self.connection_name,
{
'result': data.get('result'),
'command': data.get('command'),
'message': data.get('message'),
}
)
return
system_id = data.get('system_id', data.get('sysid', data.get('s')))
if system_id is None:
return
@@ -164,13 +125,6 @@ class JsonTelemetryProcessor:
drone_id = f"s{self.socket_id}_{system_id}"
self._emit_json_connection_type(drone_id)
# Compact XBee/UAV packets are complete snapshots. Emit every field
# group, using None for omitted values, so the GUI can show '-'
# instead of inventing zero-valued telemetry or retaining stale data.
if 's' in data and 'c' not in data:
self._process_compact_uav_telemetry(data, drone_id)
return
mode = data.get('mode', data.get('mode_name', data.get('m')))
state = {}
if mode is not None:
@@ -215,23 +169,23 @@ class JsonTelemetryProcessor:
pos = data.get('position')
if isinstance(pos, dict):
gps_data = {
'lat': pos.get('lat', pos.get('latitude')),
'lon': pos.get('lon', pos.get('longitude')),
'alt': pos.get('alt', pos.get('altitude'))
'lat': pos.get('lat', pos.get('latitude', 0)),
'lon': pos.get('lon', pos.get('longitude', 0)),
'alt': pos.get('alt', pos.get('altitude', 0))
}
self.signals.update_signal.emit('gps', drone_id, gps_data)
elif 'lat' in data or 'latitude' in data:
self.signals.update_signal.emit('gps', drone_id, {
'lat': data.get('lat', data.get('latitude')),
'lon': data.get('lon', data.get('longitude')),
'alt': data.get('alt', data.get('altitude'))
'lat': data.get('lat', data.get('latitude', 0)),
'lon': data.get('lon', data.get('longitude', 0)),
'alt': data.get('alt', data.get('altitude', 0))
})
elif isinstance(data.get('p'), (list, tuple)) and len(data.get('p')) >= 2:
dop = data.get('d') if isinstance(data.get('d'), (list, tuple)) else []
gps_data = {
'lat': data['p'][0],
'lon': data['p'][1],
'alt': data.get('h')
'alt': data.get('h', 0)
}
if 'g' in data:
gps_data['fix_type'] = data.get('g')
@@ -347,82 +301,6 @@ class JsonTelemetryProcessor:
except Exception as e:
print(f"{self.source_type} JSON telemetry processing error: {e}")
def _process_compact_uav_telemetry(self, data, drone_id):
"""Convert one compact XBee UAV snapshot into existing GUI events."""
snapshot = {'_snapshot': True}
self.signals.update_signal.emit('state', drone_id, {
**snapshot,
'mode': data.get('m'),
'armed': bool(data.get('a')) if data.get('a') is not None else None,
})
self.signals.update_signal.emit('battery', drone_id, {
**snapshot,
'percentage': data.get('b'),
'voltage': None,
})
position = data.get('p')
has_position = isinstance(position, (list, tuple)) and len(position) >= 2
dop = data.get('d')
has_dop = isinstance(dop, (list, tuple))
ypr = data.get('ypr')
has_ypr = isinstance(ypr, (list, tuple)) and len(ypr) >= 3
has_yaw = data.get('y') is not None
# In the legacy compact shape, a lone 'h' is heading. In the newer
# y/ypr shapes it is height, matching the original ground-station parser.
height = data.get('h') if has_ypr or has_yaw else None
yaw = ypr[0] if has_ypr else data.get('y', data.get('h'))
pitch = ypr[1] if has_ypr else None
roll = ypr[2] if has_ypr else None
self.signals.update_signal.emit('gps', drone_id, {
**snapshot,
'lat': position[0] if has_position else None,
'lon': position[1] if has_position else None,
'alt': height,
'fix_type': data.get('g'),
'eph': dop[0] if has_dop and len(dop) >= 1 else None,
'epv': dop[1] if has_dop and len(dop) >= 2 else None,
'satellites_visible': None,
})
self.signals.update_signal.emit('altitude', drone_id, {
**snapshot,
'altitude': height,
})
self.signals.update_signal.emit('local_pose', drone_id, {
**snapshot,
'x': None,
'y': None,
'z': height,
})
# Compact 'v' is scalar ground speed, not an XY velocity vector.
self.signals.update_signal.emit('velocity', drone_id, {
**snapshot,
'vx': None,
'vy': None,
'vz': None,
})
self.signals.update_signal.emit('attitude', drone_id, {
**snapshot,
'roll': roll,
'pitch': pitch,
'yaw': yaw,
'rates': None,
})
self.signals.update_signal.emit('hud', drone_id, {
**snapshot,
'heading': yaw,
'groundspeed': data.get('v'),
'airspeed': None,
'throttle': None,
'alt': height,
'climb': None,
})
class UDPMavlinkReceiver(threading.Thread):
"""UDP MAVLink 接收器"""
def __init__(self, ip, port, signals, connection_name, monitor=None):
@@ -570,7 +448,6 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
self.source_type = 'Serial'
self.running = False
self.serial_conn = None
self.serial_write_lock = Lock()
self.mav_parser = mavutil.mavlink.MAVLink(None)
self.mav_parser.robust_parsing = True
self.json_buffer = []
@@ -613,24 +490,11 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
finally:
if self.serial_conn:
try:
with self.serial_write_lock:
self.serial_conn.close()
except Exception:
pass
self._release_socket_id()
def send_json_command(self, system_id, command, position=None):
"""Send one newline-delimited XBee JSON command on this serial link."""
payload = build_xbee_json_command(system_id, command, position) + '\n'
with self.serial_write_lock:
if not self.running or self.serial_conn is None:
raise RuntimeError("Serial 連線尚未啟動")
if hasattr(self.serial_conn, 'is_open') and not self.serial_conn.is_open:
raise RuntimeError("Serial 連線已關閉")
self.serial_conn.write(payload.encode('utf-8'))
self.serial_conn.flush()
return payload.strip()
def _process_mavlink_byte(self, byte):
try:
msg = self.mav_parser.parse_char(byte)
@@ -787,7 +651,6 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
self.running = False
if self.serial_conn:
try:
with self.serial_write_lock:
self.serial_conn.close()
except Exception:
pass
@@ -812,19 +675,12 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
self.running = False
self.max_retries = 5
self.base_delay = 1.0
self.loop = None
self.websocket = None
self.websocket_lock = Lock()
def run(self):
"""執行 WebSocket 接收循環"""
self.running = True
self.loop = asyncio.new_event_loop()
asyncio.set_event_loop(self.loop)
try:
self.loop.run_until_complete(self.ws_client_loop())
finally:
self.loop.close()
asyncio.set_event_loop(asyncio.new_event_loop())
asyncio.get_event_loop().run_until_complete(self.ws_client_loop())
async def ws_client_loop(self):
"""WebSocket 連接的主循環"""
@@ -835,11 +691,9 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
while self.running and retry_count < self.max_retries:
try:
async with websockets.connect(self.url) as websocket:
with self.websocket_lock:
self.websocket = websocket
print(f"WebSocket {self.connection_name} connected to {self.url}")
retry_count = 0 # 重置重試計數
try:
async for message in websocket:
if not self.running:
break
@@ -853,10 +707,6 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
print(f"WebSocket {self.connection_name} JSON decode error: {e}")
except Exception as e:
print(f"WebSocket {self.connection_name} message processing error: {e}")
finally:
with self.websocket_lock:
if self.websocket is websocket:
self.websocket = None
except websockets.exceptions.ConnectionClosedError:
print(f"WebSocket {self.connection_name} connection closed")
@@ -880,37 +730,6 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
print(f"WebSocket client {self.connection_name} stopped")
self._release_socket_id()
def send_json_command(self, system_id, command, position=None):
"""Schedule an XBee JSON command on this receiver's WebSocket loop."""
payload = build_xbee_json_command(system_id, command, position)
with self.websocket_lock:
loop = self.loop
websocket = self.websocket
if not self.running or loop is None or not loop.is_running():
raise RuntimeError("WebSocket 連線尚未啟動")
if websocket is None:
raise RuntimeError("WebSocket 尚未連線")
async def _send_on_current_connection():
with self.websocket_lock:
if self.websocket is not websocket:
raise RuntimeError("WebSocket 連線已變更")
await websocket.send(payload)
future = asyncio.run_coroutine_threadsafe(
_send_on_current_connection(), loop)
def _report_send_error(done_future):
try:
done_future.result()
except Exception as e:
print(
f"WebSocket {self.connection_name} command send error: {e}")
future.add_done_callback(_report_send_error)
return payload
def process_websocket_message(self, data):
"""處理 WebSocket 訊息"""
self.process_json_telemetry_message(data)
@@ -918,11 +737,6 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
def stop(self):
"""停止接收器"""
self.running = False
with self.websocket_lock:
loop = self.loop
websocket = self.websocket
if loop is not None and loop.is_running() and websocket is not None:
asyncio.run_coroutine_threadsafe(websocket.close(), loop)
self._release_socket_id()
def _release_socket_id(self):
@@ -936,6 +750,7 @@ class DroneMonitor(Node):
def __init__(self):
# Use a unique node name with timestamp to avoid conflicts on restart
import time
node_name = f'drone_monitor_{int(time.time() * 1000) % 100000}'
super().__init__(node_name)
self.signals = DroneSignals()
@@ -960,9 +775,6 @@ class DroneMonitor(Node):
# 【新增】儲存 GPS 資料的字典
# ================================================================================
self.drone_gps = {} # {drone_id: {'lat': ..., 'lon': ..., 'alt': ...}}
# GnssRaw.altitude 是 AMSL；交戰/航點指令的 alt 則是相對 Home 高度。
self.drone_relative_alt = {}
self.drone_relative_alt_updated_at = {}
# ================================================================================
# ================================================================================
@@ -1010,35 +822,6 @@ class DroneMonitor(Node):
200
)
def get_drone_gps_snapshot(self):
"""回傳執行緒安全的定位快照。"""
with self.lock:
return {
drone_id: dict(position)
for drone_id, position in self.drone_gps.items()
if isinstance(position, dict)
}
def record_relative_gps(self, drone_id, gps_data, updated_at=None):
"""記錄已知以 Home 為基準的 GPS/高度（UDP、Serial、WebSocket）。"""
now = time.monotonic() if updated_at is None else float(updated_at)
stored = dict(gps_data)
stored['_gps_updated_at'] = now
alt = stored.get('alt')
if isinstance(alt, (int, float)) and math.isfinite(float(alt)):
stored['alt'] = float(alt)
stored['_relative_alt_updated_at'] = now
stored['_altitude_reference'] = 'relative_home'
else:
stored['alt'] = None
with self.lock:
if stored.get('_altitude_reference') == 'relative_home':
self.drone_relative_alt[drone_id] = stored['alt']
self.drone_relative_alt_updated_at[drone_id] = now
self.drone_gps[drone_id] = stored
return stored
def get_next_socket_id(self):
"""取得目前最小的未使用 socket_id（從 0 開始）。"""
with self.socket_id_lock:
@@ -1772,15 +1555,10 @@ class DroneMonitor(Node):
if actual_drone_id is None:
return
# GnssRaw.altitude 是 GLOBAL_POSITION_INT.alt（AMSL）。交戰航點使用
# GLOBAL_RELATIVE_ALT_INT，因此 alt 僅能使用 position_ned 的相對高度。
now = time.monotonic()
gps_data = {
'lat': msg.latitude,
'lon': msg.longitude,
'alt': None,
'amsl_alt': msg.altitude,
'_gps_updated_at': now,
'alt': msg.altitude
}
if hasattr(msg, 'fix_type'):
@@ -1792,20 +1570,12 @@ class DroneMonitor(Node):
if hasattr(msg, 'epv'):
gps_data['epv'] = msg.epv
self.latest_data[(actual_drone_id, 'gps')] = gps_data
# ================================================================================
# 【新增】儲存 GPS 資料到 drone_gps 字典
# ================================================================================
with self.lock:
relative_alt = self.drone_relative_alt.get(actual_drone_id)
relative_alt_updated_at = self.drone_relative_alt_updated_at.get(
actual_drone_id
)
if relative_alt_updated_at is not None:
gps_data['alt'] = relative_alt
gps_data['_relative_alt_updated_at'] = relative_alt_updated_at
gps_data['_altitude_reference'] = 'relative_home'
self.drone_gps[actual_drone_id] = gps_data.copy()
self.latest_data[(actual_drone_id, 'gps')] = gps_data
# ================================================================================
def local_vel_callback(self, drone_id, msg):
@@ -1866,18 +1636,6 @@ class DroneMonitor(Node):
vy = msg.twist.twist.linear.x
vz = -msg.twist.twist.linear.z
now = time.monotonic()
with self.lock:
self.drone_relative_alt[actual_drone_id] = z
self.drone_relative_alt_updated_at[actual_drone_id] = now
current_gps = self.drone_gps.get(actual_drone_id)
if isinstance(current_gps, dict):
current_gps = dict(current_gps)
current_gps['alt'] = z
current_gps['_relative_alt_updated_at'] = now
current_gps['_altitude_reference'] = 'relative_home'
self.drone_gps[actual_drone_id] = current_gps
# 儲存高度信息
self.latest_data[(actual_drone_id, 'altitude')] = {
'altitude': z
src/GUI/drone_panel.py
+9 -9
View File
@@ -222,11 +222,11 @@ class DronePanel(QWidget):
status_title = QLabel("狀態：")
_set_scaled_stylesheet(status_title, "color: #888; min-width: 50px;")
self.mode_label = QLabel("-")
self.mode_label = QLabel("--")
self.mode_label.setObjectName(f"{self.drone_id}_mode")
_set_scaled_stylesheet(self.mode_label, "color: #DDD;")
self.armed_label = QLabel("-")
self.armed_label = QLabel("--")
self.armed_label.setObjectName(f"{self.drone_id}_armed")
_set_scaled_stylesheet(self.armed_label, "color: #DDD;")
@@ -284,7 +284,7 @@ class DronePanel(QWidget):
battery_title = QLabel("電池：")
_set_scaled_stylesheet(battery_title, "color: #888; min-width: 50px;")
self.battery_pct_label = QLabel("-")
self.battery_pct_label = QLabel("--")
self.battery_pct_label.setObjectName(f"{self.drone_id}_battery_pct")
_set_scaled_stylesheet(self.battery_pct_label, "color: #DDD;")
@@ -294,7 +294,7 @@ class DronePanel(QWidget):
_set_scaled_stylesheet(separator1, "color: #DDD;")
# 顯示電壓
self.battery_vol_label = QLabel("-")
self.battery_vol_label = QLabel("--")
self.battery_vol_label.setObjectName(f"{self.drone_id}_battery_vol")
_set_scaled_stylesheet(self.battery_vol_label, "color: #DDD;")
@@ -304,7 +304,7 @@ class DronePanel(QWidget):
_set_scaled_stylesheet(separator2, "color: #DDD;")
# 顯示電池節數 (S count)
self.battery_cells_label = QLabel("-")
self.battery_cells_label = QLabel("--")
self.battery_cells_label.setObjectName(f"{self.drone_id}_battery_cells")
_set_scaled_stylesheet(self.battery_cells_label, "color: #DDD;")
@@ -327,14 +327,14 @@ class DronePanel(QWidget):
altitude_title = QLabel("高度：")
_set_scaled_stylesheet(altitude_title, "color: #888; min-width: 50px;")
self.altitude_label = QLabel("-")
self.altitude_label = QLabel("--")
self.altitude_label.setObjectName(f"{self.drone_id}_altitude")
_set_scaled_stylesheet(self.altitude_label, "color: #DDD;")
speed_title = QLabel("速度：")
_set_scaled_stylesheet(speed_title, "color: #888; margin-left: 10px;")
self.speed_label = QLabel("-")
self.speed_label = QLabel("--")
self.speed_label.setObjectName(f"{self.drone_id}_speed")
_set_scaled_stylesheet(self.speed_label, "color: #DDD;")
@@ -355,14 +355,14 @@ class DronePanel(QWidget):
fix_title = QLabel("定位：")
_set_scaled_stylesheet(fix_title, "color: #888; min-width: 50px;")
self.fix_type_label = QLabel("-")
self.fix_type_label = QLabel("--")
self.fix_type_label.setObjectName(f"{self.drone_id}_fix_type_label")
_set_scaled_stylesheet(self.fix_type_label, "color: #DDD;")
error_title = QLabel("誤差(H/V)：")
_set_scaled_stylesheet(error_title, "color: #888; margin-left: 8px;")
self.gnss_error_label = QLabel("-")
self.gnss_error_label = QLabel("--")
self.gnss_error_label.setObjectName(f"{self.drone_id}_gnss_error")
_set_scaled_stylesheet(self.gnss_error_label, "color: #DDD;")
src/GUI/gui.py
+75 -361
View File
@@ -147,11 +147,10 @@ class ToggleSwitch(QWidget):
class ControlStationUI(QMainWindow):
planning_finished = pyqtSignal(object)
VERSION = '2.9.1'
VERSION = '2.7.1'
FONT_SCALE_MIN = 70
FONT_SCALE_MAX = 180
FONT_SCALE_DEFAULT = 100
ENGAGEMENT_TELEMETRY_TIMEOUT_SEC = 5.0
def __init__(self):
super().__init__()
@@ -247,7 +246,6 @@ 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)
@@ -279,7 +277,7 @@ class ControlStationUI(QMainWindow):
# ================================================================================
self.mission_planner = FormationPlanner(
spacing=5.0, # 5 公尺間距
base_altitude=30.0, # 基準高度 30 公尺
base_altitude=10.0, # 基準高度 10 公尺
altitude_diff=2.0 # 高低差 2 公尺
)
# ================================================================================
@@ -299,7 +297,6 @@ 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
@@ -367,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()
@@ -452,7 +450,7 @@ class ControlStationUI(QMainWindow):
self.right_vertical_splitter.setChildrenCollapsible(False)
self.right_vertical_splitter.setStretchFactor(0, 0)
self.right_vertical_splitter.setStretchFactor(1, 1)
self.right_vertical_splitter.setSizes([210, 640])
self.right_vertical_splitter.setSizes([180, 640])
right_layout.addWidget(self.right_vertical_splitter)
# 添加地圖
@@ -876,16 +874,6 @@ class ControlStationUI(QMainWindow):
'drone', f"/fc_network/vehicle/{sys_id}/status_text "
f"[{label}] {data.get('text', '')}")
def _record_xbee_ack(self, connection_name, data):
"""將 XBee JSON ACK 顯示於既有無人機紀錄與狀態列。"""
command = data.get('command', '-')
result = data.get('result', '-')
message = data.get('message', '-')
text = (f"{connection_name} XBee ACK｜命令:{command} "
f"狀態:{result} 訊息:{message}")
self._append_history('drone', text)
self.statusBar().showMessage(text, 5000)
def _record_sys_diags_warning(self, sys_id, data):
"""sys_diags 只在診斷值異常或內容改變時顯示警告。"""
installed = int(data.get('sensors_install_mask', 0))
@@ -1075,11 +1063,8 @@ class ControlStationUI(QMainWindow):
def handle_serial_connection_added(self, port, baudrate):
conn = {'name': 'Serial', 'port': port, 'baudrate': baudrate, 'enabled': False, 'receiver': None}
self.serial_connections.append(conn)
panel = self.comm_panel.add_serial_panel(conn)
# Serial 新增後立即啟動；若啟動失敗，面板仍保留為停止狀態，
# 使用者可在排除連線問題後手動重試。
self.toggle_serial_connection(
conn, panel.toggle_btn, panel.status_label)
self.comm_panel.add_serial_panel(conn)
self.statusBar().showMessage(f"已添加 Serial 連接: {port} @ {baudrate}", 3000)
def toggle_serial_connection(self, conn, btn, status_label):
if conn.get('enabled', False):
@@ -1149,7 +1134,7 @@ class ControlStationUI(QMainWindow):
def handle_takeoff(self, drone_id):
loop = asyncio.get_event_loop()
future = self.monitor.takeoff_drone(drone_id, 30.0)
future = self.monitor.takeoff_drone(drone_id, 10.0)
loop.create_task(self.handle_service_response(future, f"起飛 {drone_id}"))
def handle_setpoint_selected(self):
@@ -1234,7 +1219,7 @@ class ControlStationUI(QMainWindow):
loop = asyncio.get_event_loop()
for drone_id in selected:
future = self.monitor.takeoff_drone(drone_id, 30.0)
future = self.monitor.takeoff_drone(drone_id, 10.0)
loop.create_task(self.handle_service_response(future, f"批次起飛 {drone_id}"))
# ================================================================================
@@ -1294,7 +1279,6 @@ class ControlStationUI(QMainWindow):
panel.arm_requested.connect(self._handle_group_arm)
panel.takeoff_requested.connect(self._handle_group_takeoff)
panel.reboot_requested.connect(self._handle_group_reboot)
panel.xbee_command_requested.connect(self._handle_xbee_command)
panel.box_select_requested.connect(self._handle_box_select)
panel.select_all_requested.connect(self._handle_select_all_for_group)
panel.clear_group_requested.connect(self._handle_clear_group)
@@ -1321,7 +1305,6 @@ class ControlStationUI(QMainWindow):
"""切換 group tab — 只需切換 active group 並刷新 UI"""
if index < 0:
self.active_group_id = None
self._sync_map_follow_for_active_group()
return
tab_text = self.group_tab_widget.tabText(index)
@@ -1349,13 +1332,6 @@ class ControlStationUI(QMainWindow):
group = self._get_active_group()
selected = group.selected_drone_ids if group else set()
current_group_id = group.group_id if group else None
selection_history = getattr(
self, '_last_map_selection_by_group', {})
previous_selected = selection_history.get(current_group_id, set())
newly_selected = set(selected) - set(previous_selected)
if current_group_id is not None:
selection_history[current_group_id] = set(selected)
self._last_map_selection_by_group = selection_history
# 左側 drone checkbox
for drone_id, panel in self.drones.items():
@@ -1383,23 +1359,6 @@ class ControlStationUI(QMainWindow):
panel.update_status()
self._sync_map_group_colors()
self._sync_map_follow_for_active_group()
if newly_selected:
# 選取 UAV 時移動到該機目前位置；非任務狀態只移動一次。
self.drone_map.focus_on_drone(sorted(newly_selected)[0])
def _sync_map_follow_for_active_group(self):
"""只在 active group 的任務執行中啟用地圖 UAV 跟隨。"""
group = self._get_active_group()
running = bool(
group is not None
and group.executor is not None
and group.executor.state == MissionState.RUNNING
)
preferred_drone_id = None
if running and group.selected_drone_ids:
preferred_drone_id = sorted(group.selected_drone_ids)[0]
self.drone_map.set_mission_following(running, preferred_drone_id)
def _sync_map_group_colors(self):
"""同步每台 drone 的 group 顏色到地圖箭頭外框。"""
@@ -1566,7 +1525,6 @@ class ControlStationUI(QMainWindow):
group.executor.hover_stable_sec = ep.get(
'hover_stable_sec', group.executor.hover_stable_sec)
group.executor.start(group.planned_waypoints)
self._sync_map_follow_for_active_group()
panel = self.group_panels.get(group_id)
if panel:
panel.update_status()
@@ -1583,7 +1541,6 @@ class ControlStationUI(QMainWindow):
elif group.executor.state == MissionState.PAUSED:
group.executor.resume()
self.statusBar().showMessage(f"Group {group_id}: 任務已恢復", 3000)
self._sync_map_follow_for_active_group()
panel = self.group_panels.get(group_id)
if panel:
panel.update_status()
@@ -1595,12 +1552,8 @@ class ControlStationUI(QMainWindow):
return
if group.executor:
group.executor.stop()
self._sync_map_follow_for_active_group()
group.planned_waypoints = None
group.mission_target = None
self.drone_map.clear_mission_plan_for_group(group_id)
self.drone_map.clear_grid_sweep_area_for_group(
group_id, restart_selection=(group.mission_type == 'GRID_SWEEP'))
panel = self.group_panels.get(group_id)
if panel:
panel.update_status()
@@ -1714,81 +1667,6 @@ class ControlStationUI(QMainWindow):
future = self.monitor.reboot_drone(drone_id)
loop.create_task(self.handle_service_response(future, f"重啟飛控 {drone_id}"))
def _handle_xbee_command(self, group_id, command):
"""依 UAV 所屬連線透過 Serial 或 WebSocket 發送 XBee JSON 指令。"""
group = self.mission_groups.get(group_id)
if not group:
self.statusBar().showMessage(f"Group {group_id} 不存在", 3000)
return
selected = sorted(group.selected_drone_ids)
if not selected:
self.statusBar().showMessage(
f"Group {group_id}: 請先分配無人機", 3000)
return
command = str(command).upper()
position = None
if command == 'GOTO':
position = group.mission_target
if position is None:
self.statusBar().showMessage(
f"Group {group_id}: 請先在地圖點選目標點", 4000)
return
receivers_by_socket = {}
for conn in self.serial_connections:
receiver = conn.get('receiver')
if conn.get('enabled') and receiver is not None:
receivers_by_socket[str(receiver.socket_id)] = (
receiver, 'Serial')
for conn in self.ws_connections:
receiver = conn.get('receiver')
if conn.get('enabled') and receiver is not None:
receivers_by_socket[str(receiver.socket_id)] = (
receiver, 'WebSocket')
sent = []
failed = []
for drone_id in selected:
match = re.fullmatch(r's(\d+)_(\d+)', drone_id)
if not match:
failed.append(f"{drone_id}(ID格式錯誤)")
continue
socket_id, system_id = match.groups()
route = receivers_by_socket.get(socket_id)
if route is None:
failed.append(f"{drone_id}(無可用Serial/WebSocket)")
continue
receiver, transport = route
try:
payload = receiver.send_json_command(
int(system_id), command, position=position)
sent.append(drone_id)
self._append_history(
'gui', f"{drone_id} XBee {transport} TX｜{payload}")
except Exception as e:
failed.append(f"{drone_id}({e})")
if sent and not failed:
target_text = (
f" → {position[0]:.7f}, {position[1]:.7f}"
if position is not None else ""
)
self.statusBar().showMessage(
f"Group {group_id}: XBee {command} 已送至 "
f"{len(sent)} 台{target_text}", 4000)
elif sent:
self.statusBar().showMessage(
f"Group {group_id}: XBee {command} 已送 {len(sent)} 台；"
f"失敗 {len(failed)} 台：{', '.join(failed)}", 6000)
else:
self.statusBar().showMessage(
f"Group {group_id}: XBee {command} 未送出："
f"{', '.join(failed)}", 6000)
def _handle_box_select(self, group_id):
"""觸發地圖框選 → 框選完成後直接分配到該群組"""
self._pending_box_assign = group_id
@@ -1877,11 +1755,9 @@ class ControlStationUI(QMainWindow):
group.selected_drone_ids.clear()
group.planned_waypoints = None
group.mission_target = None
if group.executor:
group.executor.stop()
self.drone_map.clear_mission_plan_for_group(group_id)
self.drone_map.clear_grid_sweep_area_for_group(group_id)
# 只有當操作目標組是 active 組時，才更新 UI
if group_id == self.active_group_id:
@@ -1916,7 +1792,6 @@ class ControlStationUI(QMainWindow):
# 移除地圖上的任務計畫
self.drone_map.clear_mission_plan_for_group(group_id)
self.drone_map.clear_grid_sweep_area_for_group(group_id)
# 移除 tab
for i in range(self.group_tab_widget.count()):
@@ -1952,7 +1827,6 @@ class ControlStationUI(QMainWindow):
def _on_group_mission_completed(self, group_id):
"""群組任務完成回呼"""
self._sync_map_follow_for_active_group()
panel = self.group_panels.get(group_id)
if panel:
panel.update_status()
@@ -1964,9 +1838,6 @@ class ControlStationUI(QMainWindow):
def update_ui(self, msg_type, drone_id, data):
"""只做數據快取，不在這裡更新 UI"""
if msg_type == 'xbee_ack':
self._record_xbee_ack(drone_id, data)
return
if msg_type == 'status_text':
self._record_drone_status(drone_id, data)
return
@@ -1989,6 +1860,7 @@ class ControlStationUI(QMainWindow):
if drone_id not in self.drones:
self.add_drone(drone_id)
return
# 只做資料快取，不更新 UI - 所有 UI 更新都在 _update_panel_and_map 中進行
if drone_id not in self._message_cache:
@@ -2262,11 +2134,6 @@ class ControlStationUI(QMainWindow):
center_lat, center_lon, _ = center_origin
target_lat, target_lon = context['draw_target']
group.mission_target = (float(target_lat), float(target_lon))
if expected_mission_type == 'GRID_SWEEP':
# Grid Sweep 的範圍已由矩形圖層表示，不另外顯示中心／目標點。
self.drone_map.clear_mission_plan_for_group(group_id)
else:
self.drone_map.draw_mission_plan_for_group(
group_id, group.color,
center_lat, center_lon, target_lat, target_lon
@@ -2297,9 +2164,6 @@ class ControlStationUI(QMainWindow):
self.statusBar().showMessage("請先建立任務群組", 3000)
return
# 不論後續是否能進行任務規劃，都先記錄最新點擊的目標點。
group.mission_target = (float(lat), float(lon))
mode_map = {
'M_FORMATION': MissionType.M_FORMATION,
'CIRCLE_FORMATION': MissionType.CIRCLE_FORMATION,
@@ -2316,7 +2180,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', 30.0))
base_alt = params.get('base_altitude', params.get('altitude', 10.0))
target_gps = (lat, lon, base_alt)
self.statusBar().showMessage(
f"Group {group.group_id}: 正在規劃 {group.mission_type} ({len(selected_drones)} 台)", 2000)
@@ -2362,13 +2226,9 @@ class ControlStationUI(QMainWindow):
self.statusBar().showMessage("矩形座標解析失敗", 3000)
return
# 將確認後的框選範圍存入群組專屬圖層，供停止任務時精準清除。
self.drone_map.draw_grid_sweep_area_for_group(
group.group_id, group.color, rect_corners)
panel = self.group_panels.get(group.group_id)
params = panel.get_mission_params() if panel else {}
base_alt = params.get('altitude', 30.0)
base_alt = params.get('altitude', 10.0)
self.statusBar().showMessage(
f"Group {group.group_id}: 正在規劃 Grid Sweep ({len(selected_drones)} 台)", 2000)
drone_gps_positions = self._collect_drone_gps(selected_drones)
@@ -2429,7 +2289,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', 30.0)
base_alt = params.get('altitude', 10.0)
self.statusBar().showMessage(
f"Group {group.group_id}: 正在規劃跟隨模式 ({len(selected_drones)} 台)", 2000)
@@ -2582,7 +2442,7 @@ class ControlStationUI(QMainWindow):
def _format_gnss_fix_type(fix_type):
fix_labels = {
0: "No GPS",
1: "No Fix",
1: "No GPS",
2: "2D Fix",
3: "3D Fix",
4: "DGPS",
@@ -2592,7 +2452,7 @@ class ControlStationUI(QMainWindow):
try:
return fix_labels.get(int(fix_type), f"Fix {fix_type}")
except (TypeError, ValueError):
return "-"
return "--"
def update_overview_table(self, drone_id=None, field=None, value=None):
if not hasattr(self, 'overview_table') or self.overview_table is None: return
@@ -2635,59 +2495,19 @@ class ControlStationUI(QMainWindow):
MissionPhase.IDLE.value: "待機（尚未設定敵機/A 組）",
MissionPhase.WATCH.value: "監看偵測中",
MissionPhase.DETECTED.value: "已偵測到敵機 — 等待確認重編隊",
MissionPhase.CONVERGE.value: "收斂中（動態分配包圍位置）",
MissionPhase.PURSUE.value: "前進中（4 機 leader-follow）",
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 陣形失去敵機機首方向。
"""
if hasattr(self.monitor, 'get_drone_gps_snapshot'):
snapshot = self.monitor.get_drone_gps_snapshot()
else:
source = getattr(self.monitor, 'drone_gps', {})
snapshot = {
drone_id: dict(pos)
for drone_id, pos in source.items()
if isinstance(pos, dict)
}
now = time.monotonic()
for position in snapshot.values():
relative_updated_at = position.get('_relative_alt_updated_at')
relative_is_fresh = (
position.get('_altitude_reference') == 'relative_home'
and isinstance(relative_updated_at, (int, float))
and 0.0 <= now - relative_updated_at
<= self.ENGAGEMENT_TELEMETRY_TIMEOUT_SEC
)
if not relative_is_fresh:
# 不得把 AMSL 或過期高度帶入 GLOBAL_RELATIVE_ALT 指令。
position.pop('alt', None)
for drone_id, heading in self.drone_headings.items():
updated_at = self.drone_heading_updated_at.get(drone_id)
heading_is_fresh = (
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=self._engagement_gps_provider,
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,
target_lost_timeout_sec=self.ENGAGEMENT_TELEMETRY_TIMEOUT_SEC,
)
self._engagement_a_group = None
self._engagement_b_group = None
@@ -2789,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()
@@ -2822,61 +2649,6 @@ 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[13], drones_by_sysid[14]},
'C': {drones_by_sysid[15]},
}
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[15]
if not self.engagement_panel.set_configuration(enemy_id, 'A', 'B'):
_log("WARN", "自動交戰編組完成，但交戰面板設定失敗")
return
self._sync_orchestrator_config()
self.statusBar().showMessage(
"已自動設定交戰：A=sysid 11,12；B=sysid 13,14；敵機=sysid 15（Group C）",
6000)
def reorganize_socket_groups(self):
while self.drone_panel_layout.count():
@@ -2926,21 +2698,15 @@ class ControlStationUI(QMainWindow):
# 處理所有快取的消息類型
for msg_type, data in cached_data.items():
if msg_type == 'state':
mode_value = data.get('mode')
mode = str(mode_value) if mode_value is not None else '-'
mode = data.get('mode', 'UNKNOWN')
armed = data.get('armed', None)
if mode == '-':
mode_color = '#AAAAAA'
else:
mode_color = '#FF5555' if any(
x in mode.upper() for x in ['RTL', '返航', 'EMERGENCY']
) else '#55FF55'
mode_color = '#FF5555' if any(x in mode.upper() for x in ['RTL', '返航', 'EMERGENCY']) else '#55FF55'
if armed is True:
arm_text, arm_color = "ARMED", '#55FF55'
elif armed is False:
arm_text, arm_color = "DISARMED", '#FF5555'
else:
arm_text, arm_color = "-", '#AAAAAA'
arm_text, arm_color = "--", '#AAAAAA'
self.update_field(panel, drone_id, 'mode', mode, mode_color)
self.update_field(panel, drone_id, 'armed', arm_text, arm_color)
self.queue_overview_update(drone_id, 'mode', mode)
@@ -2948,59 +2714,43 @@ class ControlStationUI(QMainWindow):
elif msg_type == 'battery':
voltage = data.get('voltage')
cells = round(voltage / 3.95) if isinstance(voltage, (int, float)) else None
calculated_percentage = (
cells = round(voltage / 3.95) if voltage is not None else None
percentage = (
(voltage / cells - 3.7) / 0.5 * 100
if isinstance(voltage, (int, float)) and cells and cells > 0
else None
if voltage is not None and cells and cells > 0
else data.get('percentage', 0)
)
percentage = data.get('percentage')
if not isinstance(percentage, (int, float)):
percentage = calculated_percentage
if isinstance(percentage, (int, float)):
if percentage < 20: voltage_color = '#FF6464'
elif percentage < 50: voltage_color = '#FFA500'
else: voltage_color = '#FFFFFF'
percentage_text = f"{percentage:.0f}%"
else:
voltage_color = '#AAAAAA'
percentage_text = '-'
self.update_field(panel, drone_id, 'battery_pct', percentage_text, voltage_color)
if isinstance(voltage, (int, float)):
percentage = data.get('percentage', percentage)
self.update_field(panel, drone_id, 'battery_pct', f"{percentage:.0f}%", voltage_color)
if voltage is not None:
self.update_field(panel, drone_id, 'battery_sep1', " - ")
self.update_field(panel, drone_id, 'battery_vol', f"{voltage:.2f}V")
self.update_field(panel, drone_id, 'battery_sep2', " - ")
self.update_field(panel, drone_id, 'battery_cells', f"{cells}S")
self.queue_overview_update(drone_id, 'battery', f"{voltage:.2f}V")
elif data.get('_snapshot'):
self.update_field(panel, drone_id, 'battery_sep1', " - ")
self.update_field(panel, drone_id, 'battery_vol', "-")
self.update_field(panel, drone_id, 'battery_sep2', " - ")
self.update_field(panel, drone_id, 'battery_cells', "-")
self.queue_overview_update(drone_id, 'battery', percentage_text)
else:
self.update_field(panel, drone_id, 'battery_sep1', "")
self.update_field(panel, drone_id, 'battery_vol', "")
self.update_field(panel, drone_id, 'battery_sep2', "")
self.update_field(panel, drone_id, 'battery_cells', "")
self.queue_overview_update(drone_id, 'battery', percentage_text)
self.queue_overview_update(drone_id, 'battery', f"{percentage:.0f}%")
elif msg_type == 'altitude':
altitude = data.get('altitude')
text = f"{altitude:.1f} m" if isinstance(altitude, (int, float)) else '-'
altitude = data.get('altitude', 0)
text = f"{altitude:.1f} m"
self.update_field(panel, drone_id, 'altitude', text)
self.queue_overview_update(drone_id, 'altitude', text)
elif msg_type == 'local_pose':
x, y = data.get('x'), data.get('y')
if isinstance(x, (int, float)) and isinstance(y, (int, float)):
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}
local_text = f"{x:.1f}, {y:.1f}"
else:
local_text = '-'
self.queue_overview_update(drone_id, 'local', local_text)
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':
text = f"{data.get('loss_rate', 0):.1f}%"
@@ -3013,52 +2763,24 @@ class ControlStationUI(QMainWindow):
self.queue_overview_update(drone_id, 'ping', text)
elif msg_type == 'velocity':
vx, vy = data.get('vx'), data.get('vy')
velocity_text = (
f"{vx:.1f}, {vy:.1f}"
if isinstance(vx, (int, float)) and isinstance(vy, (int, float))
else '-'
)
self.queue_overview_update(drone_id, 'velocity', velocity_text)
self.queue_overview_update(drone_id, 'velocity', f"{data['vx']:.1f}, {data['vy']:.1f}")
elif msg_type == 'attitude':
roll, pitch, yaw = data.get('roll'), data.get('pitch'), data.get('yaw')
self.queue_overview_update(
drone_id, 'roll',
f"{roll:.1f}°" if isinstance(roll, (int, float)) else '-'
)
self.queue_overview_update(
drone_id, 'pitch',
f"{pitch:.1f}°" if isinstance(pitch, (int, float)) else '-'
)
self.queue_overview_update(
drone_id, 'yaw',
f"{yaw:.1f}°" if isinstance(yaw, (int, float)) else '-'
)
roll, pitch, yaw = data.get('roll', 0), data.get('pitch', 0), data.get('yaw', 0)
self.queue_overview_update(drone_id, 'roll', f"{roll:.1f}°")
self.queue_overview_update(drone_id, 'pitch', f"{pitch:.1f}°")
self.queue_overview_update(drone_id, 'yaw', f"{yaw:.1f}°")
elif msg_type == 'gps':
gps_data = dict(data)
lat, lon = gps_data.get('lat'), gps_data.get('lon')
has_position = (
isinstance(lat, (int, float)) and isinstance(lon, (int, float))
)
if has_position:
gps_data = data
lat, lon = gps_data.get('lat', 0), gps_data.get('lon', 0)
self.drone_positions[drone_id] = (lat, lon)
self._map_dirty_drones.add(drone_id)
if not hasattr(self.monitor, 'drone_gps'):
self.monitor.drone_gps = {}
# ROS callback 已先寫入含時間戳的快照；其餘來源的
# GLOBAL_POSITION_INT/JSON 高度均定義為相對 Home。
if '_gps_updated_at' not in gps_data:
gps_data = self.monitor.record_relative_gps(
drone_id, gps_data
)
self.queue_overview_update(
drone_id, 'latitude', f"{lat:.6f}°" if has_position else '-'
)
self.queue_overview_update(
drone_id, 'longitude', f"{lon:.6f}°" if has_position else '-'
)
self.monitor.drone_gps[drone_id] = gps_data.copy()
self.queue_overview_update(drone_id, 'latitude', f"{lat:.6f}°")
self.queue_overview_update(drone_id, 'longitude', f"{lon:.6f}°")
fix_type = gps_data.get('fix_type')
satellites_visible = gps_data.get('satellites_visible')
eph = gps_data.get('eph')
@@ -3068,7 +2790,7 @@ class ControlStationUI(QMainWindow):
if isinstance(eph, (int, float)) and isinstance(epv, (int, float)):
error_text = f"{eph:.1f}/{epv:.1f}"
else:
error_text = "-"
error_text = "--"
self.update_field(panel, drone_id, 'gnss_error', error_text)
self.queue_overview_update(
drone_id, 'fix_type',
@@ -3076,42 +2798,38 @@ class ControlStationUI(QMainWindow):
)
self.queue_overview_update(
drone_id, 'satellites_visible',
str(satellites_visible) if satellites_visible is not None else "-"
str(satellites_visible) if satellites_visible is not None else "--"
)
self.queue_overview_update(
drone_id, 'eph',
f"{eph:.2f}" if isinstance(eph, (int, float)) else "-"
f"{eph:.2f}" if isinstance(eph, (int, float)) else "--"
)
self.queue_overview_update(
drone_id, 'epv',
f"{epv:.2f}" if isinstance(epv, (int, float)) else "-"
f"{epv:.2f}" if isinstance(epv, (int, float)) else "--"
)
elif msg_type == 'hud':
hud_data = data
heading = hud_data.get('heading')
if isinstance(heading, (int, float)):
heading = hud_data.get('heading', 0)
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')
throttle = hud_data.get('throttle')
hud_alt = hud_data.get('alt')
climb = hud_data.get('climb')
groundspeed = hud_data.get('groundspeed', 0)
airspeed = hud_data.get('airspeed', 0)
throttle = hud_data.get('throttle', 0)
hud_alt = hud_data.get('alt', 0)
climb = hud_data.get('climb', 0)
heading_text = f"{heading:.1f}°" if isinstance(heading, (int, float)) else '-'
groundspeed_text = f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else '-'
self.queue_overview_update(drone_id, 'heading', heading_text)
self.queue_overview_update(drone_id, 'groundspeed', groundspeed_text)
self.queue_overview_update(drone_id, 'airspeed', f"{airspeed:.1f} m/s" if isinstance(airspeed, (int, float)) else "-")
self.queue_overview_update(drone_id, 'throttle', f"{throttle:.0f}%" if isinstance(throttle, (int, float)) else "-")
self.queue_overview_update(drone_id, 'hud_alt', f"{hud_alt:.1f} m" if isinstance(hud_alt, (int, float)) else "-")
self.queue_overview_update(drone_id, 'climb', f"{climb:.1f} m/s" if isinstance(climb, (int, float)) else "-")
self.queue_overview_update(drone_id, 'heading', f"{heading:.1f}°")
self.queue_overview_update(drone_id, 'groundspeed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--")
self.queue_overview_update(drone_id, 'airspeed', f"{airspeed:.1f} m/s" if isinstance(airspeed, (int, float)) else "--")
self.queue_overview_update(drone_id, 'throttle', f"{throttle:.0f}%" if isinstance(throttle, (int, float)) else "--")
self.queue_overview_update(drone_id, 'hud_alt', f"{hud_alt:.1f} m" if isinstance(hud_alt, (int, float)) else "--")
self.queue_overview_update(drone_id, 'climb', f"{climb:.1f} m/s" if isinstance(climb, (int, float)) else "--")
self.update_field(panel, drone_id, 'heading', heading_text)
self.update_field(panel, drone_id, 'groundspeed', groundspeed_text)
self.update_field(panel, drone_id, 'speed', groundspeed_text)
self.update_field(panel, drone_id, 'heading', f"{heading:.1f}°")
self.update_field(panel, drone_id, 'groundspeed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--")
self.update_field(panel, drone_id, 'speed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--")
elapsed = (time.time() - start_time) * 1000
@@ -3151,13 +2869,9 @@ class ControlStationUI(QMainWindow):
if not panel or not hasattr(panel, 'update_attitude'):
continue
roll = data.get('roll')
pitch = data.get('pitch')
yaw = data.get('yaw')
# The ADI is graphical and cannot render '-'. Keep its last valid
# pose while the text/table fields show missing snapshot values.
if not all(isinstance(value, (int, float)) for value in (roll, pitch, yaw)):
continue
roll = data.get('roll', 0)
pitch = data.get('pitch', 0)
yaw = data.get('yaw', 0)
heading = self.drone_headings.get(drone_id, yaw)
panel._last_roll = roll
src/GUI/map_layout.py
+27 -109
View File
@@ -21,8 +21,6 @@ class DroneMap:
self.font_scale = 1.0
self.socket_colors = {}
self.drone_group_colors = {}
self.mission_following_enabled = False
self.mission_following_drone_id = None
# 創建橋接對象
self.bridge = MapBridge()
@@ -201,18 +199,16 @@ class DroneMap:
bridge = channel.objects.bridge;
});
var map = L.map('map', {maxZoom: 20}).setView([0, 0], 19);
var map = L.map('map').setView([0, 0], 19);
// 創建不同的地圖圖層
var streetLayer = L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', {
maxNativeZoom: 19,
maxZoom: 20,
maxZoom: 19,
attribution: '© OpenStreetMap contributors'
});
var satelliteLayer = L.tileLayer('https://server.arcgisonline.com/ArcGIS/rest/services/World_Imagery/MapServer/tile/{z}/{y}/{x}', {
maxNativeZoom: 19,
maxZoom: 20,
maxZoom: 19,
attribution: 'Tiles © Esri'
});
@@ -221,6 +217,7 @@ class DroneMap:
currentLayer.addTo(map);
var isSatellite = true;
// 地圖點擊事件
map.on('click', function(e) {
if (selectionMode === 'polygon') {
@@ -352,7 +349,6 @@ class DroneMap:
var socketColors = {};
var droneGroupColors = {};
var focusedId = null;
var missionFollowingEnabled = false;
var initialized = false;
var trajectoryGroup = L.layerGroup().addTo(map);
@@ -368,8 +364,6 @@ class DroneMap:
// 多群組任務規劃 — 每個 group 各自的 layer group
var groupMissionLayers = {}; // group_id → L.layerGroup
// Grid Sweep 框選範圍也依群組保存，停止時可單獨清除。
var gridSweepAreaLayers = {}; // group_id → L.layerGroup
// 選擇工具變量
var selectionMode = null; // 'drones', 'rect', 'polygon', 'route', null
@@ -583,6 +577,23 @@ class DroneMap:
[bounds.getSouth(), bounds.getEast()],
[bounds.getSouth(), bounds.getWest()]
];
L.rectangle(bounds, {
color: '#FF6B6B',
fillColor: '#FF6B6B',
fillOpacity: 0.2,
weight: 2
}).addTo(selectionLayer);
rectPoints.forEach(point => {
L.circleMarker(point, {
radius: 5,
color: '#FF6B6B',
fillColor: '#FF6B6B',
fillOpacity: 0.8
}).addTo(selectionLayer);
});
if (bridge) {
var pointsStr = JSON.stringify(rectPoints);
bridge.rectangleSelected(pointsStr);
@@ -756,34 +767,15 @@ class DroneMap:
function focusOn(id) {
if (!markers[id]) return;
// 非任務狀態只移動一次；任務執行中才記住焦點持續跟隨。
focusedId = missionFollowingEnabled ? id : null;
focusedId = id;
var latlng = markers[id].getLatLng();
autoCenteringMap = true;
map.flyTo(latlng, map.getZoom());
setTimeout(() => { autoCenteringMap = false; }, 500);
}
function setMissionFollowingEnabled(enabled, preferredId) {
missionFollowingEnabled = Boolean(enabled);
if (!missionFollowingEnabled) {
// 非任務狀態不跟隨 UAV 或目標點，保留使用者手動地圖視角。
focusedId = null;
return;
}
// 任務已在執行時保留原有焦點；尚無焦點時跟隨指定 UAV。
if (focusedId && markers[focusedId]) {
return;
}
if (preferredId && markers[preferredId]) {
focusOn(preferredId);
initialized = true;
}
}
setInterval(() => {
if (missionFollowingEnabled && focusedId && markers[focusedId]) {
if (focusedId && markers[focusedId]) {
var latlng = markers[focusedId].getLatLng();
autoFollowPanTo(latlng);
}
@@ -801,7 +793,7 @@ class DroneMap:
.setLatLng([lat, lon])
.setRotationAngle(heading);
idLabels[id].setLatLng([lat, lon]);
if (missionFollowingEnabled && id === focusedId) {
if (id === focusedId) {
autoFollowPanTo(markers[id].getLatLng());
}
} else {
@@ -821,6 +813,7 @@ class DroneMap:
if (bridge) {
bridge.emitDroneClicked(id);
}
focusOn(id);
})
.addTo(map);
@@ -835,23 +828,14 @@ class DroneMap:
if (bridge) {
bridge.emitDroneClicked(id);
}
focusOn(id);
})
.addTo(map);
if (!initialized) {
// 第一次收到 UAV 位置時只做一次初始置中，
// 非任務狀態不設 focusedId，所以後續不會跟隨。
if (missionFollowingEnabled) {
if (!initialized || id < focusedId) {
focusedId = id;
}
autoCenteringMap = true;
map.setView([lat, lon], 19);
setTimeout(() => { autoCenteringMap = false; }, 300);
initialized = true;
} else if (missionFollowingEnabled &&
(!focusedId || id < focusedId)) {
focusedId = id;
map.setView([lat, lon], 19);
}
}
}
@@ -959,41 +943,6 @@ class DroneMap:
}
}
function drawGridSweepAreaForGroup(groupId, color, rectPoints) {
clearGridSweepAreaForGroup(groupId);
var layerGroup = L.layerGroup().addTo(map);
gridSweepAreaLayers[groupId] = layerGroup;
var bounds = L.latLngBounds(rectPoints);
L.rectangle(bounds, {
color: color,
fillColor: color,
fillOpacity: 0.2,
weight: 2
}).addTo(layerGroup);
rectPoints.forEach(point => {
L.circleMarker(point, {
radius: 5,
color: color,
fillColor: color,
fillOpacity: 0.8
}).addTo(layerGroup);
});
}
function clearGridSweepAreaForGroup(groupId, restartSelection) {
if (gridSweepAreaLayers[groupId]) {
map.removeLayer(gridSweepAreaLayers[groupId]);
delete gridSweepAreaLayers[groupId];
}
if (restartSelection) {
clearSelectionMode();
selectionMode = 'rect';
map.dragging.disable();
console.log('Grid Sweep: 已清除範圍並重新進入框選模式');
}
}
function clearAllMissionPlans() {
for (var gid in groupMissionLayers) {
map.removeLayer(groupMissionLayers[gid]);
@@ -1026,9 +975,6 @@ class DroneMap:
self.set_font_scale(self.font_scale)
self.set_socket_colors(self.socket_colors)
self.set_drone_group_colors(self.drone_group_colors)
self.set_mission_following(
self.mission_following_enabled,
self.mission_following_drone_id)
else:
_log("ERROR", "地圖載入失敗")
@@ -1075,17 +1021,6 @@ class DroneMap:
if self.map_loaded:
self.map_view.page().runJavaScript(f"focusOn('{drone_id}');")
def set_mission_following(self, enabled, preferred_drone_id=None):
"""只在任務執行中啟用 UAV 自動跟隨。"""
self.mission_following_enabled = bool(enabled)
self.mission_following_drone_id = preferred_drone_id
if self.map_loaded:
preferred_json = json.dumps(preferred_drone_id)
self.map_view.page().runJavaScript(
"setMissionFollowingEnabled("
f"{str(self.mission_following_enabled).lower()}, "
f"{preferred_json});")
def set_font_scale(self, scale):
"""設定地圖 HTML 控制項的字體倍率。"""
self.font_scale = scale
@@ -1130,23 +1065,6 @@ class DroneMap:
self.map_view.page().runJavaScript(f"clearMissionPlanForGroup('{group_id}');")
_log("INFO", f"地圖已清除 Group {group_id} 任務規劃")
def draw_grid_sweep_area_for_group(self, group_id, color, rect_corners):
"""繪製指定群組的 Grid Sweep 框選範圍。"""
if self.map_loaded:
group_json = json.dumps(group_id)
color_json = json.dumps(color)
corners_json = json.dumps(rect_corners)
self.map_view.page().runJavaScript(
f"drawGridSweepAreaForGroup({group_json}, {color_json}, {corners_json});")
def clear_grid_sweep_area_for_group(self, group_id, restart_selection=False):
"""清除指定群組的 Grid Sweep 範圍，可選擇重新進入框選模式。"""
if self.map_loaded:
group_json = json.dumps(group_id)
restart_js = str(bool(restart_selection)).lower()
self.map_view.page().runJavaScript(
f"clearGridSweepAreaForGroup({group_json}, {restart_js});")
def set_mission_mode(self, mode):
"""從 Python 端切換地圖的任務模式（觸發框選/路徑標記等）"""
if self.map_loaded:
src/GUI/mission_group.py
+6 -49
View File
@@ -24,10 +24,10 @@ GROUP_COLORS = [
DEFAULT_MISSION_PARAM_VALUES = {
'spacing': '5.0',
'base_altitude': '30.0',
'base_altitude': '10.0',
'altitude_diff': '2.0',
'radius': '10.0',
'altitude': '30.0',
'altitude': '10.0',
'start_angle': '0',
'lateral_offset': '3.0',
'longitudinal_spacing': '5.0',
@@ -57,7 +57,6 @@ class MissionGroup:
self.planned_waypoints = None # 規劃結果 dict
self.executor = None # MissionExecutor 實例（延遲建立）
self.center_origin = None # 規劃原點
self.mission_target = None # (lat, lon)，最近點擊的地圖目標供 XBee GOTO 使用
self.leader_drone_id = None # LEADER_FOLLOWER 專用：指定的領隊無人機 ID
@property
@@ -164,7 +163,6 @@ class GroupPanel(QWidget):
arm_requested = pyqtSignal(str) # group_id
takeoff_requested = pyqtSignal(str, float) # group_id, altitude
reboot_requested = pyqtSignal(str) # group_id
xbee_command_requested = pyqtSignal(str, str) # group_id, START/STOP/GOTO
box_select_requested = pyqtSignal(str) # group_id — 框選直接分配
select_all_requested = pyqtSignal(str) # group_id — 全選直接分配
clear_group_requested = pyqtSignal(str) # group_id — 清除分組
@@ -249,8 +247,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", "30"])
self.alt_input.setCurrentText("30")
self.alt_input.addItems(["5", "10", "15", "20"])
self.alt_input.setCurrentText("10")
self.alt_input.setStyleSheet(COMBO)
alt_lbl = QLabel("m")
alt_lbl.setStyleSheet(LBL)
@@ -396,43 +394,6 @@ class GroupPanel(QWidget):
reboot_col.addWidget(reboot_btn)
reboot_col.addStretch()
# ============================
# XBee JSON 指令（GOTO 使用最近點擊的地圖目標點）
# ============================
xbee_col = QVBoxLayout()
xbee_col.setSpacing(3)
xbee_title = QLabel("XBee/WS 指令")
xbee_title.setStyleSheet(TITLE)
xbee_col.addWidget(xbee_title)
xbee_buttons = QVBoxLayout()
xbee_buttons.setSpacing(3)
start_xbee_btn = QPushButton("START")
start_xbee_btn.setStyleSheet(
BTN.format(bg='#2E7D32', fg='white', hover='#388E3C'))
start_xbee_btn.clicked.connect(
lambda: self.xbee_command_requested.emit(
self.group.group_id, 'START'))
stop_xbee_btn = QPushButton("STOP")
stop_xbee_btn.setStyleSheet(
BTN.format(bg='#C62828', fg='white', hover='#D32F2F'))
stop_xbee_btn.clicked.connect(
lambda: self.xbee_command_requested.emit(
self.group.group_id, 'STOP'))
goto_xbee_btn = QPushButton("GOTO")
goto_xbee_btn.setToolTip("前往目前 Group 最近一次點擊的地圖目標點")
goto_xbee_btn.setStyleSheet(
BTN.format(bg='#1565C0', fg='white', hover='#1976D2'))
goto_xbee_btn.clicked.connect(
lambda: self.xbee_command_requested.emit(
self.group.group_id, 'GOTO'))
xbee_buttons.addWidget(start_xbee_btn)
xbee_buttons.addWidget(stop_xbee_btn)
xbee_buttons.addWidget(goto_xbee_btn)
xbee_col.addLayout(xbee_buttons)
xbee_col.addStretch()
# ============================
# 第四欄：任務參數
# ============================
@@ -539,7 +500,7 @@ class GroupPanel(QWidget):
# 當任務類型切換時更新參數顯示
self.type_combo.currentTextChanged.connect(self._update_param_visibility)
# ── 組裝欄位：控制指令 > 任務規劃 > 任務參數 > 選取與分組 > 飛控操作 > XBee ──
# ── 組裝欄位：控制指令 > 任務規劃 > 任務參數 > 選取與分組 > 飛控操作 ──
# 使用伸展因子 0 讓列根據內容自動調整寬度，而不是均等分配
cols.addLayout(left, 0)
cols.addWidget(self._make_sep())
@@ -550,8 +511,6 @@ class GroupPanel(QWidget):
cols.addLayout(right, 0)
cols.addWidget(self._make_sep())
cols.addLayout(reboot_col, 0)
cols.addWidget(self._make_sep())
cols.addLayout(xbee_col, 0)
cols.addStretch() # 填充剩餘空間，使欄位置左
layout.addLayout(cols)
@@ -660,7 +619,6 @@ class GroupPanel(QWidget):
def update_mission_info(self, center_lat, center_lon, target_lat, target_lon):
"""更新中心點 / 目標點顯示"""
self.group.mission_target = (float(target_lat), float(target_lon))
info_style = f"color: {self.group.color}; font-size: 11px; font-weight: bold;"
self.center_label.setText(f"中心: {center_lat:.6f}°, {center_lon:.6f}°")
self.center_label.setStyleSheet(info_style)
@@ -669,7 +627,6 @@ class GroupPanel(QWidget):
def clear_mission_info(self):
"""清除中心點 / 目標點顯示"""
self.group.mission_target = None
self.center_label.setText("中心: --")
self.center_label.setStyleSheet("color: #AAA; font-size: 11px;")
self.target_label.setText("目標: --")
@@ -679,5 +636,5 @@ class GroupPanel(QWidget):
try:
alt = float(self.alt_input.currentText())
except ValueError:
alt = 30.0
alt = 10.0
self.takeoff_requested.emit(self.group.group_id, alt)
src/GUI/mission_orchestrator.py
+291 -1731
View File
File diff suppressed because it is too large Load Diff
src/GUI/mission_orchestrator1.py
-710
View File
@@ -1,710 +0,0 @@
#!/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 = 30.0,
base_altitude: float = 10.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', 30.0)
altitude = params.get('altitude', 10.0)
start_angle = params.get('start_angle', 0.0)
center_x, center_y, center_z = target_point
src/GUI/overview_table.py
+3 -3
View File
@@ -115,11 +115,11 @@ class OverviewTable(QTableWidget):
panel = self.drones[did]
for field, row in self.info_type_map.items():
if field == 'mission':
# 任務狀態非遙測，取自獨立保存，避免被 "-" 蓋掉
val = self.mission_status.get(did, "-")
# 任務狀態非遙測，取自獨立保存，避免被 "--" 蓋掉
val = self.mission_status.get(did, "--")
else:
lbl = panel.findChild(QLabel, f"{did}_{field}")
val = lbl.text() if lbl else "-"
val = lbl.text() if lbl else "--"
val_item = QTableWidgetItem(val)
val_item.setTextAlignment(Qt.AlignmentFlag.AlignCenter)
self.setItem(row, col, val_item)
src/GUI/websocket_client.py
-155
View File
@@ -1,155 +0,0 @@
import asyncio
import websockets
import json
import sys
WS_URL = "ws://127.0.0.1:8765"
SYSTEM_ID = 2
GOTO_POSITION = [24.1234567, 120.1234567]
class WebSocketClient:
def __init__(self, ws_url=WS_URL, system_id=SYSTEM_ID):
self.ws_url = ws_url
if isinstance(system_id, bool) or not isinstance(system_id, int):
raise ValueError("system_id 必須為整數")
self.system_id = system_id
def process_websocket_message(self, data):
"""解析 server 的 Essential／Full UAV 狀態或命令 ACK。"""
if isinstance(data, str):
try:
data = json.loads(data)
except json.JSONDecodeError as e:
print(f"JSON 格式錯誤: {e}")
return
if not isinstance(data, dict):
print(f"未知訊息格式: {data}")
return
if all(key in data for key in ("result", "command", "message")):
print(
f"ACK | Command:{data.get('command', '-')} | "
f"Result:{data.get('result', '-')} | "
f"Message:{data.get('message', '-')}"
)
return
if "s" not in data:
print(f"未知 JSON 訊息: {data}")
return
position = data.get("p")
if isinstance(position, (list, tuple)) and len(position) >= 2:
position_text = f"{position[0]}, {position[1]}"
else:
position_text = "-"
ypr = data.get("ypr")
if isinstance(ypr, (list, tuple)) and len(ypr) >= 3:
yaw, pitch, roll = ypr[0], ypr[1], ypr[2]
else:
yaw, pitch, roll = data.get("y"), None, None
dop = data.get("d")
if isinstance(dop, (list, tuple)) and len(dop) >= 2:
dop_text = f"{dop[0]}/{dop[1]}"
else:
dop_text = "-"
armed = data.get("a")
if armed is None:
armed_text = "-"
else:
armed_text = "ARMED" if bool(armed) else "DISARMED"
def value_or_dash(value, suffix=""):
return f"{value}{suffix}" if value is not None else "-"
print(
f"UAV | ID:{data.get('s', '-')} | "
f"Mode:{value_or_dash(data.get('m'))} | "
f"Arm:{armed_text} | "
f"Battery:{value_or_dash(data.get('b'), '%')} | "
f"Position:{position_text} | "
f"Height:{value_or_dash(data.get('h'), 'm')} | "
f"Velocity:{value_or_dash(data.get('v'), 'm/s')} | "
f"Yaw:{value_or_dash(yaw, 'deg')} | "
f"Pitch:{value_or_dash(pitch, 'deg')} | "
f"Roll:{value_or_dash(roll, 'deg')} | "
f"GPS:{value_or_dash(data.get('g'))} | DOP(H/V):{dop_text}"
)
async def send_commands_loop(self, websocket):
"""從 stdin 讀取按鍵 (輸入後按 Enter) 並發送對應的 JSON 指令"""
loop = asyncio.get_event_loop()
print("輸入 1 = START, 2 = STOP, 3 = GOTO (然後按 Enter)")
while True:
# readline 會包含換行
try:
line = await loop.run_in_executor(None, sys.stdin.readline)
except Exception:
return
if not line:
# EOF
return
cmd = line.strip()
if cmd == '1':
msg = {"s": self.system_id, "c": "START"}
elif cmd == '2':
msg = {"s": self.system_id, "c": "STOP"}
elif cmd == '3':
msg = {
"s": self.system_id,
"c": "GOTO",
"p": list(GOTO_POSITION),
}
else:
print(f"未知指令: {cmd}")
continue
try:
await websocket.send(json.dumps(msg))
print(f"已發送: {msg}")
except Exception as e:
print(f"發送失敗: {e}")
return
async def ws_client_loop(self):
retry_count = 0
max_retries = 5
base_delay = 1.0
while retry_count < max_retries:
try:
async with websockets.connect(self.ws_url) as websocket:
print(f"WebSocket connected: {self.ws_url}")
retry_count = 0
# 啟動後台任務來接收 stdin 指令並發送
send_task = asyncio.create_task(self.send_commands_loop(websocket))
try:
async for message in websocket:
# message 已經是 string
self.process_websocket_message(message)
finally:
send_task.cancel()
await asyncio.gather(send_task, return_exceptions=True)
except websockets.exceptions.ConnectionClosedError:
print("WebSocket connection closed")
break
except Exception as e:
retry_count += 1
delay = base_delay * (2 ** min(retry_count, 4))
print(f"WebSocket connection error: {e}, retrying in {delay}s (attempt {retry_count}/{max_retries})")
await asyncio.sleep(delay)
print("WebSocket client stopped after maximum retries")
async def start(self):
"""啟動 WebSocket 客戶端"""
await self.ws_client_loop()
if __name__ == "__main__":
client = WebSocketClient()
asyncio.run(client.start())
src/GUI/websocket_server.py
-179
View File
@@ -1,179 +0,0 @@
import asyncio
import websockets
import json
from urllib.parse import urlparse
# 可切換為 "essential" 或 "full"；預設廣播完整狀態。
STATUS_FORMAT = "full"
SYSTEM_ID = 2
# WebSocket 伺服器位址；修改此參數即可更換監聽 IP/port。
WS_URL = "ws://0.0.0.0:8765"
def parse_ws_url(ws_url):
"""將 ws://host:port 解析為 websockets.serve 所需參數。"""
parsed = urlparse(ws_url)
if parsed.scheme != "ws":
raise ValueError("WS_URL 必須使用 ws:// 格式")
if not parsed.hostname:
raise ValueError("WS_URL 缺少主機位址")
try:
port = parsed.port
except ValueError as e:
raise ValueError(f"WS_URL port 格式錯誤: {e}") from e
if port is None:
raise ValueError("WS_URL 缺少 port")
return parsed.hostname, port
# 啟動伺服器
async def main(ws_url=WS_URL):
host, port = parse_ws_url(ws_url)
async with websockets.serve(client_handler, host, port):
print(f"WebSocket server started at {ws_url}")
await asyncio.Future() # run forever
# 模擬資料
def get_uav_status(status_format=None):
status_format = status_format or STATUS_FORMAT
essential_status = {
"s": SYSTEM_ID, # system ID
"m": "GUIDED", # mode
"b": 78, # battery (%)
"p": [24.1199660, 120.677230], # [lat, lon] (deg)
"y": 178.5, # yaw (deg)
}
full_status = {
"s": SYSTEM_ID, # system ID
"m": "GUIDED", # mode
"a": 1, # 1=armed, 0=disarmed
"b": 78, # battery (%)
"h": 12.3, # height (m)
"v": 4.2, # velocity (m/s)
"p": [24.1199660, 120.677230], # [lat, lon] (deg)
"ypr": [178.5, -0.1, 0.0], # yaw, pitch, roll (deg)
"g": 3, # GPS fix type
"d": [0.8, 1.2], # HDOP, VDOP
}
if status_format == "essential":
return essential_status
if status_format == "full":
return full_status
raise ValueError(
f"不支援的 STATUS_FORMAT: {status_format}，請使用 essential 或 full")
# 處理 client 傳來的訊息
async def handle_client_messages(websocket):
try:
async for message in websocket:
try:
data = json.loads(message)
print(f"Received from client: {data}")
system_id = data.get("s")
cmd = data.get("c")
# XBee 命令格式:
# {"s":2,"c":"START"}
# {"s":2,"c":"STOP"}
# {"s":2,"c":"GOTO","p":[lat,lon]}
if system_id is None:
ack = {
"result": "ERROR",
"command": cmd,
"message": "缺少 system_id 欄位 s",
}
await websocket.send(json.dumps(ack, ensure_ascii=False))
print(f"Sent ack: {ack}")
elif (isinstance(system_id, bool)
or not isinstance(system_id, int)
or system_id != SYSTEM_ID):
ack = {
"result": "ERROR",
"command": cmd,
"message": (
f"指令目標 system_id={system_id} 不符，"
f"本 UAV system_id={SYSTEM_ID}"
),
}
await websocket.send(json.dumps(ack, ensure_ascii=False))
print(f"Sent ack: {ack}")
elif isinstance(cmd, str) and cmd in {"START", "STOP", "GOTO"}:
if cmd == "START":
print(f"收到 START 指令: system_id={system_id}")
ack = {
"result": "SUCCESS",
"command": cmd,
"message": "已接收 START 指令",
}
elif cmd == "STOP":
print(f"收到 STOP 指令: system_id={system_id}")
ack = {
"result": "SUCCESS",
"command": cmd,
"message": "已接收 STOP 指令",
}
elif cmd == "GOTO":
position = data.get("p")
if isinstance(position, (list, tuple)) and len(position) >= 2:
lat, lon = position[0], position[1]
print(
f"收到 GOTO 指令: system_id={system_id}, "
f"lat={lat}, lon={lon}"
)
ack = {
"result": "SUCCESS",
"command": cmd,
"message": "已接收 GOTO 指令",
}
else:
print("GOTO 指令缺少位置 p=[lat,lon]")
ack = {
"result": "ERROR",
"command": cmd,
"message": "GOTO 指令缺少位置 p=[lat,lon]",
}
await websocket.send(json.dumps(ack, ensure_ascii=False))
print(f"Sent ack: {ack}")
else:
ack = {
"result": "ERROR",
"command": cmd,
"message": "指令 c 必須為大寫 START、STOP 或 GOTO",
}
await websocket.send(json.dumps(ack, ensure_ascii=False))
print(f"Sent ack: {ack}")
except json.JSONDecodeError:
print("❌ JSON 格式錯誤")
except websockets.ConnectionClosed:
print("❌ Client connection closed while receiving")
# 持續廣播 UAV 資訊
async def broadcast_uav_status(websocket):
try:
while True:
msg = get_uav_status()
await websocket.send(json.dumps(msg))
await asyncio.sleep(1) # 每秒發送一次
except websockets.ConnectionClosed:
print("❌ Client connection closed while sending")
# 每個 client 的處理流程
async def client_handler(websocket, path=None):
print(f"🔗 Client connected: {websocket.remote_address}")
receive_task = asyncio.create_task(handle_client_messages(websocket))
send_task = asyncio.create_task(broadcast_uav_status(websocket))
done, pending = await asyncio.wait(
[receive_task, send_task],
return_when=asyncio.FIRST_COMPLETED,
)
for task in pending:
task.cancel()
print(f"🔌 Client disconnected: {websocket.remote_address}")
if __name__ == "__main__":
asyncio.run(main(WS_URL))
src/GUI/xbee_gcs.py
-127
View File
@@ -1,127 +0,0 @@
import serial
import json
import threading
import time
from pynput import keyboard
SYSTEM_ID = 2
try:
ser = serial.Serial('/dev/ttyUSB1', 115200, timeout=0.1)
print("XBee 地面站啟動")
print("[1] START [2] STOP [3] GOTO [Esc] 退出程式")
except Exception as e:
print(f"無法開啟序列埠: {e}")
exit()
GPS_TYPE_LABELS = {
0: "no GPS",
1: "no fix",
2: "2D",
3: "3D",
4: "DGPS",
5: "RTK float",
6: "RTK fixed",
}
def format_uav_data(uav_data):
system_id = uav_data.get("s", "?")
mode = uav_data.get("m", "?")
battery = uav_data.get("b", "?")
position = uav_data.get("p", "?")
parts = [
f"[UAV] ID:{system_id}",
f"Mode:{mode}",
f"Bat:{battery}%",
f"Pos:{position}",
]
if "a" in uav_data:
armed_text = "ARMED" if int(uav_data.get("a", 0)) == 1 else "DISARMED"
parts.append(f"Arm:{armed_text}")
ypr = uav_data.get("ypr")
if isinstance(ypr, list) and len(ypr) >= 3:
parts.append(f"Height:{uav_data.get('h', '?')}m")
parts.append(f"Yaw:{ypr[0]}deg")
parts.append(f"Pitch:{ypr[1]}deg")
parts.append(f"Roll:{ypr[2]}deg")
elif "y" in uav_data:
parts.append(f"Height:{uav_data.get('h', '?')}m")
parts.append(f"Yaw:{uav_data.get('y')}deg")
elif "h" in uav_data:
parts.append(f"Heading:{uav_data.get('h')}")
if "v" in uav_data:
parts.append(f"Vel:{uav_data.get('v')}m/s")
if "g" in uav_data:
gps_type = uav_data.get("g")
parts.append(f"GPS:{gps_type}({GPS_TYPE_LABELS.get(gps_type, 'unknown')})")
dop = uav_data.get("d")
if isinstance(dop, list) and len(dop) >= 2:
parts.append(f"DOP(H/V):{dop[0]}/{dop[1]}")
return " | ".join(parts)
def receive_thread():
while True:
try:
if ser.in_waiting > 0:
line = ser.readline().decode('utf-8', errors='ignore').strip()
if line:
try:
data = json.loads(line)
# 判斷是確認訊息（ack）還是 UAV 狀態
if "result" in data and "command" in data and "message" in data:
# 這是確認訊息
result = data.get("result")
cmd = data.get("command")
msg = data.get("message")
print(f"\n[確認] 命令:{cmd} 狀態:{result} 訊息:{msg}", flush=True)
else:
# 這是 UAV 狀態訊息
print(format_uav_data(data), flush=True)
except json.JSONDecodeError:
pass
except Exception as e:
print(f"\n[接收錯誤] {e}")
time.sleep(0.01)
t = threading.Thread(target=receive_thread, daemon=True)
t.start()
def send_command(command, position=None):
try:
if command not in {"START", "STOP", "GOTO"}:
raise ValueError("指令必須為大寫 START、STOP 或 GOTO")
cmd = {
"s": SYSTEM_ID,
"c": command
}
if position is not None:
cmd["p"] = position
payload = json.dumps(cmd, separators=(',', ':')) + "\n"
ser.write(payload.encode('utf-8'))
print(f"\n>> [指令已發送] {payload.strip()}")
except Exception as e:
print(f"\n發送失敗: {e}")
def on_press(key):
try:
if key.char == "1":
send_command("START")
elif key.char == "2":
send_command("STOP")
elif key.char == "3":
send_command("GOTO", [24.1199660, 120.677230])
except AttributeError:
if key == keyboard.Key.esc:
print("\n程式結束")
return False
with keyboard.Listener(on_press=on_press) as listener:
listener.join()
src/GUI/xbee_uav.py
-141
View File
@@ -1,141 +0,0 @@
import serial
import json
import math
import time
PORT = '/dev/ttyUSB0' # 替換為實際的序列埠名稱
BAUD_RATE = 115200
SYSTEM_ID = 2
# 可切換為 "essential" 或 "full"；預設發送完整狀態。
STATUS_FORMAT = "full"
try:
ser = serial.Serial(PORT, BAUD_RATE, timeout=0.1)
print("XBee 無人機啟動...")
except Exception as e:
print(f"無法開啟序列埠: {e}")
exit()
def get_uav_status(status_format=None):
status_format = status_format or STATUS_FORMAT
essential_status = {
"s": SYSTEM_ID, # system ID
"m": "GUIDED", # mode
"b": 78, # battery (%)
"p": [24.1199660, 120.677230], # [lat, lon] (deg)
"y": 178.5, # yaw (deg)
}
full_status = {
"s": SYSTEM_ID, # system ID
"m": "GUIDED", # mode
"a": 1, # 1=armed, 0=disarmed
"b": 78, # battery (%)
"h": 12.3, # height (m)
"v": 4.2, # velocity (m/s)
"p": [24.1199660, 120.677230], # position: [lat, lon] (deg)
"ypr": [178.5, -0.1, 0.0], # yaw, pitch, roll (deg)
"g": 3, # GPS type {0: no GPS, 1: no fix, 2: 2D, 3: 3D, 4: DGPS, 5: RTK float, 6: RTK fixed}
"d": [0.8, 1.2], # DOP: [hdop, vdop]
}
if status_format == "essential":
return essential_status
if status_format == "full":
return full_status
raise ValueError(
f"不支援的 STATUS_FORMAT: {status_format}，請使用 essential 或 full")
def handle_command(cmd_obj):
# 統一指令格式: {"s": <int>, "c": "START|STOP|GOTO"}
target_s = cmd_obj.get("s")
command = cmd_obj.get("c")
ack = None
if (isinstance(target_s, bool)
or not isinstance(target_s, int)
or target_s != SYSTEM_ID):
ack = {
"result": "ERROR",
"command": command,
"message": (
f"指令目標 system_id={target_s} 不符，"
f"本 UAV system_id={SYSTEM_ID}"
),
}
elif not isinstance(command, str) or command not in {"START", "STOP", "GOTO"}:
print(f"\n[接收指令] 未知或格式錯誤的指令: {command}")
ack = {
"result": "ERROR",
"command": command,
"message": "指令 c 必須為大寫 START、STOP 或 GOTO",
}
elif command == "START":
print("\n[接收指令] 開始任務")
ack = {"result": "SUCCESS", "command": "START", "message": "成功"}
elif command == "STOP":
print("\n[接收指令] 停止任務")
ack = {"result": "SUCCESS", "command": "STOP", "message": "成功"}
elif command == "GOTO":
# 優先使用 XBee 格式 p=[lat, lon]，並兼容舊版 lat/lon 欄位。
position = cmd_obj.get("p")
if isinstance(position, (list, tuple)) and len(position) >= 2:
lat, lon = position[0], position[1]
else:
lat, lon = cmd_obj.get("lat"), cmd_obj.get("lon")
try:
if isinstance(lat, bool) or isinstance(lon, bool):
raise ValueError
lat = float(lat)
lon = float(lon)
if not math.isfinite(lat) or not math.isfinite(lon):
raise ValueError
if not -90.0 <= lat <= 90.0 or not -180.0 <= lon <= 180.0:
raise ValueError
except (TypeError, ValueError):
print("\n[接收指令] GOTO 目標無效，需要有效的 p=[lat,lon]")
ack = {
"result": "ERROR",
"command": "GOTO",
"message": "GOTO 指令缺少或包含無效的 p=[lat,lon]",
}
else:
target = [lat, lon]
print(f"\n[接收指令] 導航任務 目標位置: {target}")
ack = {
"result": "SUCCESS",
"command": "GOTO",
"p": target,
"message": "成功",
}
# 發送回覆（ack）到 GCS via serial
if ack is not None:
try:
payload = json.dumps(ack, ensure_ascii=False) + "\n"
ser.write(payload.encode('utf-8'))
print(f"已發送回覆: {payload.strip()}")
except Exception as e:
print(f"發送回覆失敗: {e}")
try:
while True:
data = get_uav_status()
json_payload = json.dumps(data, separators=(',', ':')) + "\n"
ser.write(json_payload.encode('utf-8'))
print(f"發送狀態: {json_payload.strip()}")
for _ in range(10):
if ser.in_waiting > 0:
try:
cmd_line = ser.readline().decode('utf-8').strip()
if cmd_line:
cmd_obj = json.loads(cmd_line)
handle_command(cmd_obj)
except json.JSONDecodeError:
pass
time.sleep(0.1)
except KeyboardInterrupt:
print("發送停止")
finally:
ser.close()