code-test-drone/utils/pod_bridge/ground_link.py

"""ground_link.py —— 地面站链路（坐标注入 + 控制命令，预定流程.md「地面给坐标」）。

本吊舱无激光 → 表4 目标经纬度永不出（部署文档 §10.0），目标坐标改由地面站注入。
本模块负责接收地面站下发的两类 UDP JSON 消息：
  * 目标坐标 {"lat":..,"lon":..,"alt":..} → 维护一份带本地打戳的注入目标缓存，
    供 PodBridge 的 L2 地理引导**优先消费**（回退表4）；
  * 控制命令 {"cmd":"<name>"}（rtl / emergency / pod_*）→ 入命令队列，
    由 TaskRunner 主循环 drain_commands() 取走并分派。

设计要点：
  * UDP 无连接监听；坐标包可选 "ts" 仅诊断，不用。
  * 新鲜度用「本地接收时刻」打戳（now_ms_fn 单调时钟），**不信任地面站时钟**
    （两端时钟不同步）；超龄由下游按 GroundInjectConfig.timeout_ms 判无效。
  * 命令是事件（非状态）：队列缓存、满则丢最旧；未知命令名由上层忽略。
  * parse_payload / parse_message 为纯函数（可单测）；socket / 事件循环拆到
    run()，且 socket 延迟导入，模块 import 无副作用（bridge 仅复用 GroundTarget 类型）。
"""

import asyncio
import json
import logging
from collections import deque
from dataclasses import dataclass
from typing import Callable, Optional

from visual_track_control.staleness_checker import is_stale

logger = logging.getLogger(__name__)


def _is_number(value) -> bool:
    """数值（排除 bool）。"""
    return isinstance(value, (int, float)) and not isinstance(value, bool)


@dataclass
class GroundTarget:
    """地面注入的目标坐标（带本地接收时刻）。

    字段与 protocol.TargetGeo 语义对齐（经纬度 °、绝对高度 m），但不含表4 的
    时间字段——那些由吊舱解算生成，地面注入不提供。
    """

    latitude_deg: float = 0.0
    longitude_deg: float = 0.0
    abs_alt_m: float = 0.0
    time_ms: int = 0  # 本地接收时刻（单调毫秒），0 = 从未注入


@dataclass
class GroundCommand:
    """地面站控制命令（事件语义）。

    name 为小写命令名（rtl / emergency / pod_detect_on / …）；time_ms 为本地
    接收时刻（单调毫秒，仅日志/诊断用）。合法性（白名单）由上层分派时判。
    """

    name: str = ""
    time_ms: int = 0


@dataclass
class GroundInjectConfig:
    """地面坐标注入配置（全外置；数值占位，待落地时回填）。"""

    enabled: bool = False          # 是否启用地面注入（骨架期默认关，走表4 原路径）
    host: str = "0.0.0.0"          # UDP 监听地址
    port: int = 14555              # UDP 监听端口（避开 MAVSDK SITL 14540）
    timeout_ms: int = 5000         # 注入坐标超龄门限（超龄判无效 → 下游刹车）


