120 lines
4.4 KiB
Python
120 lines
4.4 KiB
Python
from __future__ import annotations
|
||
|
||
from uas.uas_control import command_registry
|
||
from uas.uas_control.base_controller import BaseUASController
|
||
from uas.uas_control.command import Command, CommandResult, CommandType, Commands
|
||
from uas.uas_control.telemetry import BaseTelemetry, UAVTelemetry
|
||
|
||
|
||
# ═════════════════════════════════════════════════════════════════
|
||
# 无人机控制器
|
||
# ═════════════════════════════════════════════════════════════════
|
||
|
||
class UAVController(BaseUASController):
|
||
"""
|
||
无人机控制器
|
||
|
||
扩展无人机专有 API：
|
||
takeoff / land / rtl / loiter
|
||
set_altitude / set_gimbal / camera_shoot / follow_route
|
||
"""
|
||
|
||
@property
|
||
def device_type(self) -> str:
|
||
return "controller"
|
||
|
||
@command_registry.register
|
||
def takeoff(self, altitude: float, speed: float = 3.0) -> CommandResult:
|
||
altitude += self.get_telemetry().altitude
|
||
return self.send(Commands.takeoff(self.device_id, altitude, speed))
|
||
|
||
@command_registry.register
|
||
def land(self) -> CommandResult:
|
||
return self.send(Commands.land(self.device_id))
|
||
|
||
@command_registry.register
|
||
def rtl(self) -> CommandResult:
|
||
return self.send(Commands.rtl(self.device_id))
|
||
|
||
@command_registry.register
|
||
def loiter(self, radius: float = 50.0,
|
||
turns: int = 0) -> CommandResult:
|
||
return self.send(Command(
|
||
command_type = CommandType.LOITER,
|
||
target_id = self.device_id,
|
||
params = {"radius": radius, "turns": turns},
|
||
))
|
||
|
||
@command_registry.register
|
||
def set_altitude(self, altitude: float) -> CommandResult:
|
||
return self.send(Command(
|
||
command_type = CommandType.SET_ALTITUDE,
|
||
target_id = self.device_id,
|
||
params = {"altitude": altitude},
|
||
))
|
||
|
||
@command_registry.register
|
||
def set_gimbal(self, pitch: float, yaw: float = 0.0) -> CommandResult:
|
||
return self.send(Command(
|
||
command_type = CommandType.GIMBAL_CONTROL,
|
||
target_id = self.device_id,
|
||
params = {"pitch": pitch, "yaw": yaw},
|
||
))
|
||
|
||
@command_registry.register
|
||
def camera_shoot(self) -> CommandResult:
|
||
return self.send(Command(
|
||
command_type = CommandType.CAMERA_SHOOT,
|
||
target_id = self.device_id,
|
||
))
|
||
|
||
@command_registry.register
|
||
def camera_record(self, start: bool = True) -> CommandResult:
|
||
return self.send(Command(
|
||
command_type = CommandType.CAMERA_RECORD,
|
||
target_id = self.device_id,
|
||
params = {"start": start},
|
||
))
|
||
|
||
@command_registry.register
|
||
def follow_route(self, waypoints: list) -> CommandResult:
|
||
"""
|
||
执行预设航线
|
||
waypoints: [(lat, lon, alt, speed), ...]
|
||
"""
|
||
return self.send(Command(
|
||
command_type = CommandType.FOLLOW_ROUTE,
|
||
target_id = self.device_id,
|
||
params = {"waypoints": waypoints},
|
||
timeout_s = 3600.0,
|
||
))
|
||
|
||
# ── 遥测钩子 ──────────────────────────────────────────────────
|
||
|
||
def _on_telemetry(self, telem: BaseTelemetry) -> None:
|
||
if not isinstance(telem, UAVTelemetry):
|
||
return
|
||
if 0 < telem.battery_percent < 20:
|
||
self.error_occurred.emit(
|
||
f"{self.device_id}: 低电量告警 {telem.battery_percent}%，"
|
||
f"建议立即返航"
|
||
)
|
||
|
||
def _pre_send(self, cmd: Command) -> bool:
|
||
"""紧急停止指令跳过所有前置检查"""
|
||
if cmd.command_type == CommandType.EMERGENCY_STOP:
|
||
return True
|
||
telem = self._telemetry
|
||
if isinstance(telem, UAVTelemetry):
|
||
motion_cmds = {
|
||
CommandType.GOTO,
|
||
CommandType.SET_VELOCITY,
|
||
CommandType.FOLLOW_ROUTE,
|
||
}
|
||
if cmd.command_type in motion_cmds and not telem.armed:
|
||
self.error_occurred.emit(
|
||
f"{self.device_id}: 设备未解锁，拒绝运动指令"
|
||
)
|
||
return False
|
||
return True
|