From 5340a5da0719d9d2af8f8cc62893be55e0a5cdef Mon Sep 17 00:00:00 2001 From: keita <16924591+keita_sakura@user.noreply.gitee.com> Date: Mon, 28 Sep 2026 10:30:53 +0800 Subject: [PATCH] =?UTF-8?q?=E6=96=B0=E5=A2=9Esend=5Fcontrol.py=E5=B9=B6?= =?UTF-8?q?=E7=BB=99listen=5Fpod.py=E5=8A=A0=E7=9B=AE=E6=A0=87=E8=AF=86?= =?UTF-8?q?=E5=88=AB=E9=AA=8C=E8=AF=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit send_control.py: 单帧表2-1指令下发(名字/数值均可解析, --list列全部控制码) listen_pod.py: --detect {person,drone} 下发0x07目标识别开(参数X 0人车/1无人机) 并跟踪锁定状态变化与首次锁定时刻; 打桩验证通过(0x2B帧校验和a6与规格样例一致) --- utils/pod_bridge/listen_pod.py | 46 ++++++++++++++++++--- utils/pod_bridge/send_control.py | 70 ++++++++++++++++++++++++++++++++ 2 files changed, 111 insertions(+), 5 deletions(-) create mode 100644 utils/pod_bridge/send_control.py diff --git a/utils/pod_bridge/listen_pod.py b/utils/pod_bridge/listen_pod.py index 70d9773..05d659c 100644 --- a/utils/pod_bridge/listen_pod.py +++ b/utils/pod_bridge/listen_pod.py @@ -3,11 +3,13 @@ 用途(对应 docs/树莓派4B部署注意事项.md §7 第 3 步「只接吊舱」): * 打开吊舱串口,静默监听 N 秒,统计 表2-2 遥测(EE 16)/ 表4 目标(EE 18)帧数; * 打印首帧遥测的关键状态(锁定位/激光/脱靶量)与最终判读; - * --center 选项可在监听开始时下发一帧 归中(0x2B) 验证下行链路。 + * --center 选项可在监听开始时下发一帧 归中(0x2B) 验证下行链路; + * --detect {person,drone} 先下发 目标识别开(0x07),再统计锁定状态变化与 + 目标帧,用于验证吊舱目标识别效果。 用法: - python utils/pod_bridge/listen_pod.py [/dev/ttyUSB0] [--baud 115200] - [--seconds 5] [--center] + python utils/pod_bridge/listen_pod.py /dev/ttyAMA0 --seconds 5 + python utils/pod_bridge/listen_pod.py /dev/ttyAMA0 --detect drone --seconds 10 退出码:0 = 遥测帧率达到预期(≥80% 标称 60Hz);1 = 异常(接线/电平/波特率排查)。 """ @@ -25,27 +27,43 @@ from pod_bridge.protocol import (ControlCode, ControlCommand, # noqa: E402 _TELEMETRY_NOMINAL_HZ = 60.0 # 规格 §2.1:周期 16.7ms +# 表2-1 0x07 目标识别开:参数X 0=检测人或车,1=检测无人机(规格 §2.2.1) +_DETECT_PARAM_X = {"person": 0, "drone": 1} + def main() -> int: ap = argparse.ArgumentParser(description="吊舱串口监听诊断") - ap.add_argument("port", nargs="?", default="/dev/ttyUSB0", - help="串口设备号(默认 /dev/ttyUSB0,Windows 如 COM3)") + ap.add_argument("port", nargs="?", default="/dev/ttyAMA0", + help="串口设备号(默认 /dev/ttyAMA0,USB 模块如 /dev/ttyUSB0)") ap.add_argument("--baud", type=int, default=115200, help="波特率(默认 115200)") ap.add_argument("--seconds", type=float, default=5.0, help="监听时长(默认 5s)") ap.add_argument("--center", action="store_true", help="监听开始时下发一帧 归中(0x2B) 验证下行") + ap.add_argument("--detect", choices=sorted(_DETECT_PARAM_X), + help="监听开始前下发 目标识别开(0x07)并跟踪锁定状态变化") args = ap.parse_args() pod = PodSerial(args.port, args.baud) print(f"[listen] {args.port} @ {args.baud} 8N1,监听 {args.seconds:.0f}s ...") + if args.detect: + px = _DETECT_PARAM_X[args.detect] + pod.send(encode_control(ControlCommand( + code=ControlCode.TARGET_DETECT_ON, param_x=px))) + print(f"[listen] 已下发 目标识别开(0x07,参数X={px}:" + f"{'人/车' if px == 0 else '无人机'})") + if args.center: pod.send(encode_control(ControlCommand(code=ControlCode.CENTER))) print("[listen] 已下发 归中(0x2B),观察云台是否回中") t0 = time.monotonic() n_tele, n_tgt, n_other = 0, 0, 0 + n_lock_transitions = 0 + last_locked = None + first_lock_s = None link_error = None + while time.monotonic() - t0 < args.seconds: try: frames = pod.poll() @@ -63,6 +81,14 @@ def main() -> int: frame.off_target_x_deg, frame.off_target_y_deg, frame.frame_roll_deg, frame.frame_pitch_deg, frame.frame_yaw_deg)) + locked = frame.is_target_locked + if last_locked is not None and locked != last_locked: + n_lock_transitions += 1 + print("[listen] 锁定状态 %s → %s(第 %.1fs)" + % (last_locked, locked, time.monotonic() - t0)) + if locked and first_lock_s is None: + first_lock_s = time.monotonic() - t0 + last_locked = locked elif isinstance(frame, TargetGeo): n_tgt += 1 if n_tgt == 1: @@ -72,6 +98,7 @@ def main() -> int: else: n_other += 1 time.sleep(0.005) + try: pod.close() except OSError: @@ -91,6 +118,15 @@ def main() -> int: print(f"[listen] 统计: 遥测帧={n_tele}(期望~{expected:.0f},{rate:.1f}Hz) " f"目标帧={n_tgt} 其他帧={n_other}") + if args.detect: + if first_lock_s is not None: + print(f"[listen] 目标识别: 首次锁定于 {first_lock_s:.1f}s," + f"锁定状态共变化 {n_lock_transitions} 次") + else: + print(f"[listen] 目标识别: 未观察到锁定(锁定变化 {n_lock_transitions} 次);" + "可尝试再发 AUTO_LOCK_ON_FOUND(0x0F) 或 DETECT_TO_TRACK(0x10)," + "并确认镜头视场内有目标") + if n_tele >= expected * 0.8: print("[listen] 判读: 链路正常 ✅(可进入下一步接飞控)") if n_tgt == 0: diff --git a/utils/pod_bridge/send_control.py b/utils/pod_bridge/send_control.py new file mode 100644 index 0000000..a7c6691 --- /dev/null +++ b/utils/pod_bridge/send_control.py @@ -0,0 +1,70 @@ +"""send_control.py —— 向吊舱下发单帧 表2-1 控制指令(台架工具,非飞行组件)。 + +用途:验证吊舱下行链路与各类指令动作(开目标识别/框选跟踪/归中/开电机等), +配合 listen_pod.py 观察遥测状态变化。 + +用法: + python utils/pod_bridge/send_control.py /dev/ttyAMA0 --code TARGET_DETECT_ON --param-x 0 + python utils/pod_bridge/send_control.py /dev/ttyAMA0 --code 0x2B # 归中 + python utils/pod_bridge/send_control.py --list # 列出全部控制码 + +控制码可用名字(ControlCode 成员名,如 TARGET_DETECT_ON)或数值(0x07 / 7)。 +""" + +import argparse +import sys +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, encode_control # noqa: E402 + + +def _resolve_code(text: str) -> int: + """控制码:支持名字(TARGET_DETECT_ON)或数值(0x07 / 7)。""" + name = text.upper() + if hasattr(ControlCode, name): + return int(getattr(ControlCode, name)) + try: + return int(text, 0) + except ValueError: + raise SystemExit(f"无法解析控制码: {text}(用 --list 查看可用名字)") + + +def main() -> int: + ap = argparse.ArgumentParser(description="下发单帧吊舱控制指令(表2-1)") + ap.add_argument("port", nargs="?", default="/dev/ttyAMA0", help="串口设备号") + ap.add_argument("--baud", type=int, default=115200) + ap.add_argument("--code", help="控制码名字或数值,如 TARGET_DETECT_ON / 0x2B") + ap.add_argument("--param-x", type=int, default=0) + ap.add_argument("--param-y", type=int, default=0) + ap.add_argument("--param3", type=int, default=0) + ap.add_argument("--param-x2", type=int, default=0) + ap.add_argument("--param-y2", type=int, default=0) + ap.add_argument("--list", action="store_true", help="列出全部控制码后退出") + args = ap.parse_args() + + if args.list: + for c in ControlCode: + print(f"0x{c.value:02X} {c.name}") + return 0 + + if not args.code: + ap.error("需要 --code(或用 --list 查看可用控制码)") + + code = _resolve_code(args.code) + cmd = ControlCommand(code=code, param_x=args.param_x, param_y=args.param_y, + param3=args.param3, param_x2=args.param_x2, + param_y2=args.param_y2) + frame = encode_control(cmd) + + pod = PodSerial(args.port, args.baud) + pod.send(frame) + pod.close() + print(f"[send] {args.port} 下发 0x{code:02X} → {' '.join(f'{b:02X}' for b in frame)}") + return 0 + + +if __name__ == "__main__": + sys.exit(main())