cbb
79633a2b0f
后端 PDO 循环改为按在线槽收缩状态窗口，稀疏总线下单槽每周期都能刷新， 并补上尾部槽的发现扫描游标；新增自旋节拍源（阈值/显式覆盖）、P 核识别 与线程级钉核，自旋期间释放 GIL 以免 HTTP 线程被饿死；启动时先关停旧实例， 避免两个进程争用 Npcap 适配器。 记录每拍完整时间线、GC stop-the-world 事件与网格跳拍数，经 /api/motor/dbg 导出用于归因固定频率的超时；配
685 lines
32 KiB
Python
685 lines
32 KiB
Python
"""EtherCAT 电机控制台后端自测（不接硬件）。
|
||
|
||
用一个模拟 GD32 buildInput/processOutput 行为的假从站，验证：
|
||
1. DLC 32 的电机 CMD 能完整下发（旧 24B 布局会静默截断后 8 字节）
|
||
2. 后台 PDO 循环靠滚动窗口扫完 27 个槽
|
||
3. STATUS 完整 32B 回传（旧 8B 短快照根本装不下）
|
||
4. 命令队列按 seq 沿下发，不互相覆盖
|
||
5. 循环可干净停止
|
||
6. 【1.5.56】批量发送区：多帧在**同一个周期**发出（同步下发）
|
||
7. 【1.5.56】批量接收区：一周期回收多帧，含 DLC64 CANFD
|
||
8. 【1.5.56】兼容性：批量区为空时，字节布局与 1.5.55 完全一致
|
||
9. 窗口自适应：稀疏总线(27 槽只有 2 个有主)下单槽每周期都刷新，
|
||
而不是旧实现的每 4 周期一次
|
||
10. 发现扫描：热插上来的电机仍能被找到并纳入常规窗口
|
||
（含 slot 21..26 这段尾部 —— 朴素的"+7 到越界归零"游标会永久漏掉它）
|
||
11. 自旋节拍源：自动阈值/显式覆盖，以及 1ms 档确实跑得准
|
||
12. 自旋不得持 GIL 空转（否则 HTTP 线程被饿死，真机实测劣化到 40ms）
|
||
13. P 核识别与钉核：混合架构下自旋钉到快核(P 核)组，且钉核是线程级的
|
||
14. api_motor_start 的 OP 状态核实：已在 OP 能正常启动，从站掉出 OP 能发现
|
||
|
||
跑法: python test_motor_backend.py
|
||
"""
|
||
import struct
|
||
import sys
|
||
import threading
|
||
import time
|
||
import types
|
||
import os
|
||
|
||
BASE = os.path.dirname(os.path.abspath(__file__))
|
||
sys.path.insert(0, BASE)
|
||
|
||
# pysoem 依赖 Npcap 与真网卡，自测里用桩替掉
|
||
_fake = types.ModuleType("pysoem")
|
||
_fake.find_adapters = lambda: []
|
||
_fake.INIT_STATE, _fake.PREOP_STATE = 1, 2
|
||
_fake.SAFEOP_STATE, _fake.OP_STATE = 4, 8
|
||
_fake.STATE_ACK = 0x10
|
||
_fake.Master = object
|
||
sys.modules["pysoem"] = _fake
|
||
|
||
import ecat_server as S # noqa: E402
|
||
|
||
SLOT = S.MOT_SLOT_SIZE
|
||
NSLOT = S.MOT_SLOTS_PER_PDO
|
||
|
||
|
||
class FakeSlave:
|
||
"""按 comm/ethercat_can_bridge.cpp 的语义模拟从站侧。
|
||
|
||
state/al_status 是 api_motor_start 核实"是否真在 OP"时要读的字段（pysoem
|
||
的 Slave 也有这两个），默认给 OP，掉线场景由测试临时改。
|
||
"""
|
||
|
||
state = 8 # OP_STATE
|
||
al_status = 0
|
||
|
||
def __init__(self):
|
||
self._out = bytes(S.PDO_OUT_LEN)
|
||
self.rx_frames = [] # 从站"发出"的帧 (ch,id,dlc,hexdata)
|
||
self.tx_cycles = [] # 每个 seq 沿发出的帧数，用来证明"同一周期多帧"
|
||
self.last_seq = None # 对应固件 s_lastSeq / s_seqPrimed
|
||
self.tx_count = 0
|
||
self.pending_rx = [] # 待经批量接收区回给主站的帧
|
||
self.rx_lost = 0
|
||
|
||
@property
|
||
def output(self):
|
||
return self._out
|
||
|
||
@output.setter
|
||
def output(self, v):
|
||
self._out = bytes(v)
|
||
seq = self._out[0]
|
||
# 固件语义：首次只记基准不发送；之后仅 seq 变化沿触发一次发送
|
||
if self.last_seq is not None and seq != self.last_seq:
|
||
n = 0
|
||
# 段1 单帧区（dlc=0 表示本次只发批量）
|
||
dlc = self._out[2]
|
||
if dlc:
|
||
self.rx_frames.append((
|
||
self._out[1],
|
||
struct.unpack_from("<I", self._out, 4)[0],
|
||
dlc,
|
||
self._out[S.TX_OFF_DATA:S.TX_OFF_DATA + dlc].hex().upper(),
|
||
))
|
||
n += 1
|
||
# 段2 批量发送区：逐帧解析 (8B 头 + dlc)，越界即停
|
||
count = self._out[S.TXB_OFF_COUNT]
|
||
off = S.TXB_OFF_ITEMS
|
||
for _ in range(count):
|
||
if off + S.FRM_HDR_SIZE > S.PDO_OUT_LEN:
|
||
break
|
||
d = self._out[off + S.FRM_OFF_DLC]
|
||
if off + S.FRM_HDR_SIZE + d > S.PDO_OUT_LEN:
|
||
break
|
||
self.rx_frames.append((
|
||
self._out[off + S.FRM_OFF_CH],
|
||
struct.unpack_from("<I", self._out, off + S.FRM_OFF_ID)[0],
|
||
d,
|
||
self._out[off + S.FRM_HDR_SIZE:
|
||
off + S.FRM_HDR_SIZE + d].hex().upper(),
|
||
))
|
||
off += S.FRM_HDR_SIZE + d
|
||
n += 1
|
||
if n:
|
||
self.tx_cycles.append(n)
|
||
self.tx_count += n
|
||
self.last_seq = seq
|
||
|
||
@property
|
||
def input(self):
|
||
b = bytearray(S.PDO_IN_LEN)
|
||
b[0] = self.last_seq or 0
|
||
struct.pack_into("<H", b, 22, self.tx_count)
|
||
win = self._out[S.TX_OFF_WIN]
|
||
b[S.MOTW_OFF_ACTIVE] = S.MOTOR_SLOT_COUNT
|
||
b[S.MOTW_OFF_WIN_BASE] = win
|
||
cnt = 0
|
||
for i in range(NSLOT):
|
||
idx = win + i
|
||
if idx >= S.MOTOR_SLOT_COUNT:
|
||
break
|
||
off = S.MOTW_OFF_SLOTS + i * SLOT
|
||
b[off] = idx // S.MOTOR_CHANNELS + 1 # node 1..9
|
||
b[off + 1] = idx % S.MOTOR_CHANNELS # ch 0..2
|
||
b[off + 2] = 1 # online
|
||
b[off + 3] = 32 # dlc
|
||
struct.pack_into("<I", b, off + 4, 1000 + idx)
|
||
struct.pack_into("<h", b, off + 8, idx * 100) # MCU_temp ×0.01
|
||
cnt += 1
|
||
b[S.MOTW_OFF_WIN_COUNT] = cnt
|
||
|
||
# 段4 批量接收区：按字节容量装，装不下的留到下周期（模拟 fillRxBatch）
|
||
off = S.RXB_OFF_ITEMS
|
||
n = 0
|
||
while self.pending_rx:
|
||
ch, cid, dlc, data = self.pending_rx[0]
|
||
if off + S.FRM_HDR_SIZE + dlc > S.PDO_IN_LEN:
|
||
break
|
||
self.pending_rx.pop(0)
|
||
b[off + S.FRM_OFF_CH] = ch
|
||
b[off + S.FRM_OFF_DLC] = dlc
|
||
b[off + S.FRM_OFF_FLAGS] = 0x01
|
||
struct.pack_into("<I", b, off + S.FRM_OFF_ID, cid)
|
||
b[off + S.FRM_HDR_SIZE:off + S.FRM_HDR_SIZE + dlc] = data[:dlc]
|
||
off += S.FRM_HDR_SIZE + dlc
|
||
n += 1
|
||
b[S.RXB_OFF_COUNT] = n
|
||
b[S.RXB_OFF_LOST] = self.rx_lost
|
||
b[S.RXB_OFF_PENDING] = len(self.pending_rx)
|
||
return bytes(b)
|
||
|
||
|
||
class SparseSlave(FakeSlave):
|
||
"""只有少数槽有主的从站 —— 真实现场的样子(27 槽容量，实际接 2 台)。
|
||
|
||
FakeSlave 把 27 个槽全填满，那正好是自适应窗口毫无收益的最坏情况，
|
||
测不出窗口浪费。这里按 `present` 集合填槽，并记录每个槽被窗口命中的次数，
|
||
用来算"单个电机每几个周期才被读到一次"。
|
||
"""
|
||
|
||
def __init__(self, present):
|
||
super().__init__()
|
||
self.present = set(present)
|
||
self.slot_hits = {} # 槽号 -> 被窗口盖住并回传的次数
|
||
|
||
@property
|
||
def input(self):
|
||
b = bytearray(super().input)
|
||
# 覆盖掉父类填的"全满"槽区，改按 present 填
|
||
win = self._out[S.TX_OFF_WIN]
|
||
for i in range(NSLOT):
|
||
off = S.MOTW_OFF_SLOTS + i * SLOT
|
||
b[off:off + SLOT] = bytes(SLOT) # 先清成空槽
|
||
cnt = 0
|
||
for i in range(NSLOT):
|
||
idx = win + i
|
||
if idx >= S.MOTOR_SLOT_COUNT:
|
||
break
|
||
cnt += 1
|
||
if idx not in self.present:
|
||
continue # 空槽: node_id 留 0
|
||
off = S.MOTW_OFF_SLOTS + i * SLOT
|
||
b[off] = idx // S.MOTOR_CHANNELS + 1
|
||
b[off + 1] = idx % S.MOTOR_CHANNELS
|
||
b[off + 2] = 1
|
||
b[off + 3] = 32
|
||
# seq 每次命中都要变，否则前端的 seq 活性判断看不出在刷新
|
||
self.slot_hits[idx] = self.slot_hits.get(idx, 0) + 1
|
||
struct.pack_into("<I", b, off + 4, self.slot_hits[idx])
|
||
struct.pack_into("<h", b, off + 8, idx * 100)
|
||
b[S.MOTW_OFF_ACTIVE] = len(self.present)
|
||
b[S.MOTW_OFF_WIN_BASE] = win
|
||
b[S.MOTW_OFF_WIN_COUNT] = cnt
|
||
return bytes(b)
|
||
|
||
|
||
class FakeMaster:
|
||
"""state/al_status 供 api_motor_start 的"核实是否真在 OP"那段读取。"""
|
||
|
||
expected_wkc = 3
|
||
|
||
def __init__(self):
|
||
self.slaves = [FakeSlave()]
|
||
|
||
def send_processdata(self):
|
||
pass
|
||
|
||
def receive_processdata(self, timeout):
|
||
return 3
|
||
|
||
def read_state(self):
|
||
return 8
|
||
|
||
|
||
def main():
|
||
S.g_master = FakeMaster()
|
||
S.g_op_active = True
|
||
slave = S.g_master.slaves[0]
|
||
|
||
# 完成 seq priming（等价于固件上电后的第一个输出周期）
|
||
slave.output = bytes(S.PDO_OUT_LEN)
|
||
|
||
print("=== 1. 单帧 CMD (DLC 32) 往返 ===")
|
||
cmd = bytes(range(32))
|
||
r = S.api_can_send({"ch": 1, "id": "0x30200", "dlc": 32, "data": cmd.hex()})
|
||
assert r["ok"] and r["acked"], r
|
||
ch, can_id, dlc, data = slave.rx_frames[-1]
|
||
print(f" 从站收到 ch={ch} id=0x{can_id:X} dlc={dlc}")
|
||
assert dlc == 32, f"DLC 被截断成 {dlc}"
|
||
assert data == cmd.hex().upper(), "32B payload 不一致"
|
||
print(" [OK] DLC32 完整送达，未被旧 24B 上限截断")
|
||
|
||
print("\n=== 2. 后台循环 + 窗口滚动扫全表 ===")
|
||
S._motor_thread_start(period_ms=2)
|
||
time.sleep(0.4)
|
||
snap = S.api_motor_state({})
|
||
assert snap["ok"], snap
|
||
motors = snap["motors"]
|
||
print(f" cycles={snap['cycles']} 聚合槽数={len(motors)}/{S.MOTOR_SLOT_COUNT}")
|
||
assert len(motors) == S.MOTOR_SLOT_COUNT, f"只扫到 {len(motors)} 槽"
|
||
nodes = sorted({m["node"] for m in motors})
|
||
chans = sorted({m["ch"] for m in motors})
|
||
print(f" 节点={nodes} 通道={chans}")
|
||
assert nodes == list(range(1, 10)) and chans == [0, 1, 2]
|
||
print(f" [OK] 开机首圈 {S.MOTOR_SCAN_CYCLES} 窗口扫完全表，node×ch 双键齐全")
|
||
|
||
print("\n=== 3. STATUS payload 完整 32B ===")
|
||
m5 = next(m for m in motors if m["slot"] == 5)
|
||
raw = bytes.fromhex(m5["data"])
|
||
temp = struct.unpack_from("<h", raw, 0)[0] * 0.01
|
||
print(f" slot5 node={m5['node']} ch={m5['ch']} len={len(raw)}B mcuTemp={temp}°C")
|
||
assert len(raw) == 32, f"STATUS 只有 {len(raw)}B"
|
||
assert abs(temp - 5.0) < 1e-6
|
||
print(" [OK] 32B STATUS 完整（旧 8B 短快照装不下，decodeStatus 会直接拒绝）")
|
||
|
||
print("\n=== 4. 命令队列经后台循环下发 ===")
|
||
before = len(slave.rx_frames)
|
||
S.api_motor_send({"frames": [
|
||
{"ch": 0, "id": "0x10200", "dlc": 32, "data": "AA" * 32},
|
||
{"ch": 1, "id": "0x20200", "dlc": 32, "data": "BB" * 32},
|
||
{"ch": 2, "id": "0x30301", "dlc": 24, "data": "CC" * 24},
|
||
]})
|
||
time.sleep(0.3)
|
||
sent = slave.rx_frames[before:]
|
||
for f in sent:
|
||
print(f" ch={f[0]} id=0x{f[1]:X} dlc={f[2]}")
|
||
assert len(sent) == 3, f"队列丢帧: 只发出 {len(sent)}/3"
|
||
assert [f[2] for f in sent] == [32, 32, 24]
|
||
print(" [OK] 3 帧全部下发，未互相覆盖")
|
||
|
||
print("\n=== 5. 循环可干净停止 ===")
|
||
S._motor_thread_stop()
|
||
assert S._motor_thread is None
|
||
print(" [OK] 线程已退出")
|
||
|
||
# ─────────── 1.5.56 批量收发 ───────────
|
||
print(f"\n=== 6. 批量发送：多帧同一周期发出 (容量 {S.TXB_BYTES}B) ===")
|
||
slave.tx_cycles.clear()
|
||
before = len(slave.rx_frames)
|
||
nine = [{"ch": i % 3, "id": f"0x{i+1:X}0200", "dlc": 32,
|
||
"data": f"{i:02X}" * 32} for i in range(9)]
|
||
S.api_motor_send({"frames": nine})
|
||
S._motor_thread_start(period_ms=2)
|
||
time.sleep(0.25)
|
||
S._motor_thread_stop()
|
||
sent = slave.rx_frames[before:]
|
||
print(f" 下发 9 帧 DLC32 -> 从站收到 {len(sent)} 帧，"
|
||
f"各 seq 沿帧数={slave.tx_cycles}")
|
||
assert len(sent) == 9, f"批量下发丢帧: {len(sent)}/9"
|
||
assert max(slave.tx_cycles) > 1, \
|
||
f"仍是每周期一帧({slave.tx_cycles})，批量区没生效"
|
||
# 关键：9 帧 DLC32 应在 1 个周期内发完(单帧区1 + 批量区8)，而非排队 9 个周期
|
||
assert len(slave.tx_cycles) <= 2, \
|
||
f"9 帧用了 {len(slave.tx_cycles)} 个周期，未达到同步下发"
|
||
ids = [f"0x{f[1]:X}" for f in sent]
|
||
print(f" 帧序 ={ids}")
|
||
assert [f[2] for f in sent] == [32] * 9, "批量帧 DLC 有截断"
|
||
print(f" [OK] 9 帧在 {len(slave.tx_cycles)} 个周期内发完（旧实现需 9 个周期）")
|
||
|
||
print(f"\n=== 7. 批量接收：一周期回收多帧 (容量 {S.RXB_BYTES}B) ===")
|
||
# 含一帧满长 DLC64，验证 CANFD 全量能过
|
||
slave.pending_rx = [
|
||
(0, 0x10100, 32, bytes(range(32))),
|
||
(1, 0x20100, 32, bytes(range(32, 64))),
|
||
(2, 0x30100, 64, bytes(range(64))), # CANFD 满长
|
||
(0, 0x40100, 8, bytes(range(8))),
|
||
]
|
||
S._motor_rx_log.clear()
|
||
S._motor_thread_start(period_ms=2)
|
||
time.sleep(0.25)
|
||
snap = S.api_motor_state({})
|
||
S._motor_thread_stop()
|
||
got = S.api_motor_rx({})
|
||
frames = got["frames"]
|
||
print(f" 回收 {len(frames)} 帧, rx_batch.count={snap['rx_batch']['count']}, "
|
||
f"pending={snap['rx_batch']['pending']}, lost={snap['rx_batch']['lost']}")
|
||
for f in frames:
|
||
print(f" ch={f['ch']} id=0x{f['id']:X} dlc={f['dlc']}")
|
||
assert len(frames) == 4, f"批量接收丢帧: {len(frames)}/4"
|
||
assert [f["dlc"] for f in frames] == [32, 32, 64, 8], "接收 DLC 有错"
|
||
fd = next(f for f in frames if f["dlc"] == 64)
|
||
assert bytes.fromhex(fd["data"]) == bytes(range(64)), "DLC64 payload 不一致"
|
||
assert got["remain"] == 0, "取走后缓冲应清空"
|
||
print(" [OK] 4 帧含 DLC64 满长 CANFD 全部完整回收")
|
||
|
||
print("\n=== 8. 兼容性：批量区为空时布局与 1.5.55 一致 ===")
|
||
S._fill_can_out(1, 0x30200, 32, bytes(range(32)))
|
||
out = bytes(S._can_out)
|
||
assert out[1] == 1 and out[2] == 32
|
||
assert struct.unpack_from("<I", out, 4)[0] == 0x30200
|
||
assert out[S.TX_OFF_DATA:S.TX_OFF_DATA + 32] == bytes(range(32))
|
||
assert out[S.TXB_OFF_COUNT] == 0, "未请求批量时 count 必须为 0"
|
||
assert all(v == 0 for v in out[S.TXB_OFF_ITEMS:]), "未请求批量时帧区必须全零"
|
||
print(f" 单帧区 [0..47] 与 1.5.55 逐字节一致，批量区 count=0 全零")
|
||
print(" [OK] 老主站/CAN 调试页行为不变")
|
||
|
||
print("\n=== 9. 窗口自适应：稀疏总线上单槽刷新率 ===")
|
||
# 真实现场是 27 槽里只有 2 个有主(9×3 是容量不是接入数)。旧实现每周期
|
||
# 窗口 +7 无脑轮扫 4 个窗口，其中 3 个整窗全是空槽，于是单个电机的状态
|
||
# 只有 周期率/4 的更新速度。这一项证明自适应窗口把它拉回"每周期都刷"。
|
||
sparse = SparseSlave({2, 5}) # node1/ch2 与 node2/ch2，与实测现场一致
|
||
S.g_master.slaves[0] = sparse
|
||
sparse.output = bytes(S.PDO_OUT_LEN) # seq priming
|
||
S._motor_snap = None
|
||
S._motor_thread_start(period_ms=2)
|
||
time.sleep(0.6)
|
||
snap = S.api_motor_state({})
|
||
S._motor_thread_stop()
|
||
|
||
cycles = snap["cycles"]
|
||
print(f" bases={snap['win_bases']} (只 1 个窗口即可盖住 slot 2 与 5)")
|
||
print(f" slot_period_ms={snap['slot_period_ms']} (= period_ms × 窗口数)")
|
||
assert len(snap["win_bases"]) == 1, \
|
||
f"稀疏总线不该用 {len(snap['win_bases'])} 个窗口"
|
||
assert snap["slot_period_ms"] == snap["period_ms"], \
|
||
"单槽刷新周期应等于 PDO 周期"
|
||
|
||
# 每个在线槽被"读到"的次数：命中率应接近 100%，而非旧实现的 ~25%
|
||
hits = sparse.slot_hits
|
||
for slot in (2, 5):
|
||
ratio = hits.get(slot, 0) / max(1, cycles)
|
||
print(f" slot {slot}: {hits.get(slot,0)}/{cycles} 周期命中 = {ratio*100:.0f}%")
|
||
assert ratio > 0.90, f"slot {slot} 命中率仅 {ratio*100:.0f}%，窗口没盖住"
|
||
# 空槽不该再白占窗口：整体扫描量应远低于旧实现的 27 槽/4 周期
|
||
print(f" [OK] 在线电机每周期都刷新（旧实现每 4 周期才 1 次 = 25%）")
|
||
|
||
print("\n=== 10. 发现扫描：新插上的电机仍能被找到 ===")
|
||
# 自适应窗口只盯"已知的槽"，必须靠周期性发现扫描找到新电机，
|
||
# 否则热插拔上来的电机永远不出现 —— 这是自适应的必要配套。
|
||
sparse2 = SparseSlave({2})
|
||
S.g_master.slaves[0] = sparse2
|
||
sparse2.output = bytes(S.PDO_OUT_LEN)
|
||
S._motor_snap = None
|
||
S._motor_thread_start(period_ms=1)
|
||
time.sleep(0.3)
|
||
sparse2.present.add(24) # 热插：node9/ch0 上线
|
||
deadline = time.time() + 5.0
|
||
found = False
|
||
while time.time() < deadline:
|
||
time.sleep(0.1)
|
||
snap = S.api_motor_state({})
|
||
if any(m["slot"] == 24 for m in (snap.get("motors") or [])):
|
||
found = True
|
||
break
|
||
S._motor_thread_stop()
|
||
print(f" 发现间隔 MOTOR_DISCOVERY_EVERY={S.MOTOR_DISCOVERY_EVERY} 周期")
|
||
assert found, "发现扫描没找到新上线的 slot 24（自适应窗口把它永久漏掉了）"
|
||
print(f" bases={snap['win_bases']} → 新槽已纳入常规覆盖")
|
||
assert any(b <= 24 < b + S.MOT_SLOTS_PER_PDO for b in snap["win_bases"]), \
|
||
"slot 24 被发现了但没纳入常规窗口，下一拍又会漏"
|
||
print(" [OK] 热插的电机能被发现并纳入每周期刷新")
|
||
|
||
print("\n=== 11. 自旋节拍源：策略 + 实际精度 ===")
|
||
# 分段实测表明一次周期的总线开销只有 ~165us，1.36ms 里 1.2ms 是
|
||
# Windows 内核睡眠的粒度超调。自旋能把周期跑准，代价是占满一个核，
|
||
# 所以"什么时候自旋"和"自旋到底准不准"都得有测试守着。
|
||
S._motor_spin = None
|
||
S._motor_period_ms = 4
|
||
assert S._spin_effective() is False, "4ms 默认不该自旋(内核睡眠够用，别白烧 CPU)"
|
||
# 显式开一次之后，下一次不带 spin 的请求必须回到"自动"，不能沿用 True。
|
||
# 早先 _motor_thread_start 写成 `if spin is not None` 才赋值，于是手动开过
|
||
# 一次自旋就永久粘住，长周期也照样烧一个核，自动阈值形同失效。
|
||
S._motor_thread_start(4, True)
|
||
assert S._spin_effective() is True, "显式 spin=True 必须生效"
|
||
S._motor_thread_stop()
|
||
S._motor_thread_start(4, None)
|
||
assert S._spin_effective() is False, \
|
||
"省略 spin 必须回到自动模式(4ms→不自旋)，不能粘住上一次的 True"
|
||
S._motor_thread_stop()
|
||
S._motor_period_ms = 2
|
||
S._motor_period_ms = 2
|
||
assert S._spin_effective() is True, "2ms 该自动自旋(内核睡眠在这个档睡不准)"
|
||
S._motor_period_ms = 1
|
||
assert S._spin_effective() is True, "1ms 该自动自旋"
|
||
S._motor_period_ms = 1
|
||
S._motor_spin = False
|
||
assert S._spin_effective() is False, "用户显式关掉就必须关(省 CPU 优先)"
|
||
S._motor_period_ms = 8
|
||
S._motor_spin = True
|
||
assert S._spin_effective() is True, "用户显式打开就必须在长周期也打开"
|
||
S._motor_spin = None
|
||
print(" [OK] 自动阈值 / 显式覆盖 都按预期")
|
||
|
||
# 真量一次精度。**只断言自旋档达标，不断言"自旋比睡眠快"** ——
|
||
# Windows 的定时器分辨率是全局且粘滞的(别的进程调过 timeBeginPeriod 就会
|
||
# 一直生效)，粗粒度时睡眠能慢到 700Hz、细粒度时睡眠也能跑到 ~1000Hz。
|
||
# 拿两者比值当判据会让这条测试随机翻脸，真正要保证的是：
|
||
# 1) 自旋档能稳定跑到设定周期(下面第 12 项再保证它不饿死别人)
|
||
S.g_master.slaves[0] = SparseSlave({2})
|
||
S.g_master.slaves[0].output = bytes(S.PDO_OUT_LEN)
|
||
def rate(spin, period=1):
|
||
S._motor_spin = spin
|
||
S._motor_period_ms = period
|
||
S._motor_snap = None
|
||
S._cyc_t.clear()
|
||
S._motor_thread_start()
|
||
time.sleep(1.0) # 先让周期稳定下来
|
||
a = S._motor_snapshot()["cycles"]; ta = time.perf_counter()
|
||
time.sleep(1.5)
|
||
b = S._motor_snapshot()["cycles"]; tb = time.perf_counter()
|
||
S._motor_thread_stop()
|
||
return (b - a) / (tb - ta)
|
||
h_sleep = rate(False)
|
||
h_spin = rate(True)
|
||
print(f" 内核睡眠: {h_sleep:6.1f} Hz 自旋: {h_spin:6.1f} Hz (目标 1000 Hz)")
|
||
print(f" (睡眠档快慢取决于本机定时器分辨率, 只作参考不作判据)")
|
||
assert h_spin > 850, f"1ms 自旋档只跑到 {h_spin:.0f}Hz，离目标太远"
|
||
print(f" [OK] 自旋稳定跑到设定周期的 {h_spin/10:.0f}%（1ms 档）")
|
||
|
||
print("\n=== 12. 自旋不得饿死其它线程(GIL) ===")
|
||
# 真机踩过的坑：第一版自旋写成 `while time.perf_counter() < d: pass`，
|
||
# 于是读快照的 HTTP p50 从 0.6ms 劣化到 40ms —— 页面直接废了。
|
||
# 原因是 CPython GIL 切换间隔默认 5ms，纯 Python 循环可以连拿好几个 5ms。
|
||
# 这一项量"自旋在跑时，别的线程被延迟多久"，是那条修复的回归守卫。
|
||
S.g_master.slaves[0] = SparseSlave({2})
|
||
S.g_master.slaves[0].output = bytes(S.PDO_OUT_LEN)
|
||
S._motor_spin = True
|
||
S._motor_period_ms = 1
|
||
S._motor_snap = None
|
||
S._cyc_t.clear()
|
||
S._motor_thread_start()
|
||
time.sleep(0.6) # 让自旋先跑起来
|
||
lat = []
|
||
t_end = time.perf_counter() + 1.5
|
||
while time.perf_counter() < t_end: # 模拟 HTTP 线程的活: 醒来干点事再睡
|
||
t0 = time.perf_counter()
|
||
time.sleep(0.0002)
|
||
lat.append((time.perf_counter() - t0) * 1000)
|
||
S._motor_thread_stop()
|
||
S._motor_spin = None
|
||
lat.sort()
|
||
p50, p90, mx = lat[len(lat)//2], lat[int(len(lat)*.9)], lat[-1]
|
||
print(f" 自旋跑 1ms 周期时，其它线程 sleep(0.2ms) 实测 "
|
||
f"p50 {p50:.2f}ms p90 {p90:.2f}ms max {mx:.2f}ms (n={len(lat)})")
|
||
# 纯 Python 自旋时 p50 约 5.7ms(=GIL 切换间隔)；放 GIL 的自旋回到 ~0.5ms 基线
|
||
assert p50 < 2.0, (
|
||
f"自旋把其它线程饿到 p50 {p50:.1f}ms —— 自旋循环一定又在持 GIL 空转，"
|
||
"必须走 _spin_until(ctypes QPC)，否则 HTTP 路由会全排在 GIL 后面")
|
||
print(" [OK] 自旋不持 GIL，HTTP 线程不受影响")
|
||
|
||
print("\n=== 13. P 核识别与钉核 ===")
|
||
# 自旋要占满一个逻辑处理器，钉到 P 核(快核组)让节拍跑在单核性能最高的核上。
|
||
# 分类逻辑单独测(不跑 1.6s 的真实探测)，用本机实测到的两组数字喂它。
|
||
def bits(m):
|
||
return [b for b in range(64) if m >> b & 1]
|
||
|
||
# 本机 i7-14700K 的真实测量值：P 核 11.9~16.7ms(含被 SMT 拖累的)，E 核 24.7~29.7ms
|
||
real = ([(lp, 13.0 + (lp % 4)) for lp in range(16)] +
|
||
[(lp, 25.0 + (lp % 5)) for lp in range(16, 28)])
|
||
got = bits(S._classify_fast(real))
|
||
assert got == list(range(16)), f"本机拓扑应识别出 P 核 0..15，实际 {got}"
|
||
print(f" [OK] 真实混合拓扑 → P 核 {got[0]}..{got[-1]}（16 个）")
|
||
|
||
# 关键回归：固定倍数阈值会把慢一点的 P 核误判成 E 核，间隙法不会 ——
|
||
# 被 SMT 拖累的 16.5ms 核必须仍留在 P 组里
|
||
smt = [(lp, 11.0 if lp < 8 else 16.5) for lp in range(16)] + \
|
||
[(lp, 28.0) for lp in range(16, 24)]
|
||
got = bits(S._classify_fast(smt))
|
||
assert got == list(range(16)), f"被 SMT 拖累的 P 核不该被踢出快核组，实际 {got}"
|
||
print(" [OK] 被 SMT 拖慢的 P 核没被误判成 E 核")
|
||
|
||
# 纯对称 CPU(全同速)必须老实承认分不出来 → 返回 0 = 不钉
|
||
flat = [(lp, 15.0 + lp * 0.01) for lp in range(8)]
|
||
assert S._classify_fast(flat) == 0, "对称 CPU 不该硬分出一个'快核'集合"
|
||
assert S._classify_fast([(0, 1.0), (1, 9.0)]) == 0, "样本太少不该下结论"
|
||
only_one_fast = [(0, 10.0)] + [(lp, 30.0) for lp in range(1, 8)]
|
||
assert S._classify_fast(only_one_fast) == 0, "快核只剩 1 个说明分错了"
|
||
only_one_slow = [(lp, 10.0) for lp in range(7)] + [(7, 30.0)]
|
||
assert S._classify_fast(only_one_slow) == 0, "慢核只剩 1 个说明分错了(离群噪声不是拓扑)"
|
||
print(" [OK] 对称/样本不足/单核离群 都安全返回不钉核")
|
||
|
||
# 钉核只影响 PDO 线程自己，不能把整个进程(含 HTTP 线程)钉死
|
||
S.g_pcore_mask = 0xF0
|
||
S.g_pcore_probe_done.set()
|
||
box = {}
|
||
def _pinned():
|
||
S.g_pcore_probe_done.wait(2.0)
|
||
if S._pcore_pin_enabled() and S.g_pcore_probe_done.is_set() and S.g_pcore_mask:
|
||
S._set_thread_affinity_mask(S.g_pcore_mask)
|
||
box["m"] = S._get_thread_affinity_mask()
|
||
th = threading.Thread(target=_pinned); th.start(); th.join(timeout=5)
|
||
assert bits(box.get("m", 0)) == [4, 5, 6, 7], f"线程亲和性没生效: {box}"
|
||
assert bits(S._get_thread_affinity_mask()) != [4, 5, 6, 7], \
|
||
"主线程亲和性被改了 —— 钉核必须是线程级的"
|
||
print(" [OK] 钉核是线程级的，HTTP 线程与主线程不受影响")
|
||
S.g_pcore_mask = 0
|
||
|
||
print("\n=== 14. api_motor_start 的 OP 状态核实 ===")
|
||
# 真机踩过的坑：给 api_motor_start 加了"读从站真实状态"的核实逻辑后，
|
||
# 函数里出现了对 g_op_active 的赋值却没声明 global —— 整个路由直接抛
|
||
# UnboundLocalError，电机页 EtherCAT 链路全挂。假从站自测原先都绕开这个
|
||
# 函数(直接调 _motor_thread_start)，所以完全没抓到，这里补上。
|
||
S.g_master.slaves[0] = SparseSlave({2})
|
||
S.g_master.slaves[0].output = bytes(S.PDO_OUT_LEN)
|
||
S.g_op_active = True
|
||
S._motor_spin = None
|
||
r = S.api_motor_start({"period_ms": 2})
|
||
assert r["ok"], f"已在 OP 时重复 start 不该失败: {r.get('log')}"
|
||
assert r["spin"] is True, "2ms 档应自动自旋"
|
||
print(f" [OK] 已在 OP 时 start 正常（spin={r['spin']}，无 UnboundLocalError）")
|
||
S._motor_thread_stop()
|
||
|
||
# 从站被看门狗踢出 OP 后，g_op_active 是过期缓存；必须现读状态发现并走重进，
|
||
# 否则新循环每拍都撞"未进 OP"，界面绿着却全是 --。
|
||
# 这里只验证"检测到 + 走了重进流程"，不验证重进成功 —— 完整 OP 序列要
|
||
# config_init/config_map/config_dc/state_check/write_state/sdo_read/
|
||
# set_watchdog/dc_sync/_fprd 一整套，假从站撑不起来，硬凑会变成在测桩本身。
|
||
S.g_op_active = True
|
||
S.g_master.slaves[0].state = 4 # SAFEOP
|
||
r = S.api_motor_start({"period_ms": 4})
|
||
joined = " / ".join(r.get("log") or [])
|
||
assert "掉出 OP" in joined, f"没检测到从站已掉出 OP，仍拿过期标志空转: {joined[:160]}"
|
||
assert not r["ok"], "重进失败时必须如实报错，不能假装启动成功"
|
||
print(" [OK] 检测到从站掉出 OP 并走重进流程（不拿过期标志空转）")
|
||
S.g_master.slaves[0].state = 8
|
||
S._motor_thread_stop()
|
||
S.g_op_active = True
|
||
S._motor_spin = None
|
||
|
||
print("\n=== 15. 亚毫秒周期 + /api/motor/period 热调 ===")
|
||
# 需求：内核级要能跑 250us(4kHz)，且周期随时可调。守两条：
|
||
# a) period 允许亚毫秒 float，并按 [MIN,MAX] 钳位；
|
||
# b) 热调路由**不停循环**、也不进总线互斥表 —— 停循环 = 掐断喂狗，
|
||
# 正是把真机板子弄楔死的那条路(见 api_motor_spin 注释)。
|
||
assert "/api/motor/period" not in S._BUS_EXCLUSIVE_PATHS, \
|
||
"周期热调绝不能进总线互斥表：dispatcher 停循环会掐断喂狗"
|
||
|
||
assert S._parse_period_ms({"period_ms": 0.25}) == 0.25
|
||
assert S._parse_period_ms({"period_us": 500}) == 0.5
|
||
assert S._parse_period_ms({"period_ms": 0.0001}) == S.MOTOR_PERIOD_MIN_MS
|
||
assert S._parse_period_ms({"period_ms": 5000}) == S.MOTOR_PERIOD_MAX_MS
|
||
assert S._parse_period_ms({"period_ms": 0}) is None
|
||
assert S._parse_period_ms({"period_ms": float("nan")}) is None
|
||
assert S._parse_period_ms({}) is None
|
||
print(" [OK] period_ms/period_us 解析与钳位(0/NaN/缺省视为没给)")
|
||
|
||
# 热调速测：假从站的收发是纯内存操作(µs 级)，这里量的是
|
||
# "自旋+float 周期"这套机制能不能把循环真正驱动到 4kHz 档，
|
||
# 不含真机 ~165us 的总线时间(那部分由 rate_probe 在板上量)。
|
||
S.g_master.slaves[0] = SparseSlave({2, 5})
|
||
S.g_master.slaves[0].output = bytes(S.PDO_OUT_LEN)
|
||
S._motor_spin = None
|
||
S._motor_thread_start(period_ms=4)
|
||
time.sleep(0.15)
|
||
r = S.api_motor_period({"period_ms": 0.25})
|
||
assert r["ok"] and r["period_ms"] == 0.25, r
|
||
assert r["spin"] is True, "0.25ms 周期在自动档下必须自旋(睡眠粒度睡不出)"
|
||
assert S._motor_thread.is_alive(), "热调周期把循环弄停了"
|
||
|
||
def _rate(sample_s=1.0):
|
||
t0 = time.perf_counter()
|
||
c0 = (S._motor_snapshot() or {}).get("cycles", 0)
|
||
time.sleep(sample_s)
|
||
c1 = (S._motor_snapshot() or {}).get("cycles", 0)
|
||
return (c1 - c0) / (time.perf_counter() - t0)
|
||
|
||
hz = _rate(1.0)
|
||
assert hz > 3000, f"0.25ms 周期只跑出 {hz:.0f}Hz，机制没到 4kHz 档"
|
||
snap = S._motor_snapshot()
|
||
assert snap["period_ms"] == 0.25 and snap["slot_period_ms"] == 0.25, snap
|
||
print(f" [OK] 热调 4ms→0.25ms 循环不停，实测 {hz:.0f}Hz(假从站 ≥3000)")
|
||
|
||
# 强制睡眠 + 亚毫秒周期 → 必须明确警告(睡眠粒度 > 目标周期，跑不到)
|
||
S._motor_spin = False
|
||
r = S.api_motor_period({"period_us": 250})
|
||
assert r["ok"] and r["spin"] is False
|
||
assert "内核睡眠" in " ".join(r["log"]), f"该警告的没警告: {r['log']}"
|
||
S._motor_spin = None
|
||
|
||
# 换回长周期：自动档应退回内核睡眠(不白烧一个核)
|
||
r = S.api_motor_period({"period_ms": 4})
|
||
assert r["ok"] and r["spin"] is False
|
||
assert S._motor_thread.is_alive()
|
||
print(" [OK] 换回 4ms 自动退回内核睡眠，循环全程没停过")
|
||
S._motor_thread_stop()
|
||
|
||
# float 周期也要能从 motor/start 一路传进来(前端首启路径)
|
||
S._motor_spin = None
|
||
assert S._motor_thread_start(period_ms=0.5) is True
|
||
assert S._motor_period_ms == 0.5
|
||
S._motor_thread_stop()
|
||
S._motor_period_ms = 4
|
||
S._motor_spin = None
|
||
print(" [OK] motor/start 也收 float 周期(0.5ms)")
|
||
|
||
print("\n=== 16. 真机总线耗时下的网格收敛（4kHz 保底的忠实代理）===")
|
||
# 第 15 项的假从站 send/receive 是 µs 级内存桩，**没建模真机 165µs 总线时间**，
|
||
# 所以它证明不了"166us 工作 + 250us 网格"这个真机核心交互能不能收敛到 4000Hz。
|
||
# 这里给收发注入按真机分段实测拆开的耗时(send 48us + receive 118us = 166us)，
|
||
# 且用 _spin_until 释放 GIL —— 和真机 pysoem 在 Npcap 调用期间放 GIL 的行为一致。
|
||
# 这是不接硬件能产出的最强预测：工作 166us < 周期 250us，网格不该丢拍，
|
||
# 平均率应收敛到 ~4000Hz；若收敛不了，说明缺口是结构性的，得上真机前就预警。
|
||
BUS_SEND_US, BUS_RX_US = 48.0, 118.0
|
||
|
||
class BusCostMaster(FakeMaster):
|
||
def send_processdata(self):
|
||
S._spin_until(time.perf_counter() + BUS_SEND_US / 1e6)
|
||
|
||
def receive_processdata(self, timeout):
|
||
S._spin_until(time.perf_counter() + BUS_RX_US / 1e6)
|
||
return 3
|
||
|
||
S.g_master = BusCostMaster()
|
||
S.g_master.slaves[0] = SparseSlave({2, 5})
|
||
S.g_master.slaves[0].output = bytes(S.PDO_OUT_LEN)
|
||
S._motor_spin = None
|
||
S._motor_thread_start(period_ms=0.25)
|
||
time.sleep(0.2)
|
||
|
||
def _rate(sample_s):
|
||
t0 = time.perf_counter()
|
||
c0 = (S._motor_snapshot() or {}).get("cycles", 0)
|
||
time.sleep(sample_s)
|
||
c1 = (S._motor_snapshot() or {}).get("cycles", 0)
|
||
return (c1 - c0) / (time.perf_counter() - t0)
|
||
|
||
hz = _rate(1.5)
|
||
# 分位数改读侧现算(_cyc_fresh)，从 api_motor_state 拿顺带验证这条路径
|
||
cyc = S.api_motor_state({}).get("cyc_us") or {}
|
||
tot = (cyc.get("total") or {}).get("p50")
|
||
print(f" 166us 总线负载下 250us 网格实测 {hz:.0f}Hz (cyc_us.total p50={tot}us)")
|
||
# 工作 166us 远小于 250us 周期，网格不该丢拍 → 应收敛到接近 4000。
|
||
# 阈值取 3700 留 GIL/调度噪声余量；真机若明显低于这个值才需要动 send/receive 合并。
|
||
assert hz > 3700, \
|
||
f"166us 工作负载下网格只收敛到 {hz:.0f}Hz，缺口是结构性的，真机大概率也到不了 4kHz"
|
||
print(" [OK] 真机同等总线负载下网格仍收敛到 ~4kHz → 保底 4kHz 的强预测")
|
||
# 防回归：分位数排序一旦爬回 PDO 循环，每第 8 拍必超时(真机实测 86.6%)。
|
||
# 用源码级检查钉死，比任何数值断言都稳。
|
||
import inspect
|
||
assert "_cyc_stats(" not in inspect.getsource(S._pdo_worker), \
|
||
"_cyc_stats 又回到 PDO 热循环了 —— 必须走读侧 _cyc_fresh"
|
||
S._motor_thread_stop()
|
||
S.g_master = FakeMaster()
|
||
S._motor_period_ms = 4
|
||
S._motor_spin = None
|
||
|
||
print("\n全部通过 ✅")
|
||
return 0
|
||
|
||
|
||
if __name__ == "__main__":
|
||
sys.exit(main())
|