精简仓库结构并增强任务链路：归档双目可选后端、新增LOS判据与自定义返航
归档 legacy/：stereo_yolo/stereo_ranger/target_tracker 三包 + 175 条测试 + demo/配置/权重/三份规格；运行链路不再依赖，默认 pytest 446 条，归档测试按需运行。 目标有效性第二判据改为 LOS 运动学一致性（pod_bridge/los_consistency.py），删除 require_vision；pod_locked() 叠加该判据，状态机随之后退重搜。 自定义返航（task_runner/return_home.py）：保持转场高度回起飞点 → 恢复初始航向 → 垂直降落，不再爬 RTL_RETURN_ALT；未起飞/无起飞点/超时/异常仍退回 PX4 原生 RTL。 任务链安全兜底：主循环异常→紧急返航、stop 幂等、landing_stop 保留遥测、in_air 流复用+超时、解锁/起飞与吊舱识别仅执行一次、rtl_timeout 上限。 入口共享件拆到 utils/sitl_support.py；pod_sitl_test.py→l1_sitl_test.py；删除旧架构 test.py；相机脚本移入 tools/。 新增 .gitignore 并剔除历史误跟踪的 59 个 .pyc；requirements 按实际依赖重写；文档全面对齐，新增 legacy/README.md 与 tools/README.md。
This commit is contained in:
keita
@@ -1,20 +1,25 @@
|
||||
"""ground_link.py —— 地面坐标注入通道（预定流程.md「地面给坐标」）。
|
||||
"""ground_link.py —— 地面站链路（坐标注入 + 控制命令，预定流程.md「地面给坐标」）。
|
||||
|
||||
本吊舱无激光 → 表4 目标经纬度永不出（部署文档 §10.0），目标坐标改由地面站注入。
|
||||
本模块负责接收地面站下发的目标坐标（UDP JSON），维护一份带本地打戳的注入目标
|
||||
缓存，供 PodBridge 的 L2 地理引导**优先消费**（回退表4）。
|
||||
本模块负责接收地面站下发的两类 UDP JSON 消息：
|
||||
* 目标坐标 {"lat":..,"lon":..,"alt":..} → 维护一份带本地打戳的注入目标缓存，
|
||||
供 PodBridge 的 L2 地理引导**优先消费**（回退表4）；
|
||||
* 控制命令 {"cmd":"<name>"}（rtl / emergency / pod_*）→ 入命令队列，
|
||||
由 TaskRunner 主循环 drain_commands() 取走并分派。
|
||||
|
||||
设计要点：
|
||||
* UDP 无连接监听，每包 JSON {"lat":..,"lon":..,"alt":..}（可选 "ts" 仅诊断，不用）。
|
||||
* UDP 无连接监听；坐标包可选 "ts" 仅诊断，不用。
|
||||
* 新鲜度用「本地接收时刻」打戳（now_ms_fn 单调时钟），**不信任地面站时钟**
|
||||
（两端时钟不同步）；超龄由下游按 GroundInjectConfig.timeout_ms 判无效。
|
||||
* parse_payload 为纯函数（可单测）；socket / 事件循环拆到 run()，且 socket
|
||||
延迟导入，模块 import 无副作用（bridge 仅复用 GroundTarget 类型）。
|
||||
* 命令是事件（非状态）：队列缓存、满则丢最旧；未知命令名由上层忽略。
|
||||
* 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
|
||||
|
||||
@@ -42,6 +47,18 @@ class GroundTarget:
|
||||
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:
|
||||
"""地面坐标注入配置（全外置；数值占位，待落地时回填）。"""
|
||||
@@ -53,12 +70,13 @@ class GroundInjectConfig:
|
||||
|
||||
|
||||
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()` 读取注入坐标。
|
||||
PodBridge 通过 `ground_target_fn=lambda: link.latest()` 读取注入坐标；
|
||||
TaskRunner 主循环通过 `drain_commands()` 取控制命令。
|
||||
"""
|
||||
|
||||
def __init__(self, cfg: GroundInjectConfig,
|
||||
@@ -66,6 +84,7 @@ class GroundLink:
|
||||
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 表示从未注入）。"""
|
||||
@@ -76,6 +95,12 @@ class GroundLink:
|
||||
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
|
||||
@@ -84,15 +109,19 @@ class GroundLink:
|
||||
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（静默丢弃）。"""
|
||||
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):
|
||||
@@ -103,6 +132,27 @@ class GroundLink:
|
||||
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 时才需要
|
||||
@@ -123,13 +173,17 @@ class GroundLink:
|
||||
logger.warning("[GroundLink] 接收异常: %s", exc)
|
||||
await asyncio.sleep(0.05)
|
||||
continue
|
||||
target = self.parse_payload(data)
|
||||
if target is None:
|
||||
msg = self.parse_message(data)
|
||||
if msg is None:
|
||||
continue # 非法包静默丢弃（热路径不打日志）
|
||||
target.time_ms = self._now_ms()
|
||||
self._target = target
|
||||
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)",
|
||||
target.latitude_deg, target.longitude_deg,
|
||||
target.abs_alt_m)
|
||||
msg.latitude_deg, msg.longitude_deg,
|
||||
msg.abs_alt_m)
|
||||
finally:
|
||||
sock.close()
|
||||
|
||||
Reference in New Issue
Block a user