354 lines
14 KiB
Python
354 lines
14 KiB
Python
|
#!/usr/bin/env python3
|
|||
|
|
# -*- coding: utf-8 -*-
|
|||
|
|
"""pdo-fast-send 端到端分段延迟（模式切换代理）。
|
|||
|
|
|
|||
|
|
电机启动/转动有惯性，rtAngle/rtVelocity 阈值难踩准。改用 **CONFIG 回报的
|
|||
|
|
Target_Control_Mode（0x000N0101 B0）** 作为「命令已被电机接收并应用」的可见
|
|||
|
|
代理：CMD 的 mode 字节一旦被接受，下一拍 CONFIG 就会以新 mode 回报。
|
|||
|
|
|
|||
|
|
路径:
|
|||
|
|
json — HTTP POST /ecat/api/motor/send（浏览器同款页面代理）
|
|||
|
|
fast — ecat_send_raw_frames（桌面桥同构 ESF1 TCP，不含 pywebview hop）
|
|||
|
|
|
|||
|
|
分段:
|
|||
|
|
t_click = 发请求前
|
|||
|
|
t_sent = 控制面返回（入队完成）
|
|||
|
|
t_can = bridge.tx_count/ack 变化（EtherCAT→GD32 CAN 出口）
|
|||
|
|
t_mode = 观测到目标节点 CONFIG B0 == 期望 mode（xrx 或 motor/rx）
|
|||
|
|
|
|||
|
|
用法:
|
|||
|
|
python scripts/check_e2e_click_motion.py
|
|||
|
|
python scripts/check_e2e_click_motion.py --node 3 --ch 2 --trials 5
|
|||
|
|
python scripts/check_e2e_click_motion.py --proxy motion # 旧：角度/速度阈值
|
|||
|
|
"""
|
|||
|
|
from __future__ import annotations
|
|||
|
|
|
|||
|
|
import argparse
|
|||
|
|
import json
|
|||
|
|
import os
|
|||
|
|
import statistics
|
|||
|
|
import struct
|
|||
|
|
import sys
|
|||
|
|
import time
|
|||
|
|
import urllib.request
|
|||
|
|
|
|||
|
|
sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), "..")))
|
|||
|
|
from esp32_server import ecat_send_raw_frames # noqa: E402
|
|||
|
|
|
|||
|
|
PAGE = os.environ.get("DCU_PAGE_BASE", "http://127.0.0.1:2625")
|
|||
|
|
ECAT = PAGE + "/ecat/api"
|
|||
|
|
# 观测默认打 native 直连：页面代理 p50~2ms×多次 + xrx 单帧槽会把 CONFIG 观测抬到 ~12ms
|
|||
|
|
NATIVE = os.environ.get("DCU_NATIVE_API", "http://127.0.0.1:8090/api")
|
|||
|
|
OBS = NATIVE # 观测环（motor/state、motor/rx）
|
|||
|
|
|
|||
|
|
# 安全模式对：0=Disable（输出关闭），2=Velocity 但目标速度 0（不驱动转动）
|
|||
|
|
SAFE_TOGGLE = (0, 2)
|
|||
|
|
|
|||
|
|
|
|||
|
|
def post(url, obj=None, raw=None, ctype="application/json", timeout=8.0):
|
|||
|
|
if raw is not None:
|
|||
|
|
data = raw
|
|||
|
|
headers = {"Content-Type": ctype}
|
|||
|
|
else:
|
|||
|
|
data = json.dumps(obj or {}, ensure_ascii=False, separators=(",", ":")).encode("utf-8")
|
|||
|
|
headers = {"Content-Type": ctype}
|
|||
|
|
req = urllib.request.Request(url, data=data, headers=headers, method="POST")
|
|||
|
|
t0 = time.perf_counter()
|
|||
|
|
with urllib.request.urlopen(req, timeout=timeout) as r:
|
|||
|
|
body = r.read()
|
|||
|
|
ms = (time.perf_counter() - t0) * 1000.0
|
|||
|
|
try:
|
|||
|
|
return ms, json.loads(body.decode("utf-8", "replace") or "{}")
|
|||
|
|
except Exception:
|
|||
|
|
return ms, {"_raw_len": len(body)}
|
|||
|
|
|
|||
|
|
|
|||
|
|
def decode_status(data_hex: str):
|
|||
|
|
if not data_hex or len(data_hex) < 36:
|
|||
|
|
return None
|
|||
|
|
raw = bytes.fromhex(data_hex[:72] if len(data_hex) >= 72 else data_hex)
|
|||
|
|
if len(raw) < 21:
|
|||
|
|
return None
|
|||
|
|
|
|||
|
|
def f32(off):
|
|||
|
|
if off + 4 > len(raw):
|
|||
|
|
return 0.0
|
|||
|
|
v = struct.unpack_from("<f", raw, off)[0]
|
|||
|
|
return v if v == v else 0.0
|
|||
|
|
|
|||
|
|
return {"rtAngle": f32(13), "rtVelocity": f32(17), "rtCurrent": f32(21), "b10": raw[10]}
|
|||
|
|
|
|||
|
|
|
|||
|
|
def motor_sample(node: int, ch: int):
|
|||
|
|
_, snap = post(OBS + "/motor/state", {}, timeout=5)
|
|||
|
|
if not snap.get("ok"):
|
|||
|
|
return None, None, snap
|
|||
|
|
br = snap.get("bridge") or {}
|
|||
|
|
outlet = (br.get("tx_count"), br.get("ack"), br.get("last_tx_id_hex"))
|
|||
|
|
st = None
|
|||
|
|
for m in snap.get("motors") or []:
|
|||
|
|
if m.get("node") == node and m.get("ch") == ch:
|
|||
|
|
st = decode_status(m.get("data") or "")
|
|||
|
|
break
|
|||
|
|
return st, outlet, snap
|
|||
|
|
|
|||
|
|
|
|||
|
|
def build_cmd(mode, voltage=24.0, current=0.0, velocity=0.0, angle=0.0, pid=100):
|
|||
|
|
buf = bytearray(32)
|
|||
|
|
buf[0] = mode & 0xFF
|
|||
|
|
struct.pack_into("<H", buf, 1, int(round(voltage * 1000)) & 0xFFFF)
|
|||
|
|
struct.pack_into("<f", buf, 3, float(current))
|
|||
|
|
struct.pack_into("<f", buf, 7, float(velocity))
|
|||
|
|
struct.pack_into("<f", buf, 11, float(angle))
|
|||
|
|
struct.pack_into("<h", buf, 15, pid)
|
|||
|
|
struct.pack_into("<h", buf, 17, pid)
|
|||
|
|
struct.pack_into("<h", buf, 19, pid)
|
|||
|
|
return buf.hex()
|
|||
|
|
|
|||
|
|
|
|||
|
|
def send_path(path: str, ch: int, node: int, data_hex: str):
|
|||
|
|
frames = [{"ch": ch, "id": (node << 16) | 0x0200, "dlc": 32, "data": data_hex}]
|
|||
|
|
if path == "fast":
|
|||
|
|
t0 = time.perf_counter()
|
|||
|
|
resp = ecat_send_raw_frames(frames, 2000)
|
|||
|
|
return (time.perf_counter() - t0) * 1000.0, resp
|
|||
|
|
ms, resp = post(ECAT + "/motor/send", {"frames": frames}, timeout=6)
|
|||
|
|
return ms, resp
|
|||
|
|
|
|||
|
|
|
|||
|
|
def outlet_changed(a, b):
|
|||
|
|
if not a or not b:
|
|||
|
|
return False
|
|||
|
|
return (a[0], a[1]) != (b[0], b[1]) and a[0] is not None
|
|||
|
|
|
|||
|
|
|
|||
|
|
def cfg_mode_from_hex(data_hex: str):
|
|||
|
|
if not data_hex or len(data_hex) < 2:
|
|||
|
|
return None
|
|||
|
|
try:
|
|||
|
|
return int(data_hex[:2], 16)
|
|||
|
|
except ValueError:
|
|||
|
|
return None
|
|||
|
|
|
|||
|
|
|
|||
|
|
def scan_config_mode(node: int, snap: dict):
|
|||
|
|
"""从 xrx + motor/rx 找目标节点 CONFIG(0x000N0101) 的 B0 mode。"""
|
|||
|
|
want_id = (node << 16) | 0x0101
|
|||
|
|
hits = []
|
|||
|
|
x = snap.get("xrx") or {}
|
|||
|
|
if x.get("id") == want_id:
|
|||
|
|
m = cfg_mode_from_hex(x.get("data") or "")
|
|||
|
|
if m is not None:
|
|||
|
|
hits.append(("xrx", m, x.get("data")))
|
|||
|
|
rxb = snap.get("rx_batch") or {}
|
|||
|
|
# frames 可能在快照里；若空则单独 drain
|
|||
|
|
for f in rxb.get("frames") or []:
|
|||
|
|
if f.get("id") == want_id:
|
|||
|
|
m = cfg_mode_from_hex(f.get("data") or "")
|
|||
|
|
if m is not None:
|
|||
|
|
hits.append(("rxb", m, f.get("data")))
|
|||
|
|
return hits
|
|||
|
|
|
|||
|
|
|
|||
|
|
def drain_rx_modes(node: int):
|
|||
|
|
want_id = (node << 16) | 0x0101
|
|||
|
|
_, rx = post(OBS + "/motor/rx", {}, timeout=4)
|
|||
|
|
out = []
|
|||
|
|
for f in rx.get("frames") or []:
|
|||
|
|
if f.get("id") == want_id:
|
|||
|
|
m = cfg_mode_from_hex(f.get("data") or "")
|
|||
|
|
if m is not None:
|
|||
|
|
out.append(m)
|
|||
|
|
return out
|
|||
|
|
|
|||
|
|
|
|||
|
|
def observe_config_mode(node: int):
|
|||
|
|
"""返回 (mode_or_None, source)。优先 drain rx（CONFIG 主通道），再 xrx。"""
|
|||
|
|
modes = drain_rx_modes(node)
|
|||
|
|
if modes:
|
|||
|
|
return modes[-1], "rx"
|
|||
|
|
_, snap = post(OBS + "/motor/state", {}, timeout=4)
|
|||
|
|
hits = scan_config_mode(node, snap)
|
|||
|
|
if hits:
|
|||
|
|
return hits[-1][1], hits[-1][0]
|
|||
|
|
return None, None
|
|||
|
|
|
|||
|
|
|
|||
|
|
def run_once(path: str, node: int, ch: int, proxy: str, timeout: float, next_mode: int) -> dict:
|
|||
|
|
st0, outlet0, _ = motor_sample(node, ch)
|
|||
|
|
# 基线 mode：SIG 必须相对它变化，避免 CONFIG 持续回报同一 mode 造成假阳性
|
|||
|
|
base_mode, base_src = observe_config_mode(node)
|
|||
|
|
if proxy == "mode":
|
|||
|
|
# 强制与基线不同
|
|||
|
|
if base_mode == next_mode:
|
|||
|
|
next_mode = SAFE_TOGGLE[1] if base_mode == SAFE_TOGGLE[0] else SAFE_TOGGLE[0]
|
|||
|
|
data = build_cmd(mode=next_mode, voltage=24.0, current=0.0, velocity=0.0, angle=0.0)
|
|||
|
|
expect = next_mode
|
|||
|
|
else:
|
|||
|
|
data = build_cmd(mode=2, voltage=24.0, current=0.0, velocity=40.0, angle=0.0)
|
|||
|
|
expect = 2
|
|||
|
|
base_mode = None
|
|||
|
|
|
|||
|
|
t_click = time.perf_counter()
|
|||
|
|
sent_ms, resp = send_path(path, ch, node, data)
|
|||
|
|
t_sent = time.perf_counter()
|
|||
|
|
|
|||
|
|
t_can = None
|
|||
|
|
t_mode = None
|
|||
|
|
t_motion = None
|
|||
|
|
can_detail = mode_detail = motion_detail = ""
|
|||
|
|
ang0 = st0["rtAngle"] if st0 else 0.0
|
|||
|
|
vel0 = st0["rtVelocity"] if st0 else 0.0
|
|||
|
|
|
|||
|
|
while time.perf_counter() - t_click < timeout:
|
|||
|
|
st, outlet, snap = motor_sample(node, ch)
|
|||
|
|
now = time.perf_counter()
|
|||
|
|
if t_can is None and outlet_changed(outlet0, outlet):
|
|||
|
|
t_can = now
|
|||
|
|
can_detail = f"{outlet0} → {outlet}"
|
|||
|
|
if t_mode is None and proxy == "mode":
|
|||
|
|
# 观测优先 drain rx：CONFIG 约 ~200Hz/节点，xrx 只有单槽会被其它节点挤掉
|
|||
|
|
drained = drain_rx_modes(node)
|
|||
|
|
if expect in drained and expect != base_mode:
|
|||
|
|
t_mode = now
|
|||
|
|
mode_detail = f"mode {base_mode}→{expect} via rx"
|
|||
|
|
else:
|
|||
|
|
hits = scan_config_mode(node, snap)
|
|||
|
|
for src, m, raw in reversed(hits):
|
|||
|
|
if m == expect and m != base_mode:
|
|||
|
|
t_mode = now
|
|||
|
|
mode_detail = f"mode {base_mode}→{m} via {src} data={(raw or '')[:16]}"
|
|||
|
|
break
|
|||
|
|
if st and t_motion is None and proxy == "motion":
|
|||
|
|
d_ang = abs(st["rtAngle"] - ang0)
|
|||
|
|
d_vel = abs(st["rtVelocity"] - vel0)
|
|||
|
|
if d_ang > 0.3 or abs(d_vel) > 5.0:
|
|||
|
|
t_motion = now
|
|||
|
|
motion_detail = f"Δang={d_ang:.3f} Δvel={d_vel:.2f}"
|
|||
|
|
done_mode = (proxy == "mode" and t_mode is not None) or (proxy == "motion" and t_motion is not None)
|
|||
|
|
if t_can is not None and done_mode:
|
|||
|
|
break
|
|||
|
|
# 直连 native 时观测环约 0.6–1ms；页面代理则 ≥2ms×N，会主导 p50
|
|||
|
|
time.sleep(0)
|
|||
|
|
|
|||
|
|
send_path(path, ch, node, build_cmd(mode=0, voltage=24.0, current=0.0, velocity=0.0, angle=0.0))
|
|||
|
|
time.sleep(0.08)
|
|||
|
|
|
|||
|
|
def ms(dt):
|
|||
|
|
return None if dt is None else dt * 1000.0
|
|||
|
|
|
|||
|
|
t_sig = t_mode if proxy == "mode" else t_motion
|
|||
|
|
return {
|
|||
|
|
"ok": True,
|
|||
|
|
"path": path,
|
|||
|
|
"proxy": proxy,
|
|||
|
|
"base_mode": base_mode,
|
|||
|
|
"base_src": base_src,
|
|||
|
|
"expect_mode": expect if proxy == "mode" else None,
|
|||
|
|
"resp_ok": bool(resp.get("ok")),
|
|||
|
|
"resp_path": resp.get("path"),
|
|||
|
|
"sent_ms": sent_ms,
|
|||
|
|
"click_to_sent": ms(t_sent - t_click),
|
|||
|
|
"click_to_can": ms(None if t_can is None else t_can - t_click),
|
|||
|
|
"click_to_sig": ms(None if t_sig is None else t_sig - t_click),
|
|||
|
|
"sent_to_can": ms(None if t_can is None else t_can - t_sent),
|
|||
|
|
"sent_to_sig": ms(None if t_sig is None else t_sig - t_sent),
|
|||
|
|
"can_to_sig": ms(None if (t_can is None or t_sig is None) else t_sig - t_can),
|
|||
|
|
"can_detail": can_detail,
|
|||
|
|
"sig_detail": mode_detail if proxy == "mode" else motion_detail,
|
|||
|
|
"base_vel": vel0,
|
|||
|
|
"sig_ok": t_sig is not None,
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
def summarize(rows, key):
|
|||
|
|
xs = [r[key] for r in rows if r.get(key) is not None]
|
|||
|
|
if not xs:
|
|||
|
|
return "n/a"
|
|||
|
|
return (
|
|||
|
|
f"n={len(xs)} mean={statistics.mean(xs):.2f}ms "
|
|||
|
|
f"p50={statistics.median(xs):.2f} min={min(xs):.2f} max={max(xs):.2f}"
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
|
|||
|
|
def main() -> int:
|
|||
|
|
ap = argparse.ArgumentParser()
|
|||
|
|
ap.add_argument("--node", type=int, default=3)
|
|||
|
|
ap.add_argument("--ch", type=int, default=2)
|
|||
|
|
ap.add_argument("--timeout", type=float, default=2.0)
|
|||
|
|
ap.add_argument("--trials", type=int, default=4)
|
|||
|
|
ap.add_argument("--paths", type=str, default="json,fast")
|
|||
|
|
ap.add_argument("--proxy", type=str, default="mode", choices=["mode", "motion"])
|
|||
|
|
args = ap.parse_args()
|
|||
|
|
paths = [p.strip() for p in args.paths.split(",") if p.strip()]
|
|||
|
|
|
|||
|
|
_, health = post(OBS + "/native/health", {})
|
|||
|
|
print("health:", json.dumps(health, ensure_ascii=False)[:220])
|
|||
|
|
print(f"观测环直连: {OBS}（发送仍按 path 走 page/fast）")
|
|||
|
|
post(ECAT + "/motor/start",
|
|||
|
|
{"period_ms": 0.25, "dc_cycle_us": 250, "dc_shift_us": 0}, timeout=30)
|
|||
|
|
time.sleep(0.25)
|
|||
|
|
|
|||
|
|
cur_mode, src = observe_config_mode(args.node)
|
|||
|
|
print(f"基线 CONFIG mode node={args.node}: {cur_mode} (via {src})")
|
|||
|
|
print(f"代理={args.proxy} 模式对={SAFE_TOGGLE} 每次在两者间切换，目标速度恒 0")
|
|||
|
|
|
|||
|
|
all_rows: dict[str, list] = {}
|
|||
|
|
for path in paths:
|
|||
|
|
print(f"\n======== path={path} proxy={args.proxy} node={args.node} ch={args.ch} ========")
|
|||
|
|
rows = []
|
|||
|
|
for i in range(args.trials):
|
|||
|
|
# 每轮现读基线再选「与基线不同」的 mode
|
|||
|
|
cur_mode, _src = observe_config_mode(args.node)
|
|||
|
|
if args.proxy == "mode":
|
|||
|
|
if cur_mode == SAFE_TOGGLE[0]:
|
|||
|
|
next_mode = SAFE_TOGGLE[1]
|
|||
|
|
elif cur_mode == SAFE_TOGGLE[1]:
|
|||
|
|
next_mode = SAFE_TOGGLE[0]
|
|||
|
|
else:
|
|||
|
|
next_mode = SAFE_TOGGLE[0]
|
|||
|
|
else:
|
|||
|
|
next_mode = 2
|
|||
|
|
print(f"--- trial {i+1}/{args.trials} base_mode={cur_mode} → send mode={next_mode} ---")
|
|||
|
|
r = run_once(path, args.node, args.ch, args.proxy, args.timeout, next_mode)
|
|||
|
|
rows.append(r)
|
|||
|
|
if not r.get("resp_ok") or not r.get("sig_ok", True):
|
|||
|
|
print(" FAIL/NOSIG", {k: r.get(k) for k in
|
|||
|
|
("resp_ok", "sig_ok", "base_mode", "expect_mode", "resp_path", "sig_detail")})
|
|||
|
|
else:
|
|||
|
|
print(
|
|||
|
|
f" via={r['resp_path']} mode {r.get('base_mode')}→{r.get('expect_mode')} "
|
|||
|
|
f"sent={r['sent_ms']:.2f}ms "
|
|||
|
|
f"click→sent={r['click_to_sent']:.2f} "
|
|||
|
|
f"click→can={r['click_to_can'] if r['click_to_can'] is not None else 'n/a'} "
|
|||
|
|
f"click→sig={r['click_to_sig'] if r['click_to_sig'] is not None else 'n/a'} "
|
|||
|
|
f"sent→can={r['sent_to_can'] if r['sent_to_can'] is not None else 'n/a'} "
|
|||
|
|
f"sent→sig={r['sent_to_sig'] if r['sent_to_sig'] is not None else 'n/a'} "
|
|||
|
|
f"can→sig={r['can_to_sig'] if r['can_to_sig'] is not None else 'n/a'}"
|
|||
|
|
)
|
|||
|
|
if r.get("can_detail"):
|
|||
|
|
print(" CAN", r["can_detail"])
|
|||
|
|
if r.get("sig_detail"):
|
|||
|
|
print(" SIG", r["sig_detail"])
|
|||
|
|
time.sleep(0.35)
|
|||
|
|
all_rows[path] = rows
|
|||
|
|
good = [r for r in rows if r.get("ok") and r.get("resp_ok") and r.get("sig_ok", True)]
|
|||
|
|
print(f"\n=== {path} 汇总 ({len(good)}/{len(rows)}) ===")
|
|||
|
|
for key in ("sent_ms", "click_to_sent", "click_to_can", "click_to_sig",
|
|||
|
|
"sent_to_can", "sent_to_sig", "can_to_sig"):
|
|||
|
|
print(f" {key:14s} {summarize(good, key)}")
|
|||
|
|
|
|||
|
|
print("\n======== 口径 ========")
|
|||
|
|
if args.proxy == "mode":
|
|||
|
|
print("主信号: CONFIG Target_Control_Mode (0x000N0101 B0) 变为期望值")
|
|||
|
|
print(" = 电机已接收并回报新控制模式（不依赖转动/角度阈值）")
|
|||
|
|
else:
|
|||
|
|
print("主信号: STATUS rtAngle/rtVelocity 阈值（旧口径，启动惯性会漏测）")
|
|||
|
|
print("t_click 不含真实鼠标与 pywebview hop；观测分辨率受 motor/state/rx 轮询限制。")
|
|||
|
|
print("fast 路径为 GUI 桥同构 Python 调用。")
|
|||
|
|
return 0
|
|||
|
|
|
|||
|
|
|
|||
|
|
if __name__ == "__main__":
|
|||
|
|
sys.exit(main())
|