base_agent/uas/uas_control/controllers/uav_controller.py

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