feat(motor): 序列每步新增绿色「单发」按钮，支持仅发送该步动作一次
This commit is contained in:
@@ -0,0 +1,204 @@
|
||||
#!/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()
|
||||
Reference in New Issue
Block a user