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。
56 lines
2.6 KiB
Python
56 lines
2.6 KiB
Python
"""pod_control.py —— 吊舱目标识别/锁定控制（真机，任务编排的一部分）。
|
||
|
||
真机吊舱不会自动输出"锁定"——需要先发控制指令让它进入目标识别 + 自动锁定模式：
|
||
* 0x07 目标识别开（param_x = 目标类型：0=人/车，1=无人机）
|
||
* 0x0F 发现目标自动锁定（常驻模式：目标进画面自动锁定，非一次性动作）
|
||
结束（任务完成/返航）时：
|
||
* 0x0E 停止跟踪（只停当前这一次跟踪）
|
||
* 0x08 目标识别关（真正退出识别）
|
||
|
||
本模块把这些指令序列封装成 PodController，供 TaskRunner 在 GOTO / RTL 阶段调用。
|
||
另含地面站手动命令（stop_track / auto_lock / center），与 pod_console.py 按键
|
||
u / a / c 语义对齐。SITL 用 VirtualPod（直接合成锁定态），不注入 PodController。
|
||
"""
|
||
|
||
from pod_bridge.protocol import ControlCode, ControlCommand
|
||
|
||
|
||
class PodController:
|
||
"""吊舱目标识别/锁定控制器。
|
||
|
||
arm_detection / stop_tracking 为同步方法，经 bridge.send_control 下发 表2-1
|
||
控制帧（编码由 PodBridge 完成，本类只关心"发什么指令序列"）。
|
||
"""
|
||
|
||
def __init__(self, bridge, target_type: int = 0) -> None:
|
||
"""bridge：PodBridge（需有 send_control）；target_type：0=人/车，1=无人机。"""
|
||
self._bridge = bridge
|
||
self._target_type = int(target_type)
|
||
|
||
def arm_detection(self) -> None:
|
||
"""开启目标识别 + 发现自动锁定（GOTO 阶段进入时调用）。"""
|
||
self._bridge.send_control(ControlCommand(
|
||
code=ControlCode.TARGET_DETECT_ON, param_x=self._target_type))
|
||
self._bridge.send_control(ControlCommand(
|
||
code=ControlCode.AUTO_LOCK_ON_FOUND))
|
||
|
||
def stop_tracking(self) -> None:
|
||
"""停止跟踪 + 关闭目标识别（RTL 前调用）。"""
|
||
self._bridge.send_control(ControlCommand(code=ControlCode.STOP_TRACK))
|
||
self._bridge.send_control(ControlCommand(code=ControlCode.TARGET_DETECT_OFF))
|
||
|
||
def stop_track(self) -> None:
|
||
"""停止跟踪（只停当前这一次，0x0E；0x0F 常驻自动锁定仍生效）。"""
|
||
self._bridge.send_control(ControlCommand(code=ControlCode.STOP_TRACK))
|
||
|
||
def auto_lock(self) -> None:
|
||
"""发现目标自动锁定 + 识别转跟踪（0x0F + 0x10）。"""
|
||
self._bridge.send_control(ControlCommand(
|
||
code=ControlCode.AUTO_LOCK_ON_FOUND))
|
||
self._bridge.send_control(ControlCommand(
|
||
code=ControlCode.DETECT_TO_TRACK))
|
||
|
||
def center(self) -> None:
|
||
"""手动归中（0x2B）。"""
|
||
self._bridge.send_control(ControlCommand(code=ControlCode.CENTER))
|