keita
df91b1110e
归档 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。
116 lines
4.9 KiB
Python
116 lines
4.9 KiB
Python
"""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)
|