pod_console: 支持主动解除锁定 + 控制台不再刷屏
解除锁定: 按键u发0x0E停止跟踪, s=0x0E+0x08关识别, d开识别, a自动锁定, c归中, q退出; --auto-unlock SEC 在锁定中表4超时SEC秒自动发0x0E(针对本吊舱锁定位置位 丢失后不复位); --unlock-on-exit 退出前解锁。 显示: 新增Renderer原地重绘(光标上移+清行, 不再清屏刷屏), 刷新默认0.5s, 事件行追加在面板下方; 非TTY输出改为每周期一行紧凑记录。 打桩验证: 自动解锁与退出解锁均正确发出0x0E
This commit is contained in:
keita
+161
-79
@@ -1,25 +1,33 @@
|
||||
"""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 --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 # 定时自动结束
|
||||
|
||||
锁定不起来时的排查:先看面板「状态字」行(电机/自检/视频源),再试 --auto-lock
|
||||
(识别开 0x07 + 发现目标自动锁定 0x0F + 识别转跟踪 0x10),必要时 --motor-on
|
||||
先开电机(0x27)。协议上「识别」与「跟踪」是两个阶段,只开识别常不锁定。
|
||||
交互按键(输入后回车):
|
||||
u = 解除锁定(0x0E 停止跟踪)
|
||||
s = 停止跟踪 + 关识别(0x0E + 0x08)
|
||||
d = 开识别(0x07,类型取 --detect,默认 person)
|
||||
a = 自动锁定序列(0x0F + 0x10)
|
||||
c = 归中(0x2B)
|
||||
q = 退出并输出汇总
|
||||
|
||||
说明:
|
||||
* 默认 --duration 0 表示一直运行直到 Ctrl+C;
|
||||
* 终端(TTY)下清屏刷新,输出重定向到文件时改为逐次追加,便于留存日志;
|
||||
* 锁定事件取自 表2-2 状态字节1 的 bit2(规格 §2.2.2)。
|
||||
* 本吊舱锁定位置位(表2-2 status1 bit2)在目标丢失后**不复位**(实测),
|
||||
必须用 0x0E 停止跟踪主动解除;可用 --auto-unlock 让脚本在表4 停止更新
|
||||
N 秒后自动发 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
|
||||
@@ -33,6 +41,7 @@ from pod_bridge.protocol import (ControlCode, ControlCommand, # noqa: E402
|
||||
# 表2-1 0x07 目标识别开:参数X 0=检测人或车,1=检测无人机(规格 §2.2.1)
|
||||
_DETECT_PARAM_X = {"person": 0, "drone": 1}
|
||||
_TELEMETRY_NOMINAL_HZ = 60.0
|
||||
_KEY_HINT = "按键: u=解锁(0x0E) s=停止+关识别 d=开识别 a=自动锁定 c=归中 q=退出"
|
||||
|
||||
|
||||
def _fmt_hhmmss(seconds: float) -> str:
|
||||
@@ -44,7 +53,7 @@ class LockTracker:
|
||||
"""记录锁定区间(闭合区间在 close() 时补终值)。"""
|
||||
|
||||
def __init__(self):
|
||||
self.events = [] # [(start_s, end_s_or_None, laser_start, laser_last)]
|
||||
self.events = [] # [(start_s, end_s, laser0, laser1, n_frames)]
|
||||
self._open = None
|
||||
|
||||
def update(self, locked: bool, now_s: float, laser) -> bool:
|
||||
@@ -75,65 +84,104 @@ class LockTracker:
|
||||
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
|
||||
|
||||
class Renderer:
|
||||
"""TTY 下原地重绘(光标上移 + 清行),非 TTY 下每周期一行紧凑记录。"""
|
||||
|
||||
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")
|
||||
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.2, help="刷新周期(秒)")
|
||||
ap.add_argument("--refresh", type=float, default=0.5, help="刷新周期(秒)")
|
||||
ap.add_argument("--duration", type=float, default=0.0,
|
||||
help="运行时长(秒);0 表示一直跑到 Ctrl+C")
|
||||
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("--motor-on", action="store_true", help="启动时先下发 开电机(0x27)")
|
||||
ap.add_argument("--auto-unlock", type=float, default=0.0, metavar="SEC",
|
||||
help="锁定中若表4 超过 SEC 秒未更新,自动发 0x0E 解除锁定")
|
||||
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) # 吊舱处理指令需要时间,连续下发间隔 200ms
|
||||
print(f"[send] 0x{int(code):02X} {params or ''}")
|
||||
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) # 0x27 开电机
|
||||
_send(ControlCode.MOTOR_ON)
|
||||
if args.detect:
|
||||
_send(ControlCode.TARGET_DETECT_ON, # 0x07 目标识别开
|
||||
param_x=_DETECT_PARAM_X[args.detect])
|
||||
_send(ControlCode.TARGET_DETECT_ON, param_x=detect_x)
|
||||
if args.auto_lock:
|
||||
_send(ControlCode.AUTO_LOCK_ON_FOUND) # 0x0F 发现目标自动锁定
|
||||
_send(ControlCode.DETECT_TO_TRACK) # 0x10 识别转跟踪
|
||||
_send(ControlCode.AUTO_LOCK_ON_FOUND)
|
||||
_send(ControlCode.DETECT_TO_TRACK)
|
||||
elif args.detect:
|
||||
print("[提示] 只开了识别(0x07)可能仍不锁定,可加 --auto-lock 追加 0x0F/0x10")
|
||||
if tty:
|
||||
render.newline("[提示] 只开了识别(0x07)可能仍不锁定,可按 a 追加 0x0F/0x10")
|
||||
else:
|
||||
print("[提示] 只开了识别(0x07)可能仍不锁定,可加 --auto-lock")
|
||||
|
||||
tty = sys.stdout.isatty()
|
||||
t0 = time.monotonic()
|
||||
last_render = 0.0
|
||||
last_tele_stamp = t0
|
||||
rate_hz = 0.0
|
||||
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
|
||||
latest = None # 最近一帧遥测
|
||||
tracker = LockTracker()
|
||||
recent = [] # 最近事件行(屏幕滚动区)
|
||||
events = []
|
||||
|
||||
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]})")
|
||||
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:
|
||||
@@ -144,8 +192,8 @@ def main() -> int:
|
||||
try:
|
||||
frames = pod.poll()
|
||||
except OSError as exc:
|
||||
print(f"\n[pod-console] 链路中断: {exc}")
|
||||
print("[pod-console] 排查:串口控制台占用(serial-getty)/"
|
||||
render.newline(f"[pod-console] 链路中断: {exc}")
|
||||
render.newline("[pod-console] 排查:串口控制台占用(serial-getty)/"
|
||||
"mini-UART 不稳 → 换 PL011 或 USB 转串口")
|
||||
return 1
|
||||
|
||||
@@ -153,67 +201,102 @@ def main() -> int:
|
||||
if isinstance(frame, Telemetry):
|
||||
n_tele += 1
|
||||
latest = frame
|
||||
dt = time.monotonic() - last_tele_stamp
|
||||
last_tele_stamp = time.monotonic()
|
||||
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}"
|
||||
recent.append(line)
|
||||
if not tty: # 重定向到文件时保留事件流水
|
||||
events.append(line)
|
||||
if not tty:
|
||||
print(f"[事件] {line}")
|
||||
elif isinstance(frame, TargetGeo):
|
||||
n_tgt += 1
|
||||
latest_tgt = (frame, now_s)
|
||||
last_tgt_s = now_s
|
||||
latest_tgt = frame
|
||||
|
||||
# ---- 自动解除锁定:锁定中但表4 长时间不更新 ----
|
||||
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
|
||||
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}")
|
||||
if latest is not None:
|
||||
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} | 视频源={latest.track_source} "
|
||||
f"s3=0x{latest.status3:02X} | "
|
||||
f"视频源={latest.track_source} "
|
||||
f"电机={'开' if latest.motor_on else '关'} "
|
||||
f"自检=0x{latest.self_check:02X}")
|
||||
else:
|
||||
st = "状态字: 尚无遥测"
|
||||
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)")
|
||||
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,标称 {_TELEMETRY_NOMINAL_HZ:.0f}) "
|
||||
f"| 目标帧 {n_tgt}",
|
||||
f" 锁定: {lock_txt}",
|
||||
f" 激光: {laser}",
|
||||
f" 脱靶量(X,Y): {off} 框架角(R,P,Y): {frame_ang}",
|
||||
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(recent[-6:]) if recent else "尚无锁定事件"),
|
||||
" Ctrl+C 退出并输出汇总",
|
||||
" " + (" | ".join(events[-5:]) if events else "暂无事件"),
|
||||
f" {_KEY_HINT}",
|
||||
]
|
||||
_render(lines, tty)
|
||||
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()
|
||||
@@ -221,18 +304,13 @@ def main() -> int:
|
||||
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(" 未出现锁定,排查顺序:")
|
||||
print(" 1) 视场内是否有目标、识别类型是否匹配(--detect person / drone,对应 0x07 参数X 0/1)")
|
||||
print(" 2) 加 --auto-lock:协议上「识别」与「跟踪」是两个阶段,只发 0x07 常不锁定")
|
||||
print(" 3) 看面板「状态字」行:电机是否为开(否则 --motor-on 发 0x27)、自检是否异常")
|
||||
print(" 4) 手动试探:send_control.py --code AUTO_LOCK_ON_FOUND / DETECT_TO_TRACK")
|
||||
print(" 5) 仍不锁定则确认吊舱是否已进入可跟踪模式(镜头/变焦/抗遮挡等)")
|
||||
print(" 未出现锁定:确认视场内有目标并按 d/a 开识别+自动锁定;"
|
||||
"仍不行看面板状态字的电机/自检")
|
||||
else:
|
||||
for i, (s, e, l0, l1, n) in enumerate(tracker.events, 1):
|
||||
lz = "-"
|
||||
@@ -242,10 +320,14 @@ def main() -> int:
|
||||
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
|
||||
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(占比 {pct:.1f}%) | "
|
||||
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
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user