精简仓库结构并增强任务链路:归档双目可选后端、新增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
2026-10-21 14:48:41 +08:00
parent 53fdd3b13f
commit df91b1110e
159 changed files with 6121 additions and 1617 deletions
+115
View File
@@ -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)