精简仓库结构并增强任务链路:归档双目可选后端、新增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:
@@ -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)
|
||||
Reference in New Issue
Block a user