280 lines
11 KiB
Python
280 lines
11 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 完全一致
|
|||
|
|
|
|||
|
|
跑法: python test_motor_backend.py
|
|||
|
|
"""
|
|||
|
|
import struct
|
|||
|
|
import sys
|
|||
|
|
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 的语义模拟从站侧。"""
|
|||
|
|
|
|||
|
|
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 FakeMaster:
|
|||
|
|
expected_wkc = 3
|
|||
|
|
|
|||
|
|
def __init__(self):
|
|||
|
|
self.slaves = [FakeSlave()]
|
|||
|
|
|
|||
|
|
def send_processdata(self):
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
def receive_processdata(self, timeout):
|
|||
|
|
return 3
|
|||
|
|
|
|||
|
|
|
|||
|
|
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全部通过 ✅")
|
|||
|
|
return 0
|
|||
|
|
|
|||
|
|
|
|||
|
|
if __name__ == "__main__":
|
|||
|
|
sys.exit(main())
|