252 lines
10 KiB
Python
252 lines
10 KiB
Python
#!/usr/bin/env python3
|
||
# -*- coding: utf-8 -*-
|
||
"""复现实验：高频回发 CMD（与电机当前目标一致的 no-op）+ 全拍监控。
|
||
|
||
用户反馈：数据错乱要"频繁发指令"才出现。本脚本把 TX 压力拉起来，同时
|
||
段2(全拍)与段4(全拍)双路监控，命中即记录 dlc/完整48B/当时CONFIG/时刻，
|
||
用于判定残余拼接的方向与机制。
|
||
|
||
安全性：CMD 载荷 = 电机当前自报 CONFIG 的回声(mode/limitV/cur/vel/ang)
|
||
+ 页面默认 PID(100/100/100)，不改变电机行为。
|
||
|
||
用法: python scripts/repro_cmdload.py [秒数=90] [发送Hz=500]
|
||
"""
|
||
import json
|
||
import math
|
||
import socket
|
||
import struct
|
||
import sys
|
||
import threading
|
||
import time
|
||
|
||
HOST, PORT = "127.0.0.1", 8090
|
||
DUR_S = float(sys.argv[1]) if len(sys.argv) > 1 else 90.0
|
||
SEND_HZ = float(sys.argv[2]) if len(sys.argv) > 2 else 500.0
|
||
|
||
|
||
def post(path, body=b"{}"):
|
||
req = (f"POST {path} HTTP/1.0\r\nHost: {HOST}:{PORT}\r\n"
|
||
f"Content-Type: application/json\r\nContent-Length: {len(body)}\r\n"
|
||
"Connection: close\r\n\r\n").encode("ascii") + body
|
||
s = socket.create_connection((HOST, PORT))
|
||
try:
|
||
s.settimeout(5)
|
||
s.sendall(req)
|
||
buf = bytearray()
|
||
while True:
|
||
c = s.recv(65536)
|
||
if not c:
|
||
break
|
||
buf.extend(c)
|
||
finally:
|
||
s.close()
|
||
return json.loads(bytes(buf).partition(b"\r\n\r\n")[2].decode("utf-8", "replace"))
|
||
|
||
|
||
def build_cmd(cfg, ang, vel):
|
||
"""CMD 帧：mode3 速度+位置，limitV 回声当前 CONFIG，角度=正弦轨迹。
|
||
vel 按节点取 200/150 —— 给 CONFIG 回显的 B7-10 速度字段打节点指纹。"""
|
||
b = bytearray(32)
|
||
b[0] = 3 # mode: Velocity_Angle
|
||
b[1:3] = cfg[1:3] # limitV u16 回声
|
||
b[3:7] = struct.pack("<f", 0.0) # target cur 0
|
||
b[7:11] = struct.pack("<f", vel) # vel limit = 节点指纹
|
||
b[11:15] = struct.pack("<f", ang) # target angle = sine
|
||
for off in (15, 17, 19):
|
||
struct.pack_into("<h", b, off, 100)
|
||
return bytes(b).hex()
|
||
|
||
|
||
stop = threading.Event()
|
||
lock = threading.Lock()
|
||
stats = {"sends": 0, "send_err": 0, "t0": time.time()}
|
||
torn_events = [] # (t, node, dlc, data48hex, cfg_hex)
|
||
batch_events = []
|
||
fp_events = [] # 过滤后仍残余的指纹违规
|
||
fp_total = [0] # （弃用，保留兼容）
|
||
cfg_total = [0] # 监控实际消费的 CONFIG 帧总数（活性自证，非 0 才有资格谈违规=0）
|
||
fp_after = [0] # 过滤器放行掉的违规（应为 0）
|
||
fp_dropped = [0] # 过滤器正确丢弃的违规
|
||
fp_falsekill = [0] # 过滤器误杀的真帧（应为 0）
|
||
cfg_seen = {} # node -> 最近 4 个已接受 CONFIG hex（页面过滤器模拟）
|
||
cfg_susp = {} # node -> 连续疑似计数
|
||
torn_base = [0]
|
||
|
||
|
||
def sender():
|
||
"""每拍把两电机当前 CONFIG 回声成 CMD 发出去。"""
|
||
cfgs = {}
|
||
interval = 1.0 / SEND_HZ
|
||
while not stop.is_set():
|
||
t0 = time.time()
|
||
try:
|
||
rr = post("/api/motor/rxraw")
|
||
raw = bytes.fromhex(rr.get("raw") or "")
|
||
off = 0
|
||
for _ in range(rr.get("count") or 0):
|
||
if off + 8 > len(raw):
|
||
break
|
||
dlc = raw[off + 1]
|
||
fid = struct.unpack_from("<I", raw, off + 4)[0]
|
||
w = min(dlc, 32)
|
||
cfgs[(fid >> 16) & 0xFF] = raw[off + 8: off + 8 + w]
|
||
off += 8 + w
|
||
except Exception:
|
||
time.sleep(0.05)
|
||
continue
|
||
if len(cfgs) >= 2 or time.time() - stats["t0"] > 3:
|
||
# 电机偶尔暂停 CONFIG 自报（如正弦轰炸后）：拿不到回声就用页面默认
|
||
# 限压 24.0V（u16 LE 0x5DC0 → bytes c0 5d）兜底，保证指令压力不断。
|
||
fallback = bytes([0x03, 0xc0, 0x5d]) + bytes(21)
|
||
s = math.sin(2.0 * math.pi * (time.time() - stats["t0"]) / 10.0)
|
||
frames = []
|
||
for n in (2, 3):
|
||
c = cfgs.get(n, fallback)
|
||
ang = 1000.0 + (200.0 * s if n == 2 else -200.0 * s)
|
||
vel = 200.0 if n == 2 else 150.0
|
||
frames.append({"ch": 2, "id": hex((n << 16) | 0x0200), "dlc": 32,
|
||
"data": build_cmd(c, ang, vel)})
|
||
try:
|
||
post("/api/motor/send", json.dumps({"frames": frames}).encode())
|
||
with lock:
|
||
stats["sends"] += 1
|
||
except Exception:
|
||
with lock:
|
||
stats["send_err"] += 1
|
||
dt = interval - (time.time() - t0)
|
||
if dt > 0:
|
||
time.sleep(dt)
|
||
|
||
|
||
def is_cfg_head(d, cfg):
|
||
"""段2槽头 8B 是否等于该节点当前 CONFIG 头（污染判据，比量程更准）。"""
|
||
return cfg and len(d) >= 8 and d[:8] == cfg[:8] and d[0] == 0x03
|
||
|
||
|
||
def monitor():
|
||
seen_s, seen_r = set(), set()
|
||
cfgs = {}
|
||
while not stop.is_set():
|
||
try:
|
||
st = post("/api/motor/state")
|
||
except Exception:
|
||
time.sleep(0.01)
|
||
continue
|
||
cyc = st.get("cycles")
|
||
if cyc in seen_s:
|
||
continue
|
||
seen_s.add(cyc)
|
||
if len(seen_s) > 400000:
|
||
seen_s.clear()
|
||
for m in st.get("motors") or []:
|
||
if not m.get("online") or len(m.get("data") or "") < 96:
|
||
continue
|
||
d = bytes.fromhex(m["data"])
|
||
mcu = struct.unpack_from("<h", d, 0)[0] * 0.01
|
||
cfg = cfgs.get(m["node"])
|
||
if not (-40.0 <= mcu <= 125.0) or is_cfg_head(d, cfg):
|
||
with lock:
|
||
if len(torn_events) < 40:
|
||
torn_events.append((round(time.time() - stats["t0"], 2),
|
||
m["node"], m.get("dlc"), d.hex(),
|
||
(cfg or b"").hex()))
|
||
try:
|
||
rr = post("/api/motor/rxraw")
|
||
except Exception:
|
||
continue
|
||
rc = rr.get("cycle")
|
||
if rc in seen_r:
|
||
continue
|
||
seen_r.add(rc)
|
||
if len(seen_r) > 400000:
|
||
seen_r.clear()
|
||
raw = bytes.fromhex(rr.get("raw") or "")
|
||
off = 0
|
||
for _ in range(rr.get("count") or 0):
|
||
if off + 8 > len(raw):
|
||
break
|
||
dlc = raw[off + 1]
|
||
fid = struct.unpack_from("<I", raw, off + 4)[0]
|
||
w = min(dlc, 32)
|
||
d = raw[off + 8: off + 8 + w]
|
||
node = (fid >> 16) & 0xFF
|
||
if (fid & 0xFFFF) == 0x0101:
|
||
cfgs[node] = d
|
||
cfg_total[0] += 1 # 活性自证：本拍监控实际看到的 CONFIG 帧数
|
||
if any(d[15:24]):
|
||
with lock:
|
||
if len(batch_events) < 40:
|
||
batch_events.append((round(time.time() - stats["t0"], 2),
|
||
node, d.hex()))
|
||
# CONFIG↔CONFIG 拼接：结构校验抓不到，用速度指纹暴露。
|
||
# 预热 3s：sender 尚未生效时电机回显的是页面/上一轮的 vel，
|
||
# 不是拼接，不能算违规。
|
||
if len(d) >= 15 and time.time() - stats["t0"] > 3.0:
|
||
vel = struct.unpack_from("<f", d, 7)[0]
|
||
exp_vel = 200.0 if node == 2 else 150.0
|
||
viol = abs(vel - exp_vel) > 1.0
|
||
with lock:
|
||
# 模拟页面 CONFIG 跨节点拼接过滤器（transport-ecat.js
|
||
# dispatchFrame 同逻辑），统计过滤后残余违规与误杀。
|
||
hx = d.hex()
|
||
seen = cfg_seen.setdefault(node, [])
|
||
own = hx in seen
|
||
foreign = (not own) and any(
|
||
hx in s2 for n2, s2 in cfg_seen.items() if n2 != node)
|
||
if own or not foreign or cfg_susp.get(node, 0) >= 5:
|
||
cfg_susp[node] = 0
|
||
seen.insert(0, hx)
|
||
del seen[4:]
|
||
if viol:
|
||
fp_after[0] += 1
|
||
if len(fp_events) < 40:
|
||
fp_events.append((round(time.time() - stats["t0"], 2),
|
||
node, vel, exp_vel, hx))
|
||
else:
|
||
cfg_susp[node] = cfg_susp.get(node, 0) + 1
|
||
if viol:
|
||
fp_dropped[0] += 1
|
||
else:
|
||
fp_falsekill[0] += 1
|
||
off += 8 + w
|
||
|
||
|
||
def main():
|
||
try:
|
||
r = post("/api/app/load")
|
||
torn_base[0] = r.get("rx_torn") or 0
|
||
except Exception:
|
||
pass
|
||
ts = threading.Thread(target=sender, daemon=True)
|
||
tm = threading.Thread(target=monitor, daemon=True)
|
||
ts.start(); tm.start()
|
||
time.sleep(DUR_S)
|
||
stop.set(); ts.join(3); tm.join(3)
|
||
try:
|
||
r = post("/api/app/load")
|
||
torn_now = r.get("rx_torn") or 0
|
||
except Exception:
|
||
torn_now = -1
|
||
with lock:
|
||
print(f"时长 {DUR_S:.0f}s 发送 {stats['sends']} 次(错{stats['send_err']}) "
|
||
f"≈{stats['sends']/DUR_S:.0f} CMD拍/s")
|
||
print(f"rx_torn: {torn_base[0]} -> {torn_now} (Δ={torn_now-torn_base[0]})")
|
||
print(f"段2 污染事件: {len(torn_events)}（前 8）")
|
||
for e in torn_events[:8]:
|
||
print(f" t={e[0]}s node={e[1]} dlc={e[2]}")
|
||
print(f" slot: {e[3]}")
|
||
print(f" cfg : {e[4]}")
|
||
print(f"段4 污染事件: {len(batch_events)}（前 8）")
|
||
for e in batch_events[:8]:
|
||
print(f" t={e[0]}s node={e[1]} data={e[2]}")
|
||
print(f"CONFIG↔CONFIG 指纹(预热后): 违规={fp_after[0]+fp_dropped[0]} "
|
||
f"过滤器正确丢弃={fp_dropped[0]} 过滤后残余={fp_after[0]} "
|
||
f"误杀真帧={fp_falsekill[0]}（样本 {len(fp_events)}）")
|
||
print(f"监控消费的 CONFIG 帧总数: {cfg_total[0]}（活性自证：>0 才说明违规=0 是真结果）")
|
||
for e in fp_events[:8]:
|
||
print(f" t={e[0]}s node={e[1]} vel={e[2]:.1f} 应为{e[3]:.0f} data={e[4]}")
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|