精简仓库结构并增强任务链路：归档双目可选后端、新增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
+115
-13
@@ -33,6 +33,8 @@ from visual_track_control.types import (ControlMode, ControlSnapshot,
|
||||
from .config import BridgeConfig
|
||||
from .ground_link import GroundTarget
|
||||
from .guidance import GuidanceInput, GuidanceLaw
|
||||
from .los_consistency import (LosConsistencyConfig, LosConsistencyMonitor,
|
||||
LosStatus)
|
||||
from .pod_serial import PodLink, PodSerial
|
||||
from .protocol import (CarrierAttitude, ControlCommand, TargetGeo, Telemetry,
|
||||
encode_attitude, encode_control)
|
||||
@@ -49,23 +51,39 @@ class PodBridge:
|
||||
"""px4：Px4Gateway（或等价 mock）；pod：PodLink（PodSerial 或内存 FakePod）。"""
|
||||
|
||||
def __init__(self, cfg: BridgeConfig, px4: Px4Gateway, pod: PodLink,
|
||||
now_ms_fn=_monotonic_ms, vision_target_fn=None,
|
||||
now_ms_fn=_monotonic_ms,
|
||||
ground_target_fn=None) -> None:
|
||||
self._cfg = cfg
|
||||
self._px4 = px4
|
||||
self._pod = pod
|
||||
self._now_ms = now_ms_fn
|
||||
self._vision_target_fn = vision_target_fn # §2.3 第二判据（可选，S3+ 接入我方视觉）
|
||||
self._ground_target_fn = ground_target_fn # 地面注入坐标访问器（A2，无激光替代表4）
|
||||
self._use_heading_servo = cfg.heading_servo.enabled # 运行时控制律选择（A3 状态机切换）
|
||||
self._guidance = GuidanceLaw(cfg.guidance)
|
||||
self._tracking = TrackingControlLaw(cfg.tracking) # L1 航向伺服（方案 §0）
|
||||
# 目标有效性第二判据：LOS 运动学一致性（取代已废弃的 require_vision）
|
||||
hs = cfg.heading_servo
|
||||
self._los_monitor = LosConsistencyMonitor(LosConsistencyConfig(
|
||||
enabled=hs.los_consistency_enabled,
|
||||
window_ms=hs.los_window_ms,
|
||||
min_yaw_change_deg=hs.los_min_yaw_change_deg,
|
||||
max_residual_deg=hs.los_max_residual_deg))
|
||||
self._los_consistent = True # 最近一次一致性判定（公共门控用）
|
||||
self._los_logged_ok = True # 日志：只在判定翻转时打印
|
||||
self._attitude_period_ms = max(
|
||||
1, int(round(1000.0 / cfg.link.attitude_downlink_hz)))
|
||||
self._last_attitude_ms: Optional[int] = None
|
||||
# 生效律周期：step() 被外部以更高频率调用时（任务编排器统一 20Hz，而
|
||||
# L2 引导律配置为 10Hz），按此节流，避免 dt 相关项/限幅被以双倍速率推进。
|
||||
self._law_period_ms = max(
|
||||
1, int(round(1000.0 / max(1e-6, self._active_frequency_hz()))))
|
||||
self._last_law_ms: Optional[int] = None
|
||||
self._last_law_output = None # 节流期内重发的上一拍输出
|
||||
self._latest_target = None # (TargetGeo, time_ms)
|
||||
self._latest_telemetry = None # (Telemetry, time_ms)
|
||||
self._step_failures = 0 # run() 连续异常计数（告警节流用）
|
||||
self._stopped = False # stop() 幂等标志（真机实测：RTL 与收尾会各调一次）
|
||||
self._landing_stopped = False # 是否已为降落退出 offboard（遥测仍保留）
|
||||
|
||||
# --- 生命周期 -----------------------------------------------------------
|
||||
async def start(self) -> bool:
|
||||
@@ -74,13 +92,22 @@ class PodBridge:
|
||||
if not await self._px4.start_offboard():
|
||||
logger.error("[Bridge] offboard 启动失败，桥接未运行")
|
||||
return False
|
||||
self._stopped = False
|
||||
logger.info("[Bridge] 启动：引导 %.1fHz，姿态下发 %.1fHz",
|
||||
self._cfg.guidance.frequency_hz,
|
||||
self._cfg.link.attitude_downlink_hz)
|
||||
return True
|
||||
|
||||
async def stop(self) -> None:
|
||||
"""安全停机：先零速刹车，再停 offboard / 遥测 / 串口。"""
|
||||
"""安全停机：先零速刹车，再停 offboard / 遥测 / 串口。
|
||||
|
||||
幂等：任务编排器在进入 RTL 时停一次，收尾 `_teardown` 还会再停一次
|
||||
（真机在环实测确认）。重复执行会二次关闭串口、二次停 offboard，
|
||||
在 MAVSDK/串口驱动上产生无意义告警，故第二次直接返回。
|
||||
"""
|
||||
if self._stopped:
|
||||
return
|
||||
self._stopped = True
|
||||
try:
|
||||
await self._px4.send_brake_hover(self._now_ms())
|
||||
finally:
|
||||
@@ -91,6 +118,20 @@ class PodBridge:
|
||||
self._tracking.reset()
|
||||
logger.info("[Bridge] 已停止")
|
||||
|
||||
async def landing_stop(self) -> None:
|
||||
"""降落前停机：零速刹车 + 退出 offboard，但**保留遥测与串口**。
|
||||
|
||||
与 `stop()` 的差别：交给飞控 `land()` 之后仍需遥测来判定落地、记录高度
|
||||
曲线。若此时就 `stop_telemetry()`，落地段的遥测会冻结在最后高度（SITL
|
||||
实测：日志停在 5m，看不到下降）。完整清理留给收尾 `stop()`。
|
||||
"""
|
||||
try:
|
||||
await self._px4.send_brake_hover(self._now_ms())
|
||||
finally:
|
||||
await self._px4.stop_offboard()
|
||||
self._landing_stopped = True
|
||||
logger.info("[Bridge] 已退出 offboard（保留遥测用于落地判定）")
|
||||
|
||||
# --- 主循环 -------------------------------------------------------------
|
||||
async def run(self, stop_event: asyncio.Event) -> None:
|
||||
"""按引导频率循环 step()，直到 stop_event 置位。
|
||||
@@ -133,8 +174,16 @@ class PodBridge:
|
||||
self._use_heading_servo = enabled
|
||||
if enabled:
|
||||
self._tracking.reset()
|
||||
# 进入 L1：LOS 一致性历史从零开始（避免上一段跟踪的样本污染本次判定）
|
||||
self._los_monitor.reset()
|
||||
self._los_consistent = True
|
||||
self._los_logged_ok = True
|
||||
else:
|
||||
self._guidance.reset()
|
||||
# 切律后按新律频率重算节流周期，并允许立即出第一拍
|
||||
self._law_period_ms = max(
|
||||
1, int(round(1000.0 / max(1e-6, self._active_frequency_hz()))))
|
||||
self._last_law_ms = None
|
||||
|
||||
async def step(self, now_ms: int):
|
||||
"""单控制周期（显式传入时钟，便于确定性单测）。返回 ControlOutput。
|
||||
@@ -142,19 +191,43 @@ class PodBridge:
|
||||
按运行时选择的控制律在 L1（TrackingControlLaw，吊舱角度直入）与
|
||||
L2（GuidanceLaw，表4/注入坐标地理引导）间切换（方案 §7）。
|
||||
"""
|
||||
# 链路维持先做：poll 吊舱 + 按 link.attitude_downlink_hz 下发表3 姿态。
|
||||
# 这两件事与生效律频率无关（吊舱解算依赖 表3，目标门控依赖遥测新鲜度）。
|
||||
self._ingest_pod_frames(now_ms)
|
||||
|
||||
own = self._px4.own_state()
|
||||
self._downlink_attitude(own, now_ms)
|
||||
|
||||
# 生效律节流：外部（任务编排器）统一 20Hz 调用，而 L2 引导律配置为 10Hz。
|
||||
# 未到下一拍时**重发上一拍输出**（而非刹车），否则会与本拍的 GOTO 指令打架。
|
||||
if (self._last_law_ms is not None
|
||||
and now_ms - self._last_law_ms < self._law_period_ms
|
||||
and self._last_law_output is not None):
|
||||
await self._dispatch(self._last_law_output, now_ms)
|
||||
return self._last_law_output
|
||||
|
||||
self._last_law_ms = now_ms
|
||||
if self._use_heading_servo:
|
||||
out = self._tracking.update(self._build_snapshot(own, now_ms), now_ms)
|
||||
else:
|
||||
out = self._guidance.update(
|
||||
self._build_guidance_input(own, now_ms), now_ms)
|
||||
await self._dispatch(out, now_ms)
|
||||
self._last_law_output = out
|
||||
return out
|
||||
|
||||
async def search_step(self, now_ms: int, yaw_rate_dps: float) -> None:
|
||||
"""搜索周期（SEARCH 态）：只维持链路与心跳 + 原地偏航扫视，不出控制律。
|
||||
|
||||
之所以不让 SEARCH 走 step()：该态既不用 L2 地理引导（已在参考点，再发
|
||||
速度会漂离），也不用 L1（还没锁定目标，控制律会判丢输出刹车）。此处
|
||||
**仍要 poll 吊舱并下发表3 姿态**——否则搜索期间吊舱遥测超龄、锁定位永远
|
||||
不新鲜，搜到目标也切不进 TRACK。
|
||||
"""
|
||||
self._ingest_pod_frames(now_ms)
|
||||
own = self._px4.own_state()
|
||||
self._downlink_attitude(own, now_ms)
|
||||
await self._px4.send_yaw_rate(yaw_rate_dps)
|
||||
|
||||
# --- 吊舱侧 -------------------------------------------------------------
|
||||
def _ingest_pod_frames(self, now_ms: int) -> None:
|
||||
for frame in self._pod.poll():
|
||||
@@ -239,16 +312,24 @@ class PodBridge:
|
||||
return target
|
||||
|
||||
def pod_locked(self, now_ms: int) -> bool:
|
||||
"""吊舱目标门控（公共接口，供编排层/状态机复用，A3）。"""
|
||||
return self._pod_locked(now_ms)
|
||||
"""目标门控（公共接口，供编排层/状态机复用，A3）。
|
||||
|
||||
= 吊舱 bit2 门控 **AND** LOS 运动学一致性判定。
|
||||
这样"bit2 粘滞但角度冻结"的幽灵目标会让状态机退出 TRACK 重新搜索，
|
||||
而不只是让控制律单方面刹车（否则状态机会一直以为还锁着）。
|
||||
"""
|
||||
return self._pod_locked(now_ms) and self._los_consistent
|
||||
|
||||
def _pod_locked(self, now_ms: int) -> bool:
|
||||
"""吊舱目标门控：表2-2 遥测在 link.telemetry_timeout_ms 内、状态字节1
|
||||
bit2=锁定、且（若 link.require_laser_valid）激光测距有效，才算目标有效。
|
||||
"""吊舱 bit2 门控（**不含** LOS 一致性，供内部合成本拍快照用）。
|
||||
|
||||
表2-2 遥测在 link.telemetry_timeout_ms 内、状态字节1 bit2=锁定、
|
||||
且（若 link.require_laser_valid）激光测距有效，才算"吊舱自报锁定"。
|
||||
|
||||
实测依据：部分吊舱目标丢失后**锁定位置位不复位**（脱靶量也没有无效标志位，
|
||||
恒为 0），仅凭 bit2 会朝过期坐标继续飞；激光无效时吊舱不解算目标经纬度，
|
||||
故此兜底。无激光或激光恒无效的吊舱把 require_laser_valid 置 False。
|
||||
更进一步的"角度是否活体"由 LOS 一致性判据在 _build_snapshot 里叠加。
|
||||
|
||||
从未收到遥测 / 遥测超龄 / 搜索状态 / 激光无效 一律视为目标无效（宁可刹车）。"""
|
||||
tel = self._latest_telemetry
|
||||
@@ -293,17 +374,38 @@ class PodBridge:
|
||||
alpha_deg = 0.0
|
||||
|
||||
tracked = self._pod_locked(now_ms)
|
||||
# §2.3：require_vision 时需我方视觉作第二判据（bit2 粘滞兜底）。未接入视觉源
|
||||
# 时 fail-safe 为 False（无法确认 → 宁可刹车）。
|
||||
if tracked and hs.require_vision:
|
||||
tracked = self._vision_target_fn() if self._vision_target_fn else False
|
||||
# 第二判据：LOS 运动学一致性（独立于 bit2）。
|
||||
# 本吊舱无电机、相机刚性固连机体 → 机头转 Δψ 时真实目标的脱靶量必变 ≈−Δψ；
|
||||
# 机头已明显转动而 β 不动 → 角度冻结/幽灵目标 → 当丢目标处理（fail-safe）。
|
||||
# 仅在"吊舱自报锁定"期间喂样本；解锁即清历史，避免跨段污染。
|
||||
if tracked:
|
||||
status = self._los_monitor.update(
|
||||
now_ms, math.degrees(own.yaw_rad), beta_deg)
|
||||
self._los_consistent = status.consistent
|
||||
if status.frozen:
|
||||
tracked = False
|
||||
if self._los_logged_ok:
|
||||
self._los_logged_ok = False
|
||||
logger.warning(
|
||||
"[Bridge] 吊舱角度与机头转动不一致（Δψ=%.1f° Δβ=%.1f° "
|
||||
"残差=%.1f°，超 %.1f°）→ 判为丢目标",
|
||||
status.delta_yaw_deg, status.delta_beta_deg,
|
||||
status.residual_deg, hs.los_max_residual_deg)
|
||||
elif self._los_consistent and not self._los_logged_ok:
|
||||
self._los_logged_ok = True
|
||||
logger.info("[Bridge] 吊舱角度一致性恢复（残差 %.1f°）",
|
||||
status.residual_deg)
|
||||
else:
|
||||
self._los_monitor.reset()
|
||||
self._los_consistent = True
|
||||
self._los_logged_ok = True
|
||||
|
||||
return ControlSnapshot(
|
||||
track=TrackState(tracked=tracked),
|
||||
track_time_ms=t_ms, # 无遥测(t_ms=0) → 控制律判视觉超龄 → 降级
|
||||
los_beta_deg=beta_deg,
|
||||
los_alpha_deg=alpha_deg,
|
||||
distance_valid=False, # L1 无距离（L2 换 stereo_ranger.forward_distance_m）
|
||||
distance_valid=False, # L1 无距离（距离源接入后改为可用，见 prompt 文档）
|
||||
attitude_present=own.has_attitude,
|
||||
attitude_time_ms=own.time_ms,
|
||||
yaw_rad=own.yaw_rad,
|
||||
|
||||
@@ -67,7 +67,14 @@ class HeadingServoConfig:
|
||||
alpha_from_frame: bool = False # α 是否叠加框架角 frame_pitch_deg
|
||||
alpha_from_off_target: bool = True # α 是否叠加脱靶量 off_target_y_deg
|
||||
vertical_enabled: bool = True # 无人机 L1 开垂直（决策3）；false → α 强制 0
|
||||
require_vision: bool = False # true = 需我方视觉作第二判据（S3 起，§2.3）
|
||||
# --- 目标有效性第二判据：LOS 运动学一致性（2026-10-19 取代 require_vision）---
|
||||
# 背景：吊舱已取代双目+YOLO 成为唯一目标源，"我方视觉"不再存在；而 bit2
|
||||
# 丢失后不复位、脱靶量丢失表现为 0。故改用"机头转动 vs 脱靶量变化"的一致性
|
||||
# 作为独立于 bit2 的活体判据（详见 los_consistency.py）。
|
||||
los_consistency_enabled: bool = True
|
||||
los_window_ms: int = 3000 # 比较窗口（毫秒）
|
||||
los_min_yaw_change_deg: float = 20.0 # 窗口内机头至少转这么多才判定（激励门槛）
|
||||
los_max_residual_deg: float = 8.0 # |Δβ+Δψ| 上限，超过判不一致
|
||||
|
||||
|
||||
@dataclass
|
||||
@@ -283,8 +290,20 @@ def _load_heading_servo(root: dict) -> HeadingServoConfig:
|
||||
s, "heading_servo", "alpha_from_off_target", c.alpha_from_off_target)
|
||||
c.vertical_enabled = _get_bool(
|
||||
s, "heading_servo", "vertical_enabled", c.vertical_enabled)
|
||||
c.require_vision = _get_bool(
|
||||
s, "heading_servo", "require_vision", c.require_vision)
|
||||
c.los_consistency_enabled = _get_bool(
|
||||
s, "heading_servo", "los_consistency_enabled",
|
||||
c.los_consistency_enabled)
|
||||
c.los_window_ms = int(_positive(
|
||||
_get_integer(s, "heading_servo", "los_window_ms", c.los_window_ms),
|
||||
"heading_servo.los_window_ms"))
|
||||
c.los_min_yaw_change_deg = _in_range(
|
||||
_get_number(s, "heading_servo", "los_min_yaw_change_deg",
|
||||
c.los_min_yaw_change_deg),
|
||||
0.0, 180.0, "heading_servo.los_min_yaw_change_deg")
|
||||
c.los_max_residual_deg = _in_range(
|
||||
_get_number(s, "heading_servo", "los_max_residual_deg",
|
||||
c.los_max_residual_deg),
|
||||
0.0, 180.0, "heading_servo.los_max_residual_deg")
|
||||
return c
|
||||
|
||||
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -0,0 +1,115 @@
|
||||
"""los_consistency.py —— 吊舱角度「运动学一致性」校验（替代旧的 require_vision）。
|
||||
|
||||
为什么需要它
|
||||
------------
|
||||
吊舱协议有两处已知缺陷（部署文档 §10）：
|
||||
* bit2 锁定位在目标丢失后**不复位**；
|
||||
* 脱靶量没有无效标志位，**丢失时表现为 0**。
|
||||
只信 bit2 会让飞机朝"过期坐标/幽灵目标"继续飞。
|
||||
|
||||
原设计用「我方双目视觉」作第二判据（`require_vision`）。但后续决定**吊舱取代
|
||||
双目 + YOLO 成为唯一目标源**，于是"我方视觉"不再存在：该判据要么恒 False
|
||||
（链路永远进不了 TRACK），要么指向吊舱自身（自证，零信息量）。
|
||||
|
||||
本模块给出**不依赖任何额外传感器**的替代判据：
|
||||
本吊舱无电机、框架刚性固连机体 → 相机光轴随刚体转动。
|
||||
因此目标在世界系静止时，机头转 Δψ，脱靶量必须变化约 −Δψ。
|
||||
若机头已经明显转动（|Δψ| ≥ 阈值）而脱靶量几乎不动 → 角度冻结/幽灵，
|
||||
判为不一致（fail-safe：当丢目标处理）。
|
||||
|
||||
判别量：`residual = Δβ + Δψ`（β 为右正视线角，ψ 为 NED 航向，均取度）。
|
||||
* 一致：|residual| ≤ max_residual_deg（目标在世界系缓慢移动也会落在余量内）
|
||||
* 不一致：|residual| 超限且 |Δψ| 足够大（否则是"没有激励"，不做判断）
|
||||
|
||||
局限（必须在文档里写清）：它只能证明「角度是活的」，**不能证明「是那个目标」**。
|
||||
若吊舱锁错了物体但角度正常变化，本判据无法识别 —— 那属于识别问题，需另接
|
||||
独立传感器（双目/其他），不是本模块能覆盖的。
|
||||
"""
|
||||
|
||||
from collections import deque
|
||||
from dataclasses import dataclass
|
||||
import math
|
||||
|
||||
|
||||
def wrap_deg_180(deg: float) -> float:
|
||||
"""角度回绕到 [-180, 180)。"""
|
||||
d = math.fmod(deg + 180.0, 360.0)
|
||||
if d < 0.0:
|
||||
d += 360.0
|
||||
return d - 180.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class LosConsistencyConfig:
|
||||
"""一致性判据参数（全外置）。"""
|
||||
|
||||
enabled: bool = True # False → 关掉判据（退回"只信 bit2"）
|
||||
window_ms: int = 3000 # 比较窗口：用"当前"与"约 window 前"的样本比对
|
||||
min_yaw_change_deg: float = 20.0 # 窗口内机头至少转这么多才做判断（激励门槛）
|
||||
max_residual_deg: float = 8.0 # |Δβ+Δψ| 超过即判不一致
|
||||
history_factor: float = 2.0 # 历史缓冲 = window × factor
|
||||
|
||||
|
||||
@dataclass
|
||||
class LosStatus:
|
||||
"""单次判定结果（供门控与日志）。"""
|
||||
|
||||
consistent: bool = True # 是否一致（默认一致 = 不干扰正常跟踪）
|
||||
excited: bool = False # 本窗口是否达到"激励门槛"（转过足够角度）
|
||||
residual_deg: float = 0.0 # Δβ + Δψ
|
||||
delta_yaw_deg: float = 0.0
|
||||
delta_beta_deg: float = 0.0
|
||||
|
||||
@property
|
||||
def frozen(self) -> bool:
|
||||
"""判为"角度冻结/幽灵"：有激励但不一致。"""
|
||||
return self.excited and not self.consistent
|
||||
|
||||
|
||||
class LosConsistencyMonitor:
|
||||
"""按窗口比较 Δβ 与 Δψ，判断吊舱角度是否与机体转动一致。
|
||||
|
||||
用法：每个控制周期喂一次（仅在吊舱报告锁定期间喂），
|
||||
`update(now_ms, yaw_deg, beta_deg)` 返回 `LosStatus`。
|
||||
吊舱解锁时应调用 `reset()`，避免上一次跟踪的历史污染下一次。
|
||||
"""
|
||||
|
||||
def __init__(self, cfg: LosConsistencyConfig | None = None) -> None:
|
||||
self._cfg = cfg or LosConsistencyConfig()
|
||||
self._samples: deque = deque()
|
||||
|
||||
def reset(self) -> None:
|
||||
self._samples.clear()
|
||||
|
||||
def update(self, now_ms: int, yaw_deg: float, beta_deg: float) -> LosStatus:
|
||||
cfg = self._cfg
|
||||
if not cfg.enabled:
|
||||
return LosStatus(consistent=True, excited=False)
|
||||
|
||||
self._samples.append((now_ms, float(yaw_deg), float(beta_deg)))
|
||||
keep_ms = cfg.window_ms * max(1.0, cfg.history_factor)
|
||||
while self._samples and now_ms - self._samples[0][0] > keep_ms:
|
||||
self._samples.popleft()
|
||||
|
||||
# 参考样本：窗口内**最老**的那个
|
||||
ref = None
|
||||
for sample in self._samples:
|
||||
if now_ms - sample[0] <= cfg.window_ms:
|
||||
ref = sample
|
||||
break
|
||||
if ref is None:
|
||||
return LosStatus(consistent=True, excited=False)
|
||||
|
||||
span_ms = now_ms - ref[0]
|
||||
# 历史不足一个窗口的 3/4 时，Δψ 天然偏小，不做判断（避免误判丢目标）
|
||||
if span_ms < cfg.window_ms * 0.75:
|
||||
return LosStatus(consistent=True, excited=False)
|
||||
|
||||
delta_yaw = wrap_deg_180(yaw_deg - ref[1])
|
||||
delta_beta = beta_deg - ref[2]
|
||||
residual = delta_beta + delta_yaw
|
||||
excited = abs(delta_yaw) >= cfg.min_yaw_change_deg
|
||||
consistent = (not excited) or (abs(residual) <= cfg.max_residual_deg)
|
||||
return LosStatus(consistent=consistent, excited=excited,
|
||||
residual_deg=residual, delta_yaw_deg=delta_yaw,
|
||||
delta_beta_deg=delta_beta)
|
||||
@@ -3,6 +3,8 @@
|
||||
职责（桥接的飞控侧）：
|
||||
* 订阅 PX4 遥测（position / attitude_euler / gps_info / velocity_ned），缓存为
|
||||
OwnState 快照，供桥接层构造 表3(载机姿态下发) 与 GuidanceInput(引导输入)；
|
||||
* 订阅飞控状态文本 status_text()（STATUSTEXT），缓存供任务日志/地面站显示
|
||||
（飞控警告/异常信息）；
|
||||
* 复用 visual_track_control.MavsdkCommandGateway 发送 offboard 速度设定点
|
||||
（不重造三分支映射，直接喂 GuidanceLaw 的 ControlOutput）。
|
||||
|
||||
@@ -15,6 +17,7 @@ import asyncio
|
||||
import logging
|
||||
import math
|
||||
import time
|
||||
from collections import deque
|
||||
from dataclasses import dataclass, replace
|
||||
from typing import Any, Callable, Optional
|
||||
|
||||
@@ -23,6 +26,7 @@ from visual_track_control.mavsdk_gateway import MavsdkCommandGateway
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
_GPS_FIX_3D = 3 # mavsdk GpsFixType.THREE_D_FIX 起视为定位有效
|
||||
_FC_MSG_BUFFER = 64 # 飞控状态文本缓存上限（低频流，仅供日志消费）
|
||||
|
||||
|
||||
def _enum_int(value: Any) -> int:
|
||||
@@ -69,6 +73,7 @@ class Px4Gateway:
|
||||
self._now_ms = now_ms_fn or (lambda: int(time.monotonic() * 1000))
|
||||
self._state = OwnState()
|
||||
self._tasks: list = []
|
||||
self._fc_msgs: deque = deque(maxlen=_FC_MSG_BUFFER)
|
||||
|
||||
# --- 连接（实机）-------------------------------------------------------
|
||||
@classmethod
|
||||
@@ -123,13 +128,25 @@ class Px4Gateway:
|
||||
"""返回当前缓存的快照副本（调用方修改不影响内部状态）。"""
|
||||
return replace(self._state)
|
||||
|
||||
def update_fc_status(self, st: Any) -> None:
|
||||
"""缓存一条飞控状态文本（STATUSTEXT）；severity 取枚举名（如 WARNING）。"""
|
||||
sev = getattr(st, "type", None)
|
||||
sev_name = getattr(sev, "name", None) or str(sev or "UNKNOWN")
|
||||
self._fc_msgs.append((sev_name, str(getattr(st, "text", ""))))
|
||||
|
||||
def take_fc_messages(self) -> list:
|
||||
"""取走并清空飞控状态文本（供任务日志落盘；无则返回 []）。"""
|
||||
msgs = list(self._fc_msgs)
|
||||
self._fc_msgs.clear()
|
||||
return msgs
|
||||
|
||||
def _recompute_valid(self) -> None:
|
||||
self._state.position_valid = (self._state.has_position
|
||||
and self._state.fix_type >= _GPS_FIX_3D)
|
||||
|
||||
# --- 遥测订阅（异步胶水）----------------------------------------------
|
||||
async def start_telemetry(self) -> None:
|
||||
"""订阅四路遥测流，后台任务持续刷新缓存。幂等：已订阅则直接返回。"""
|
||||
"""订阅遥测流 + 飞控状态文本，后台任务持续刷新缓存。幂等：已订阅则直接返回。"""
|
||||
if self._tasks:
|
||||
return # 已订阅，避免重复起任务（runner 会先订阅，bridge.start 再触发时跳过）
|
||||
t = self._drone.telemetry
|
||||
@@ -140,6 +157,8 @@ class Px4Gateway:
|
||||
asyncio.ensure_future(self._consume(t.gps_info(), self.update_gps)),
|
||||
asyncio.ensure_future(
|
||||
self._consume(t.velocity_ned(), self.update_velocity_ned)),
|
||||
asyncio.ensure_future(
|
||||
self._consume(t.status_text(), self.update_fc_status)),
|
||||
]
|
||||
logger.info("[Px4Gateway] 遥测订阅已启动")
|
||||
|
||||
@@ -171,5 +190,9 @@ class Px4Gateway:
|
||||
async def send_brake_hover(self, time_boot_ms: Optional[int] = None) -> bool:
|
||||
return await self._cmd.send_brake_hover(time_boot_ms)
|
||||
|
||||
async def send_yaw_rate(self, yaw_rate_dps: float) -> bool:
|
||||
"""原地偏航扫视（SEARCH 态）：零速 + 指定角速度，保持 offboard 心跳。"""
|
||||
return await self._cmd.send_yaw_rate(yaw_rate_dps)
|
||||
|
||||
async def stop_offboard(self) -> None:
|
||||
await self._cmd.stop()
|
||||
|
||||
Reference in New Issue
Block a user