205 lines
7.2 KiB
Python
205 lines
7.2 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 = []
|
||
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:
|
||
# 判别实验：两电机反相正弦 + 不同速度限，给帧内容打节点指纹。
|
||
# node2: ang=1000+200sin, vel=200 node3: ang=1000-200sin, vel=150
|
||
s = math.sin(2.0 * math.pi * (time.time() - stats["t0"]) / 10.0)
|
||
frames = []
|
||
for n, c in sorted(cfgs.items()):
|
||
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
|
||
if any(d[15:24]):
|
||
with lock:
|
||
if len(batch_events) < 40:
|
||
batch_events.append((round(time.time() - stats["t0"], 2),
|
||
node, d.hex()))
|
||
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]}")
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|