191 lines
7.2 KiB
Python
191 lines
7.2 KiB
Python
from __future__ import annotations
|
||
|
||
import logging
|
||
import time
|
||
from typing import Dict, List, Optional
|
||
|
||
from PyQt5.QtCore import QObject, QTimer, pyqtSignal
|
||
|
||
from uas.uas_control.base_adapter import BaseProtocolAdapter
|
||
from uas.uas_control.base_controller import BaseUASController
|
||
from uas.uas_control.command import CommandResult, Commands
|
||
from uas.uas_control.telemetry import BaseTelemetry
|
||
|
||
|
||
# ═════════════════════════════════════════════════════════════════
|
||
# 多设备统一管理器
|
||
# ═════════════════════════════════════════════════════════════════
|
||
|
||
class UASManager(QObject):
|
||
"""
|
||
无人系统多设备统一管理器
|
||
|
||
职责：
|
||
- 统一注册/注销控制器（支持任意 BaseUASController 子类）
|
||
- 批量连接/断开
|
||
- 广播指令（全部设备 or 按类型筛选）
|
||
- 汇聚遥测数据
|
||
- 设备健康监控（心跳超时检测）
|
||
- 协议热切换（switch_protocol）
|
||
"""
|
||
|
||
device_added = pyqtSignal(str) # device_id
|
||
device_removed = pyqtSignal(str) # device_id
|
||
telemetry_all = pyqtSignal(str, object) # (device_id, telemetry)
|
||
alert = pyqtSignal(str, str) # (device_id, message)
|
||
|
||
HEARTBEAT_TIMEOUT_S = 5.0
|
||
|
||
def __init__(self, parent: QObject = None):
|
||
super().__init__(parent)
|
||
self._controllers: Dict[str, BaseUASController] = {}
|
||
self._last_telem_time: Dict[str, float] = {}
|
||
|
||
# 心跳监控定时器
|
||
self._hb_timer = QTimer(self)
|
||
self._hb_timer.timeout.connect(self._check_heartbeats)
|
||
self._hb_timer.start(2000)
|
||
self.logger = logging.getLogger(
|
||
f"{self.__class__.__name__}"
|
||
)
|
||
|
||
# ── 设备管理 ──────────────────────────────────────────────────
|
||
|
||
def register(self, ctrl: BaseUASController) -> None:
|
||
"""注册控制器"""
|
||
did = ctrl.device_id
|
||
self._controllers[did] = ctrl
|
||
self._last_telem_time[did] = time.time()
|
||
|
||
ctrl.telemetry_updated.connect(
|
||
lambda t, _id=did: self._on_telemetry(_id, t)
|
||
)
|
||
ctrl.error_occurred.connect(
|
||
lambda msg, _id=did: self.alert.emit(_id, msg)
|
||
)
|
||
self.device_added.emit(did)
|
||
self.logger.info(f"设备已注册: {did} [{ctrl.device_type}]")
|
||
|
||
def unregister(self, device_id: str) -> bool:
|
||
"""注销控制器"""
|
||
ctrl = self._controllers.pop(device_id, None)
|
||
if ctrl:
|
||
ctrl.disconnect()
|
||
self._last_telem_time.pop(device_id, None)
|
||
self.device_removed.emit(device_id)
|
||
self.logger.info(f"设备已注销: {device_id}")
|
||
return True
|
||
return False
|
||
|
||
def get(self, device_id: str) -> Optional[BaseUASController]:
|
||
return self._controllers.get(device_id)
|
||
|
||
def get_all(self) -> List[BaseUASController]:
|
||
return list(self._controllers.values())
|
||
|
||
def get_by_type(self, device_type: str) -> List[BaseUASController]:
|
||
return [c for c in self._controllers.values()
|
||
if c.device_type == device_type]
|
||
|
||
@property
|
||
def device_count(self) -> int:
|
||
return len(self._controllers)
|
||
|
||
# ── 批量操作 ──────────────────────────────────────────────────
|
||
|
||
def connect_all(self) -> Dict[str, bool]:
|
||
results = {}
|
||
for did, ctrl in self._controllers.items():
|
||
results[did] = ctrl.connect()
|
||
return results
|
||
|
||
def disconnect_all(self) -> None:
|
||
for ctrl in self._controllers.values():
|
||
ctrl.disconnect()
|
||
|
||
def broadcast(
|
||
self,
|
||
cmd_builder,
|
||
device_type: Optional[str] = None,
|
||
) -> Dict[str, CommandResult]:
|
||
"""
|
||
广播指令
|
||
:param cmd_builder: callable(device_id) → Command
|
||
:param device_type: 仅发送给指定类型（None=全部）
|
||
"""
|
||
targets = (self.get_by_type(device_type)
|
||
if device_type else self.get_all())
|
||
return {
|
||
ctrl.device_id: ctrl.send(cmd_builder(ctrl.device_id))
|
||
for ctrl in targets
|
||
}
|
||
|
||
def emergency_stop_all(self) -> Dict[str, CommandResult]:
|
||
"""紧急停止所有设备"""
|
||
self.logger.warning("⚠️ 紧急停止所有设备！")
|
||
return self.broadcast(lambda did: Commands.emergency_stop(did))
|
||
|
||
def switch_protocol(
|
||
self,
|
||
device_id: str,
|
||
new_adapter: BaseProtocolAdapter,
|
||
) -> bool:
|
||
"""
|
||
为指定设备热切换协议适配器
|
||
例：MAVLink → ROS，无需重启控制器
|
||
"""
|
||
ctrl = self.get(device_id)
|
||
if ctrl is None:
|
||
self.logger.error(f"设备不存在: {device_id}")
|
||
return False
|
||
ctrl.switch_adapter(new_adapter)
|
||
self.logger.info(
|
||
f"协议热切换完成: {device_id} → "
|
||
f"{new_adapter.__class__.__name__}"
|
||
)
|
||
return True
|
||
|
||
# ── 遥测汇聚 ──────────────────────────────────────────────────
|
||
|
||
def get_all_telemetry(self) -> Dict[str, Optional[BaseTelemetry]]:
|
||
return {did: ctrl.get_telemetry()
|
||
for did, ctrl in self._controllers.items()}
|
||
|
||
def _on_telemetry(self, device_id: str,
|
||
telem: BaseTelemetry) -> None:
|
||
self._last_telem_time[device_id] = time.time()
|
||
self.telemetry_all.emit(device_id, telem)
|
||
|
||
# ── 心跳监控 ──────────────────────────────────────────────────
|
||
|
||
def _check_heartbeats(self) -> None:
|
||
now = time.time()
|
||
for did, last_t in list(self._last_telem_time.items()):
|
||
gap = now - last_t
|
||
if gap > self.HEARTBEAT_TIMEOUT_S:
|
||
self.alert.emit(
|
||
did,
|
||
f"心跳超时 {gap:.1f}s，设备可能失联"
|
||
)
|
||
|
||
def status_report(self) -> str:
|
||
"""生成所有设备状态摘要"""
|
||
lines = [f"{'─'*65}", f" UASManager 状态报告 设备数={self.device_count}",
|
||
f"{'─'*65}"]
|
||
for did, ctrl in self._controllers.items():
|
||
telem = ctrl.get_telemetry()
|
||
conn = "✅已连接" if ctrl.is_connected else "❌未连接"
|
||
bat = f"🔋{telem.battery_percent}%" if telem else "🔋--"
|
||
lines.append(
|
||
f" {did:12s} [{ctrl.device_type:10s}] "
|
||
f"{conn} {bat} "
|
||
f"adapter={ctrl._adapter.__class__.__name__}"
|
||
)
|
||
lines.append(f"{'─'*65}")
|
||
return "\n".join(lines)
|
||
|
||
def __repr__(self) -> str:
|
||
return (f"UASManager("
|
||
f"devices={self.device_count}, "
|
||
f"ids={list(self._controllers.keys())})")
|