songhaoyu/code-test-drone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

新增send_control.py并给listen_pod.py加目标识别验证

Browse Source 
send_control.py: 单帧表2-1指令下发(名字/数值均可解析, --list列全部控制码)
listen_pod.py: --detect {person,drone} 下发0x07目标识别开(参数X 0人车/1无人机)
并跟踪锁定状态变化与首次锁定时刻; 打桩验证通过(0x2B帧校验和a6与规格样例一致)
This commit is contained in:
keita
2026-09-28 10:30:53 +08:00
parent 7e43e6302d
commit 5340a5da07
2 changed files with 111 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
utils/pod_bridge/listen_pod.py
+41 -5
View File
@@ -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:
utils/pod_bridge/send_control.py
+70
View File
@@ -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())