实现预定流程全链路: 任务状态机 + 编排器 + 地面站 + 真机/SITL入口
- utils/task_runner: 6态状态机(WAIT→ARM_TAKEOFF→GOTO(L2)→TRACK(L1)→APPROACH→RTL) + 编排器(快照采集/状态动作) + 双层日志(事件JSONL/遥测CSV) + 吊舱控制指令(0x07/0x0F/0x0E/0x08) - pod_bridge: ground_link 地面坐标UDP注入(超龄判无效,无激光替代表4); set_heading_servo_enabled() 运行时L1/L2切换(切换即reset清积分防输出跳变); pod_locked 提为公共门控; config 新增 ground_inject 段 - virtual_pod 模拟吊舱(无电机语义) + verify_px4 增量④仅飞控验证 + pod_sitl_test L1 SITL全流程(飞行编排不塞进 run_bridge, 保持纯offboard闭环入口) - 入口: task_runner_sitl(VirtualPod+虚拟目标, SITL端到端跑通约1m24s) / task_runner_real(真串口+UDP注入+吊舱控制) - 地面站 ground_station: 坐标注入 + 状态可视化(Flask) - S1标定回填(2026-10-09): beta_sign=+1 / alpha_sign=-1, 加标定回归测试守护 - 新增测试: task_runner x4 + ground_link/virtual_pod*/bridge/px4_gateway 增量; 全量 483 passed - 文档: 预定流程(需求) + 预定流程-操作指南(操作/待办) + L1方案补S1结论与实现状态
This commit is contained in:
keita
@@ -0,0 +1,135 @@
|
||||
"""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()
|
||||
Reference in New Issue
Block a user