class GroundLink:
    """地面站链路接收器（坐标注入 + 控制命令队列）。

    用法（在 runner 外部以 asyncio task 运行）：
        link = GroundLink(cfg, now_ms_fn)
        task = asyncio.create_task(link.run(stop_event))
    PodBridge 通过 `ground_target_fn=lambda: link.latest()` 读取注入坐标；
    TaskRunner 主循环通过 `drain_commands()` 取控制命令。
    """

    def __init__(self, cfg: GroundInjectConfig,
                 now_ms_fn: Callable[[], int]) -> None:
        self._cfg = cfg
        self._now_ms = now_ms_fn
        self._target = GroundTarget()
        self._commands: deque[str] = deque(maxlen=16)

    def latest(self) -> GroundTarget:
        """返回当前注入目标（可能 time_ms=0 表示从未注入）。"""
        return self._target

    def inject(self, target: GroundTarget) -> None:
        """直接注入目标并本地打戳（测试 / 地面站本地注入用，与 UDP run() 等价）。"""
        target.time_ms = self._now_ms()
        self._target = target

    def drain_commands(self) -> list[str]:
        """取走全部待处理命令（FIFO，取后清空）。命令为事件，无新鲜度概念。"""
        cmds = list(self._commands)
        self._commands.clear()
        return cmds

    def is_valid(self, now_ms: int) -> bool:
        """注入坐标是否新鲜有效（未注入 / 超龄 → False）。"""
        t = self._target
        if t.time_ms <= 0:
            return False
        return not is_stale(t.time_ms, now_ms, self._cfg.timeout_ms)

    @staticmethod
    def _load_object(payload: bytes) -> Optional[dict]:
        """解 JSON 且要求顶层为对象（纯函数）。非法 JSON / 非对象 → None。"""
        try:
            obj = json.loads(payload.decode("utf-8"))
        except (UnicodeDecodeError, json.JSONDecodeError):
            return None
        if not isinstance(obj, dict):
            return None
        return obj

    @staticmethod
    def _target_from_object(obj: dict) -> Optional[GroundTarget]:
        """从 dict 提取坐标（缺 lat/lon 或类型错 → None）。"""
        lat = obj.get("lat")
        lon = obj.get("lon")
        if not _is_number(lat) or not _is_number(lon):
            return None
        alt = obj.get("alt", 0.0)
        if not _is_number(alt):
            return None
        return GroundTarget(latitude_deg=float(lat), longitude_deg=float(lon),
                            abs_alt_m=float(alt))

    @staticmethod
    def parse_payload(payload: bytes) -> Optional[GroundTarget]:
        """解析坐标包（纯函数，可单测）。返回不含 time_ms 的目标；
        非法 JSON / 非对象 / 缺 lat 或 lon → None（静默丢弃）。"""
        obj = GroundLink._load_object(payload)
        if obj is None:
            return None
        return GroundLink._target_from_object(obj)

    @staticmethod
    def parse_message(payload: bytes) -> Optional[GroundTarget | GroundCommand]:
        """解析任意地面站包（纯函数）：{"cmd":".."} → GroundCommand（优先），
        否则按坐标包解析；非法包 → None（静默丢弃）。"""
        obj = GroundLink._load_object(payload)
        if obj is None:
            return None
        cmd = obj.get("cmd")
        if isinstance(cmd, str) and cmd.strip():
            return GroundCommand(name=cmd.strip().lower())
        return GroundLink._target_from_object(obj)

    async def run(self, stop_event: asyncio.Event) -> None:
        """UDP 接收循环：每收一包合法 JSON 更新注入目标并本地打戳。"""
        import socket  # 延迟导入：仅真正跑 socket 时才需要

        sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        sock.bind((self._cfg.host, self._cfg.port))
        sock.setblocking(False)
        loop = asyncio.get_running_loop()
        logger.info("[GroundLink] 监听 UDP %s:%d", self._cfg.host, self._cfg.port)
        try:
            while not stop_event.is_set():
                try:
                    data, _addr = await loop.sock_recvfrom(sock, 1024)
                except asyncio.CancelledError:
                    raise
                except OSError as exc:
                    logger.warning("[GroundLink] 接收异常: %s", exc)
                    await asyncio.sleep(0.05)
                    continue
                msg = self.parse_message(data)
                if msg is None:
                    continue  # 非法包静默丢弃（热路径不打日志）
                msg.time_ms = self._now_ms()
                if isinstance(msg, GroundCommand):
                    self._commands.append(msg.name)
                    logger.debug("[GroundLink] 收到命令: %s", msg.name)
                    continue
                self._target = msg
                logger.debug("[GroundLink] 更新注入目标 (%f, %f, %fm)",
                             msg.latitude_deg, msg.longitude_deg,
                             msg.abs_alt_m)
        finally:
            sock.close()