DCU_DesktopController/scripts/repro_cmdload.py

#!/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()