cbb
2c6235d920
桌面 APP 下 ECAT motor 发送只走 pywebview ESF1→native TCP； 删除桥内 HTTP/JSON 回落。附延迟测量与 pack 自测脚本与规格文档。
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())
|