keita
68ba92ff5b
清屏刷新显示锁定/激光/脱靶量/框架角/目标经纬度与实时帧率, Ctrl+C 退出并 输出本次运行汇总(锁定次数、每次起止与时长、累计占比、激光变化); --detect 可先开目标识别, --duration 可定时结束(默认无限), 非TTY输出保留事件流水
221 lines
8.7 KiB
Python
221 lines
8.7 KiB
Python
"""pod_console.py —— 吊舱目标识别实时控制台(台架工具,非飞行组件)。
|
|
|
|
持续刷新显示吊舱当前状态(锁定/激光/脱靶量/框架角/目标经纬度与帧率),
|
|
Ctrl+C 退出;退出时打印本次运行的锁定事件汇总(含每次锁定的起止与时长)。
|
|
|
|
用法:
|
|
python utils/pod_bridge/pod_console.py /dev/ttyAMA0
|
|
python utils/pod_bridge/pod_console.py /dev/ttyAMA0 --detect drone --refresh 0.2
|
|
python utils/pod_bridge/pod_console.py /dev/ttyAMA0 --duration 60 # 定时自动结束
|
|
|
|
说明:
|
|
* 默认 --duration 0 表示一直运行直到 Ctrl+C;
|
|
* 终端(TTY)下清屏刷新,输出重定向到文件时改为逐次追加,便于留存日志;
|
|
* 锁定事件取自 表2-2 状态字节1 的 bit2(规格 §2.2.2)。
|
|
"""
|
|
|
|
import argparse
|
|
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
|
|
|
|
|
|
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_or_None, laser_start, laser_last)]
|
|
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
|
|
|
|
@property
|
|
def open_duration(self) -> float:
|
|
return 0.0 if self._open is None else 0.0
|
|
|
|
|
|
def _render(status_lines: list, tty: bool) -> None:
|
|
if tty:
|
|
sys.stdout.write("\x1b[2J\x1b[H")
|
|
sys.stdout.write("\n".join(status_lines) + "\n")
|
|
sys.stdout.flush()
|
|
|
|
|
|
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.2, help="刷新周期(秒)")
|
|
ap.add_argument("--duration", type=float, default=0.0,
|
|
help="运行时长(秒);0 表示一直跑到 Ctrl+C")
|
|
ap.add_argument("--detect", choices=sorted(_DETECT_PARAM_X),
|
|
help="启动时下发 目标识别开(0x07)")
|
|
args = ap.parse_args()
|
|
|
|
pod = PodSerial(args.port, args.baud)
|
|
if args.detect:
|
|
px = _DETECT_PARAM_X[args.detect]
|
|
pod.send(encode_control(ControlCommand(
|
|
code=ControlCode.TARGET_DETECT_ON, param_x=px)))
|
|
|
|
tty = sys.stdout.isatty()
|
|
t0 = time.monotonic()
|
|
last_render = 0.0
|
|
last_tele_stamp = t0
|
|
rate_hz = 0.0
|
|
n_tele = n_tgt = 0
|
|
latest_tgt = None
|
|
latest = None # 最近一帧遥测
|
|
tracker = LockTracker()
|
|
recent = [] # 最近事件行(屏幕滚动区)
|
|
|
|
print(f"[pod-console] {args.port} @ {args.baud} 8N1,运行中 ... Ctrl+C 退出")
|
|
if args.detect:
|
|
print(f"[pod-console] 已下发 目标识别开(0x07,参数X={_DETECT_PARAM_X[args.detect]})")
|
|
|
|
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:
|
|
print(f"\n[pod-console] 链路中断: {exc}")
|
|
print("[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_stamp
|
|
last_tele_stamp = 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}"
|
|
recent.append(line)
|
|
if not tty: # 重定向到文件时保留事件流水
|
|
print(f"[事件] {line}")
|
|
elif isinstance(frame, TargetGeo):
|
|
n_tgt += 1
|
|
latest_tgt = (frame, now_s)
|
|
|
|
if now_s - last_render >= args.refresh:
|
|
last_render = now_s
|
|
lock_txt = "否"
|
|
if tracker.open_start is not None:
|
|
lock_txt = f"是(已锁定 {now_s - tracker.open_start:.1f}s)"
|
|
laser = "-" if latest is None or latest.laser_range_m is None \
|
|
else f"{latest.laser_range_m:.1f} m"
|
|
off = "-, -" if latest is None else \
|
|
f"{latest.off_target_x_deg:+.2f}°, {latest.off_target_y_deg:+.2f}°"
|
|
frame_ang = "-" if latest is None else \
|
|
(f"{latest.frame_roll_deg:+.2f}, {latest.frame_pitch_deg:+.2f}, "
|
|
f"{latest.frame_yaw_deg:+.2f}")
|
|
tgt_txt = "无"
|
|
if latest_tgt is not None:
|
|
g, ts = latest_tgt
|
|
tgt_txt = (f"lat={g.latitude_deg:.7f} lon={g.longitude_deg:.7f} "
|
|
f"({ts:.0f}s)")
|
|
lines = [
|
|
f"[pod-console] {args.port} | 运行 {now_s:.1f}s | "
|
|
f"遥测 {n_tele} 帧 ({rate_hz:.1f}Hz,标称 {_TELEMETRY_NOMINAL_HZ:.0f}) "
|
|
f"| 目标帧 {n_tgt}",
|
|
f" 锁定: {lock_txt}",
|
|
f" 激光: {laser}",
|
|
f" 脱靶量(X,Y): {off} 框架角(R,P,Y): {frame_ang}",
|
|
f" 最近目标: {tgt_txt}",
|
|
f" 锁定次数: {len(tracker.events)}"
|
|
+ (f"(进行中第 {len(tracker.events) + 1} 次)"
|
|
if tracker.open_start is not None else ""),
|
|
" " + (" | ".join(recent[-6:]) if recent else "尚无锁定事件"),
|
|
" Ctrl+C 退出并输出汇总",
|
|
]
|
|
_render(lines, tty)
|
|
time.sleep(0.002)
|
|
|
|
except KeyboardInterrupt:
|
|
pass
|
|
finally:
|
|
now_s = time.monotonic() - t0
|
|
tracker.finish(now_s)
|
|
try:
|
|
pod.close()
|
|
except OSError:
|
|
pass
|
|
|
|
# ---- 退出汇总 ----
|
|
total = sum((e[1] - e[0]) for e in tracker.events)
|
|
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(" 未出现锁定:确认视场内有目标;必要时补发 AUTO_LOCK_ON_FOUND(0x0F) "
|
|
"或 DETECT_TO_TRACK(0x10)")
|
|
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})")
|
|
pct = total / now_s * 100 if now_s > 0 else 0.0
|
|
longest = max(tracker.events, key=lambda x: x[1] - x[0])
|
|
print(f"累计锁定 {total:.1f}s(占比 {pct:.1f}%) | "
|
|
f"最长一次 {longest[1] - longest[0]:.1f}s")
|
|
return 0
|
|
|
|
|
|
if __name__ == "__main__":
|
|
sys.exit(main())
|