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

"""ground_link.py —— 地面坐标注入通道（预定流程.md「地面给坐标」）。

本吊舱无激光 → 表4 目标经纬度永不出（部署文档 §10.0），目标坐标改由地面站注入。
本模块负责接收地面站下发的目标坐标（UDP JSON），维护一份带本地打戳的注入目标
缓存，供 PodBridge 的 L2 地理引导**优先消费**（回退表4）。

设计要点：
  * UDP 无连接监听，每包 JSON {"lat":..,"lon":..,"alt":..}（可选 "ts" 仅诊断，不用）。
  * 新鲜度用「本地接收时刻」打戳（now_ms_fn 单调时钟），**不信任地面站时钟**
    （两端时钟不同步）；超龄由下游按 GroundInjectConfig.timeout_ms 判无效。
  * parse_payload 为纯函数（可单测）；socket / 事件循环拆到 run()，且 socket
    延迟导入，模块 import 无副作用（bridge 仅复用 GroundTarget 类型）。
"""

import asyncio
import json
import logging
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 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()` 读取注入坐标。
    """

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

    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 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 parse_payload(payload: bytes) -> Optional[GroundTarget]:
        """解析地面站 JSON 包（纯函数，可单测）。返回不含 time_ms 的目标；
        非法 JSON / 非对象 / 缺 lat 或 lon → None（静默丢弃）。"""
        try:
            obj = json.loads(payload.decode("utf-8"))
        except (UnicodeDecodeError, json.JSONDecodeError):
            return None
        if not isinstance(obj, dict):
            return 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))

    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
                target = self.parse_payload(data)
                if target is None:
                    continue  # 非法包静默丢弃（热路径不打日志）
                target.time_ms = self._now_ms()
                self._target = target
                logger.debug("[GroundLink] 更新注入目标 (%f, %f, %fm)",
                             target.latitude_deg, target.longitude_deg,
                             target.abs_alt_m)
        finally:
            sock.close()