"""pod_console.py —— 吊舱目标识别实时控制台(台架工具,非飞行组件)。 持续显示吊舱当前状态(锁定/脱靶量/框架角/目标经纬度与帧率),终端下**原地刷新** (光标上移重绘,不刷屏);退出时打印本次运行的锁定事件汇总。 用法: python utils/pod_bridge/pod_console.py /dev/ttyAMA0 python utils/pod_bridge/pod_console.py /dev/ttyAMA0 --detect drone --auto-lock python utils/pod_bridge/pod_console.py /dev/ttyAMA0 --auto-unlock 3 # 目标丢失自动解锁 python utils/pod_bridge/pod_console.py /dev/ttyAMA0 --duration 60 # 定时自动结束 交互按键(输入后回车): u = 停止跟踪(0x0E):解除当前锁定;0x0F 自动锁定模式**仍生效**,目标重新进 入画面会自动锁定,无需重发指令 s = 停止跟踪 + 关识别(0x0E + 0x08):彻底退出识别,需按 d / a 重新开启 d = 开识别(0x07,类型取 --detect,默认 person) a = 自动锁定序列(0x0F + 0x10) c = 归中(0x2B) q = 退出并输出汇总 说明: * 本吊舱锁定位置位(表2-2 status1 bit2)在目标丢失后**不复位**(实测), 必须用 0x0E 停止跟踪主动解除;可用 --auto-unlock 让脚本在表4 停止更新 N 秒后自动发 0x0E; * 注意:0x0F「发现目标自动锁定」是**常驻模式**,0x0E 只停止当前这一次跟踪, 不会退出自动锁定——目标重新进入画面会自动重新锁定(实测结论); * --duration 0 表示一直运行到 Ctrl+C / 输入 q; * 输出重定向到文件时改为每周期一行紧凑记录,便于留存日志; * 锁定状态取自 表2-2 状态字节1 的 bit2(规格 §2.2.2)。 """ import argparse import select import sys import time from pathlib import Path sys.path.insert(0, str(Path(__file__).resolve().parents[1])) # utils/ 入 sys.path from pod_bridge.pod_serial import PodSerial # noqa: E402 from pod_bridge.protocol import (ControlCode, ControlCommand, # noqa: E402 TargetGeo, Telemetry, encode_control) # 表2-1 0x07 目标识别开:参数X 0=检测人或车,1=检测无人机(规格 §2.2.1) _DETECT_PARAM_X = {"person": 0, "drone": 1} _TELEMETRY_NOMINAL_HZ = 60.0 _KEY_HINT = ("按键: u=停止跟踪(自动锁定仍生效) s=停止+关识别 d=开识别 " "a=自动锁定 c=归中 q=退出") def _fmt_hhmmss(seconds: float) -> str: m, s = divmod(int(seconds), 60) return f"{m:02d}:{s:02d}" class LockTracker: """记录锁定区间(闭合区间在 close() 时补终值)。""" def __init__(self): self.events = [] # [(start_s, end_s, laser0, laser1, n_frames)] self._open = None def update(self, locked: bool, now_s: float, laser) -> bool: """返回 True 表示本拍发生状态跳变。""" if locked and self._open is None: self._open = {"start": now_s, "laser0": laser, "laser1": laser, "n": 0} return True if locked and self._open is not None: self._open["n"] += 1 if laser is not None: self._open["laser1"] = laser return False if not locked and self._open is not None: self._close(now_s) return True return False def _close(self, now_s: float) -> None: e = self._open self.events.append((e["start"], now_s, e["laser0"], e["laser1"], e["n"])) self._open = None def finish(self, now_s: float) -> None: if self._open is not None: self._close(now_s) @property def open_start(self): return self._open["start"] if self._open else None class Renderer: """TTY 下原地重绘(光标上移 + 清行),非 TTY 下每周期一行紧凑记录。""" def __init__(self, tty: bool): self.tty = tty self._prev_lines = 0 def draw(self, lines: list) -> None: if not self.tty: sys.stdout.write(" | ".join(lines) + "\n") sys.stdout.flush() return if self._prev_lines: sys.stdout.write(f"\x1b[{self._prev_lines}A") # 光标回到本块首行 for ln in lines: sys.stdout.write("\x1b[2K" + ln + "\n") # 清行后重写 self._prev_lines = len(lines) sys.stdout.flush() def newline(self, text: str) -> None: """在面板下方追加一行(事件行),不破坏原地重绘。""" sys.stdout.write("\x1b[2K" + text + "\n") sys.stdout.flush() def _poll_key() -> str: """非阻塞读取一行按键命令(无输入返回空串)。""" try: if select.select([sys.stdin], [], [], 0)[0]: return sys.stdin.readline().strip().lower() except Exception: return "" return "" def main() -> int: ap = argparse.ArgumentParser(description="吊舱目标识别实时控制台") ap.add_argument("port", nargs="?", default="/dev/ttyAMA0", help="串口设备号") ap.add_argument("--baud", type=int, default=115200) ap.add_argument("--refresh", type=float, default=0.5, help="刷新周期(秒)") ap.add_argument("--duration", type=float, default=0.0, help="运行时长(秒);0 表示一直跑到 Ctrl+C / 输入 q") ap.add_argument("--detect", choices=sorted(_DETECT_PARAM_X), help="启动时下发 目标识别开(0x07)") ap.add_argument("--auto-lock", action="store_true", help="在识别开之后追加 发现目标自动锁定(0x0F)与 识别转跟踪(0x10)") ap.add_argument("--motor-on", action="store_true", help="启动时先下发 开电机(0x27)") ap.add_argument("--auto-unlock", type=float, default=0.0, metavar="SEC", help="锁定中若表4 超过 SEC 秒未更新,自动发 0x0E 解除锁定。" "⚠ 触发判据依赖表4;本设备无激光测距,表4 可能根本不发, " "启用前须实测确认(见部署文档 §10)") ap.add_argument("--unlock-on-exit", action="store_true", help="退出前发一帧 0x0E 解除锁定") args = ap.parse_args() pod = PodSerial(args.port, args.baud) tty = sys.stdout.isatty() render = Renderer(tty) detect_x = _DETECT_PARAM_X.get(args.detect, 0) def _send(code, **params): pod.send(encode_control(ControlCommand(code=code, **params))) time.sleep(0.2) note = f"0x{int(code):02X}" + (f" {params}" if params else "") if tty: render.newline(f"[send] {note}") else: print(f"[send] {note}") return note if args.motor_on: _send(ControlCode.MOTOR_ON) if args.detect: _send(ControlCode.TARGET_DETECT_ON, param_x=detect_x) if args.auto_lock: _send(ControlCode.AUTO_LOCK_ON_FOUND) _send(ControlCode.DETECT_TO_TRACK) elif args.detect: if tty: render.newline("[提示] 只开了识别(0x07)可能仍不锁定,可按 a 追加 0x0F/0x10") else: print("[提示] 只开了识别(0x07)可能仍不锁定,可加 --auto-lock") t0 = time.monotonic() last_render = -1.0 n_tele = n_tgt = 0 rate_hz = 0.0 last_tele_at = t0 last_tgt_s = None # 最近一次表4 到达时刻(秒) last_unlock_s = -1e9 # 上次自动解锁时刻 latest = None latest_tgt = None tracker = LockTracker() events = [] if tty: render.newline(f"[pod-console] {args.port} @ {args.baud} | {_KEY_HINT}") else: print(f"[pod-console] {args.port} @ {args.baud} 8N1 启动") try: while True: now_s = time.monotonic() - t0 if args.duration > 0 and now_s >= args.duration: break try: frames = pod.poll() except OSError as exc: render.newline(f"[pod-console] 链路中断: {exc}") render.newline("[pod-console] 排查:串口控制台占用(serial-getty)/" "mini-UART 不稳 → 换 PL011 或 USB 转串口") return 1 for frame in frames: if isinstance(frame, Telemetry): n_tele += 1 latest = frame dt = time.monotonic() - last_tele_at last_tele_at = time.monotonic() if dt > 0: rate_hz = 1.0 / dt if tracker.update(frame.is_target_locked, now_s, frame.laser_range_m): tag = "锁定" if frame.is_target_locked else "解锁" line = f"{_fmt_hhmmss(now_s)} {tag}" events.append(line) if not tty: print(f"[事件] {line}") elif isinstance(frame, TargetGeo): n_tgt += 1 last_tgt_s = now_s latest_tgt = frame # ---- 自动解除锁定:锁定中但表4 长时间不更新 ---- # ⚠ 本设备无激光测距,表4(目标经纬度)可能始终不发 → last_tgt_s 恒为 # None,此判据会一锁定就误解锁。启用前先用控制台观察「目标帧 距今」, # 确认锁定后表4 真的在发。触发判据的替代方案见部署文档 §10。 if (args.auto_unlock > 0 and latest is not None and latest.is_target_locked and (last_tgt_s is None or now_s - last_tgt_s > args.auto_unlock) and now_s - last_unlock_s > args.auto_unlock): last_unlock_s = now_s _send(ControlCode.STOP_TRACK) events.append(f"{_fmt_hhmmss(now_s)} 自动解锁(表4 超时 " f"{args.auto_unlock:.0f}s)") if not tty: print(f"[事件] 自动解锁(0x0E)") # ---- 按键 ---- key = _poll_key() if key: if key.startswith("q"): break elif key.startswith("u"): _send(ControlCode.STOP_TRACK) elif key.startswith("s"): _send(ControlCode.STOP_TRACK) _send(ControlCode.TARGET_DETECT_OFF) elif key.startswith("d"): _send(ControlCode.TARGET_DETECT_ON, param_x=detect_x) elif key.startswith("a"): _send(ControlCode.AUTO_LOCK_ON_FOUND) _send(ControlCode.DETECT_TO_TRACK) elif key.startswith("c"): _send(ControlCode.CENTER) if now_s - last_render >= args.refresh: last_render = now_s if latest is None: lock_txt, laser, off, ang = "尚无遥测", "-", "-, -", "-" st = "状态字: 尚无遥测" else: lock_txt = ("否" if tracker.open_start is None else f"是(已锁定 {now_s - tracker.open_start:.1f}s)") laser = ("-" if latest.laser_range_m is None else f"{latest.laser_range_m:.1f} m") off = (f"{latest.off_target_x_deg:+.2f}°, " f"{latest.off_target_y_deg:+.2f}°") ang = (f"{latest.frame_roll_deg:+.2f}, " f"{latest.frame_pitch_deg:+.2f}, {latest.frame_yaw_deg:+.2f}") st = (f"状态字 s1=0x{latest.status1:02X} s2=0x{latest.status2:02X} " f"s3=0x{latest.status3:02X} | " f"视频源={latest.track_source} " f"电机={'开' if latest.motor_on else '关'} " f"自检=0x{latest.self_check:02X}") tgt_txt = "无" if latest_tgt is not None: tgt_txt = (f"lat={latest_tgt.latitude_deg:.7f} " f"lon={latest_tgt.longitude_deg:.7f}") tgt_age = ("-" if last_tgt_s is None else f"{now_s - last_tgt_s:.1f}s") lines = [ f"[pod-console] {args.port} | 运行 {now_s:.1f}s | " f"遥测 {n_tele} 帧 ({rate_hz:.1f}Hz) | 目标帧 {n_tgt} " f"(距今 {tgt_age})", f" 锁定: {lock_txt} 激光: {laser}", f" 脱靶量(X,Y): {off} 框架角(R,P,Y): {ang}", f" {st}", f" 最近目标: {tgt_txt}", f" 锁定次数: {len(tracker.events)}" + (f"(进行中第 {len(tracker.events) + 1} 次)" if tracker.open_start is not None else ""), " " + (" | ".join(events[-5:]) if events else "暂无事件"), f" {_KEY_HINT}", ] render.draw(lines) time.sleep(0.002) except KeyboardInterrupt: pass finally: now_s = time.monotonic() - t0 if args.unlock_on_exit: try: _send(ControlCode.STOP_TRACK) except OSError: pass tracker.finish(now_s) try: pod.close() except OSError: pass # ---- 退出汇总 ---- print("\n==== 本次运行汇总 ====") print(f"时长 {now_s:.1f}s | 遥测 {n_tele} 帧 " f"({n_tele / now_s:.1f}Hz 均值) | 目标帧 {n_tgt}") print(f"锁定事件 {len(tracker.events)} 次") if not tracker.events: print(" 未出现锁定:确认视场内有目标并按 d/a 开识别+自动锁定;" "仍不行看面板状态字的电机/自检") else: for i, (s, e, l0, l1, n) in enumerate(tracker.events, 1): lz = "-" if l0 is not None or l1 is not None: a = "-" if l0 is None else f"{l0:.1f}" b = "-" if l1 is None else f"{l1:.1f}" lz = f"{a}→{b} m" print(f" #{i} {_fmt_hhmmss(s)} → {_fmt_hhmmss(e)} " f"({e - s:.1f}s, {n} 帧, 激光 {lz})") total = sum((e[1] - e[0]) for e in tracker.events) longest = max(tracker.events, key=lambda x: x[1] - x[0]) print(f"累计锁定 {total:.1f}s(占比 {total / now_s * 100:.1f}%) | " f"最长一次 {longest[1] - longest[0]:.1f}s") if events: print("事件流水:") for line in events: print(f" {line}") return 0 if __name__ == "__main__": sys.exit(main())