351 lines
14 KiB
Python
351 lines
14 KiB
Python
#!/usr/bin/env python3
|
||
"""车端 CAN 报文模拟器 / 上行抓包器（联调用，非工程代码）。
|
||
|
||
依据《洗消机器人遥控器通信协议20260810.xlsx》sheet RC（分类一/二/三，全部扩展帧、Intel 小端位序）。
|
||
线格式：32 位帧头 0x5AA5FF00 + 32 位 canId + DLC(8) + 8 字节数据 + 校验和，共 18 字节，
|
||
多字节字段按小端排列，与 comm/udpcommunication.cpp 的 UdpCanFrame 一致。
|
||
|
||
用法示例：
|
||
# 向 UI 的 9001 端口周期发 VCU 反馈，看底盘页电量/电压/车速刷新
|
||
python can_sender.py --soc 85 --volt 360 --speed 4.5
|
||
|
||
# 制造故障：MCU 码 61（电机缺相故障）+ 整系统通信异常（底盘BMS）
|
||
python can_sender.py --mcu-fault 61 --comm-fault 2
|
||
|
||
# 洗消 + 气体检测仪反馈
|
||
python can_sender.py --wash disinfect --level-a 88 --press 0.3 --gas-conc 250 --gas-type 2 --gas-level 2
|
||
|
||
# 反向：绑 9000 抓 UI 下发的控制指令并解出来
|
||
python can_sender.py --listen
|
||
"""
|
||
|
||
import argparse
|
||
import socket
|
||
import struct
|
||
import time
|
||
|
||
HDR = 0x5AA5FF00
|
||
|
||
# 分类一 遥控器控制指令（上行，UI → 车端）
|
||
ID_CHASSIS_CMD = 0x18208081
|
||
ID_ARM_CMD1 = 0x18218081
|
||
ID_ARM_CMD2 = 0x18228081
|
||
ID_DECONTAM_CMD = 0x18238081
|
||
ID_DECONTAM_DBG = 0x18248081
|
||
ID_DETECTOR_CMD = 0x18258081
|
||
ID_EMERGENCY = 0x18268081
|
||
|
||
# 分类二/三 VCU 状态反馈（下行，车端 → UI）
|
||
ID_VEHICLE_FB1 = 0x18308180
|
||
ID_VEHICLE_FB2 = 0x18318180
|
||
ID_ARM_FB1 = 0x18328180
|
||
ID_ARM_FB2 = 0x18338180
|
||
ID_ARM_FB3 = 0x18348180
|
||
ID_ARM_FB4 = 0x18358180
|
||
ID_DECONTAM_FB1 = 0x18368180
|
||
ID_DECONTAM_FB2 = 0x18378180
|
||
ID_DETECTOR_FB1 = 0x18388180
|
||
ID_DETECTOR_FB2 = 0x18398180
|
||
ID_VEHICLE_FB3 = 0x183A8180
|
||
ID_VEHICLE_FB4 = 0x183B8180
|
||
|
||
NAMES = {
|
||
ID_CHASSIS_CMD: "Chassis_ControlCmd", ID_ARM_CMD1: "Arm_ControlCmd1",
|
||
ID_ARM_CMD2: "Arm_ControlCmd2", ID_DECONTAM_CMD: "Decontam_ControlCmd",
|
||
ID_DECONTAM_DBG: "Decontam_DebugCmd", ID_DETECTOR_CMD: "Detector_ControlCmd",
|
||
ID_EMERGENCY: "Emergency_Cmd", ID_VEHICLE_FB1: "Vehicle_StatusFeedBack1",
|
||
ID_VEHICLE_FB2: "Vehicle_StatusFeedBack2", ID_ARM_FB1: "Arm_StatusFeedBack1",
|
||
ID_ARM_FB2: "Arm_StatusFeedBack2", ID_ARM_FB3: "Arm_StatusFeedBack3",
|
||
ID_ARM_FB4: "Arm_StatusFeedBack4", ID_DECONTAM_FB1: "Decontam_StatusFeedBack1",
|
||
ID_DECONTAM_FB2: "Decontam_StatusFeedBack2", ID_DETECTOR_FB1: "Detector_StatusFeedBack1",
|
||
ID_DETECTOR_FB2: "Detector_StatusFeedBack2", ID_VEHICLE_FB3: "Vehicle_StatusFeedBack3",
|
||
ID_VEHICLE_FB4: "Vehicle_StatusFeedBack4",
|
||
}
|
||
|
||
|
||
def set_bits(d, start, ln, v):
|
||
"""Intel（小端）：起始位即最低位序号，位序由低到高，可跨字节。"""
|
||
v = int(v)
|
||
for i in range(ln):
|
||
b = start + i
|
||
m = 1 << (b & 7)
|
||
if v & (1 << i):
|
||
d[b >> 3] |= m
|
||
else:
|
||
d[b >> 3] &= ~m & 0xFF
|
||
|
||
|
||
def get_bits(d, start, ln):
|
||
v = 0
|
||
for i in range(ln):
|
||
b = start + i
|
||
if d[b >> 3] & (1 << (b & 7)):
|
||
v |= 1 << i
|
||
return v
|
||
|
||
|
||
def get_signed(d, start, ln):
|
||
raw = get_bits(d, start, ln)
|
||
sign = 1 << (ln - 1)
|
||
return (raw ^ sign) - sign
|
||
|
||
|
||
def checksum(can_id, dlc, d):
|
||
return (sum((can_id >> (8 * i)) & 0xFF for i in range(4)) + dlc + sum(d)) & 0xFF
|
||
|
||
|
||
def pack(can_id, d):
|
||
return struct.pack("<IIB8BB", HDR, can_id, 8, *d, checksum(can_id, 8, d))
|
||
|
||
|
||
def clamp(v, lo, hi):
|
||
return max(lo, min(hi, int(v)))
|
||
|
||
|
||
# ===================== 下行：构造分类二/三 反馈帧 =====================
|
||
def fb_vehicle1(a):
|
||
d = bytearray(8)
|
||
set_bits(d, 0, 16, clamp(round(a.volt * 10), 0, 0xFFFF)) # Total_voltage 精度 0.1
|
||
set_bits(d, 16, 16, clamp(round(a.soc * 10), 0, 0xFFFF)) # SOC 精度 0.1
|
||
set_bits(d, 32, 8, a.mcu_fault) # MCU_FaultCode
|
||
set_bits(d, 40, 2, a.mcu_level) # MCU_FaultLevel
|
||
set_bits(d, 45, 2, a.celllow_level) # UnderOver_FaultLevel
|
||
set_bits(d, 47, 2, a.cellhigh_level) # CellOver_FaultLevel（跨字节）
|
||
return d
|
||
|
||
|
||
def fb_vehicle2(a):
|
||
d = bytearray(8)
|
||
set_bits(d, 22, 2, 1 if a.handbrake else 0) # Handbrake
|
||
# Vehi_spd 16 位 0~500 × 0.01；其后字段整体后移 8 位
|
||
set_bits(d, 24, 16, clamp(round(a.speed * 100), 0, 500)) # Vehi_spd
|
||
set_bits(d, 48, 8, a.comm_fault) # All_Comm_FaultCode
|
||
return d
|
||
|
||
|
||
def fb_arm1(a):
|
||
d = bytearray(8)
|
||
set_bits(d, 0, 16, a.posx & 0xFFFF) # End_positionX (Signed mm)
|
||
set_bits(d, 16, 16, a.posy & 0xFFFF)
|
||
set_bits(d, 32, 16, a.posz & 0xFFFF)
|
||
return d
|
||
|
||
|
||
def fb_arm2(a):
|
||
d = bytearray(8)
|
||
set_bits(d, 0, 16, a.rx & 0xFFFF)
|
||
set_bits(d, 16, 16, a.ry & 0xFFFF)
|
||
set_bits(d, 32, 16, a.rz & 0xFFFF)
|
||
set_bits(d, 48, 8, clamp(a.endspd, -128, 127) & 0xFF) # End_spd (Signed)
|
||
return d
|
||
|
||
|
||
def fb_arm3(a):
|
||
d = bytearray(8)
|
||
for i in range(4):
|
||
set_bits(d, i * 16, 16, getattr(a, "j%d" % (i + 1)) & 0xFFFF)
|
||
return d
|
||
|
||
|
||
def fb_arm4(a):
|
||
d = bytearray(8)
|
||
set_bits(d, 0, 16, a.j5 & 0xFFFF)
|
||
set_bits(d, 16, 16, a.j6 & 0xFFFF)
|
||
return d
|
||
|
||
|
||
def fb_decontam1(a):
|
||
d = bytearray(8)
|
||
set_bits(d, 0, 2, a.wash_mode) # 2=消除 3=消毒
|
||
set_bits(d, 2, 1, 1 if a.wash_start else 0)
|
||
set_bits(d, 3, 1, 1 if a.plunger else 0)
|
||
set_bits(d, 4, 1, 1 if a.air else 0)
|
||
set_bits(d, 5, 1, 1 if a.leaf else 0)
|
||
set_bits(d, 8, 1, 1 if a.d1open else 0) # MBV_D1_Sta
|
||
set_bits(d, 9, 1, 1 if a.d2open else 0)
|
||
set_bits(d, 10, 1, 1 if a.sys_fault else 0)
|
||
set_bits(d, 16, 8, clamp(a.level_a, 0, 255))
|
||
set_bits(d, 24, 8, clamp(a.level_foam, 0, 255))
|
||
set_bits(d, 32, 8, clamp(a.flow_a, 0, 255))
|
||
set_bits(d, 40, 8, clamp(a.flow_b, 0, 255))
|
||
set_bits(d, 48, 8, clamp(a.air_flow_fb, 0, 255))
|
||
set_bits(d, 56, 8, clamp(a.expansion, 0, 255))
|
||
return d
|
||
|
||
|
||
def fb_decontam2(a):
|
||
d = bytearray(8)
|
||
set_bits(d, 0, 8, clamp(round(a.press * 10), 0, 255)) # Pipe_pressure 精度 0.1
|
||
set_bits(d, 8, 8, clamp(round(a.air_press * 10), 0, 255)) # AirComp_pressure
|
||
set_bits(d, 48, 8, a.air_fault) # AirComp_FaultCode
|
||
set_bits(d, 56, 8, a.plunger_fault) # PlungerPump_FaultCode
|
||
return d
|
||
|
||
|
||
def fb_detector1(a):
|
||
d = bytearray(8)
|
||
set_bits(d, 0, 8, a.detector_sta)
|
||
set_bits(d, 8, 8, a.gas_type) # Toxic_types 位组合
|
||
set_bits(d, 16, 8, a.gas_level)
|
||
set_bits(d, 24, 8, a.warn)
|
||
return d
|
||
|
||
|
||
def fb_detector2(a):
|
||
d = bytearray(8)
|
||
set_bits(d, 0, 40, clamp(a.gas_conc, 0, (1 << 40) - 1)) # Toxic_concentration 40 位
|
||
return d
|
||
|
||
|
||
FEEDBACK = [
|
||
(ID_VEHICLE_FB1, fb_vehicle1), (ID_VEHICLE_FB2, fb_vehicle2),
|
||
(ID_ARM_FB1, fb_arm1), (ID_ARM_FB2, fb_arm2),
|
||
(ID_ARM_FB3, fb_arm3), (ID_ARM_FB4, fb_arm4),
|
||
(ID_DECONTAM_FB1, fb_decontam1), (ID_DECONTAM_FB2, fb_decontam2),
|
||
(ID_DETECTOR_FB1, fb_detector1), (ID_DETECTOR_FB2, fb_detector2),
|
||
]
|
||
|
||
|
||
# ===================== 上行：解析分类一 控制帧 =====================
|
||
def describe(can_id, d):
|
||
if can_id == ID_CHASSIS_CMD:
|
||
return ("Chassis 模式Cmd=%d 使能=%d gear=%d(0后/2048中/4096前) "
|
||
"目标速度=%.2fkm/h turning=%d(0左/2048中/4096右)"
|
||
% (get_bits(d, 0, 1), get_bits(d, 3, 1), get_bits(d, 8, 16),
|
||
get_bits(d, 24, 16) * 0.01, get_bits(d, 40, 16)))
|
||
if can_id == ID_ARM_CMD1:
|
||
return ("Arm1 模式=%d 使能=%d 上下=%d 左右=%d 前后=%d"
|
||
% (get_bits(d, 0, 1), get_bits(d, 1, 1), get_bits(d, 8, 16),
|
||
get_bits(d, 24, 16), get_bits(d, 40, 16)))
|
||
if can_id == ID_ARM_CMD2:
|
||
return ("Arm2 横滚=%d 俯仰=%d 偏航=%d 回位/暂停=0x%02X(0xAA回位/0x55暂停)"
|
||
% (get_bits(d, 0, 16), get_bits(d, 16, 16), get_bits(d, 32, 16),
|
||
get_bits(d, 48, 8)))
|
||
if can_id == ID_DECONTAM_CMD:
|
||
return ("Decontam 模式=%d(2消除/3消毒) 启停=%d 发泡=%d 泡沫比例=%d "
|
||
"空压流量=%d 柱塞流量=%d"
|
||
% (get_bits(d, 0, 2), get_bits(d, 2, 1), get_bits(d, 8, 8),
|
||
get_bits(d, 16, 8), get_bits(d, 24, 8), get_bits(d, 32, 8)))
|
||
if can_id == ID_DECONTAM_DBG:
|
||
return ("DecontamDbg 调试=%d 柱塞上电=%d 空压上电=%d 叶片上电=%d D1=%d D2=%d "
|
||
"空压rpm=%d 柱塞rpm=%d 叶片rpm=%d 启动=%d/%d/%d"
|
||
% (get_bits(d, 0, 1), get_bits(d, 1, 1), get_bits(d, 2, 1), get_bits(d, 3, 1),
|
||
get_bits(d, 4, 1), get_bits(d, 5, 1), get_bits(d, 8, 16),
|
||
get_bits(d, 24, 16), get_bits(d, 40, 16),
|
||
get_bits(d, 56, 1), get_bits(d, 57, 1), get_bits(d, 58, 1)))
|
||
if can_id == ID_DETECTOR_CMD:
|
||
return "Detector 指令=0x%02X(0xAA启动/0x55停止)" % get_bits(d, 0, 8)
|
||
if can_id == ID_EMERGENCY:
|
||
return "Emergency 急停=0x%02X(0x55急停/0正常)" % get_bits(d, 0, 8)
|
||
return None
|
||
|
||
|
||
def listen(host, port, once):
|
||
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||
sock.bind((host, port))
|
||
print("监听 %s:%d，解 UI 下发的控制帧（Ctrl-C 退出）" % (host, port))
|
||
last = {}
|
||
try:
|
||
while True:
|
||
data, addr = sock.recvfrom(4096)
|
||
off = 0
|
||
while off + 18 <= len(data):
|
||
hdr, cid, dlc = struct.unpack_from("<IIB", data, off)
|
||
if hdr == HDR and dlc == 8:
|
||
body = data[off + 9:off + 17]
|
||
cks = data[off + 17]
|
||
if cks == checksum(cid, dlc, body):
|
||
txt = describe(cid, body)
|
||
if txt:
|
||
# 周期 50ms，只在内容变化时打印，避免刷屏
|
||
if once is None or last.get(cid) != body:
|
||
last[cid] = bytes(body)
|
||
print("%-16s %s" % (NAMES.get(cid, hex(cid)), txt))
|
||
off += 18
|
||
continue
|
||
off += 1
|
||
except KeyboardInterrupt:
|
||
pass
|
||
|
||
|
||
def main():
|
||
p = argparse.ArgumentParser()
|
||
p.add_argument("--host", default="127.0.0.1")
|
||
p.add_argument("--port", type=int, default=9001, help="发送目标（UI 的 net/recv_port）")
|
||
p.add_argument("--listen", action="store_true", help="改为绑端口抓 UI 上行控制帧")
|
||
p.add_argument("--hz", type=float, default=10.0, help="反馈帧发送频率，默认 10Hz")
|
||
p.add_argument("--count", type=int, default=0, help="0=一直发")
|
||
|
||
# 电池 / 底盘
|
||
p.add_argument("--soc", type=float, default=85.0, help="电量 %")
|
||
p.add_argument("--volt", type=float, default=360.0, help="总电压 V（精度 0.1）")
|
||
p.add_argument("--speed", type=float, default=3.45, help="km/h，精度 0.01")
|
||
p.add_argument("--no-handbrake", action="store_true")
|
||
p.add_argument("--mcu-fault", type=int, default=0)
|
||
p.add_argument("--mcu-level", type=int, default=0)
|
||
p.add_argument("--celllow-level", type=int, default=0)
|
||
p.add_argument("--cellhigh-level", type=int, default=0)
|
||
p.add_argument("--comm-fault", type=int, default=0, help="整系统通信故障码 0~8")
|
||
# 机械臂
|
||
p.add_argument("--posx", type=int, default=1200)
|
||
p.add_argument("--posy", type=int, default=300)
|
||
p.add_argument("--posz", type=int, default=800)
|
||
p.add_argument("--rx", type=int, default=0)
|
||
p.add_argument("--ry", type=int, default=0)
|
||
p.add_argument("--rz", type=int, default=0)
|
||
p.add_argument("--endspd", type=int, default=0)
|
||
for i in range(1, 7):
|
||
p.add_argument("--j%d" % i, type=int, default=[30, 20, 0, 90, 45, 0][i - 1])
|
||
# 洗消
|
||
p.add_argument("--wash", choices=["clean", "disinfect"], default="clean")
|
||
p.add_argument("--wash-start", action="store_true")
|
||
p.add_argument("--plunger", action="store_true")
|
||
p.add_argument("--air", action="store_true")
|
||
p.add_argument("--leaf", action="store_true")
|
||
p.add_argument("--d1open", action="store_true")
|
||
p.add_argument("--d2open", action="store_true")
|
||
p.add_argument("--sys-fault", action="store_true")
|
||
p.add_argument("--level-a", type=float, default=100)
|
||
p.add_argument("--level-foam", type=float, default=30)
|
||
p.add_argument("--flow-a", type=float, default=20)
|
||
p.add_argument("--flow-b", type=float, default=0.5)
|
||
p.add_argument("--air-flow-fb", type=float, default=100)
|
||
p.add_argument("--expansion", type=float, default=6)
|
||
p.add_argument("--press", type=float, default=0.3, help="MPa，精度 0.1")
|
||
p.add_argument("--air-press", type=float, default=0.35)
|
||
p.add_argument("--air-fault", type=int, default=0)
|
||
p.add_argument("--plunger-fault", type=int, default=0)
|
||
# 气体检测仪
|
||
p.add_argument("--detector-sta", type=int, default=4, help="4=检测中")
|
||
p.add_argument("--gas-type", type=int, default=2, help="位组合 1=P 2=S 4=HNO 8=CH 16=AS")
|
||
p.add_argument("--gas-level", type=int, default=2)
|
||
p.add_argument("--warn", type=int, default=0, help="1=进气嘴未接 2=氢气不足 4=仪器故障 8=浓度过高")
|
||
p.add_argument("--gas-conc", type=float, default=250.0, help="ppb")
|
||
a = p.parse_args()
|
||
a.wash_mode = 3 if a.wash == "disinfect" else 2
|
||
a.handbrake = not a.no_handbrake
|
||
|
||
if a.listen:
|
||
listen(a.host, a.port, None)
|
||
return
|
||
|
||
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||
period = 1.0 / a.hz
|
||
print("→ %s:%d 反馈帧 %d 条 @ %gHz 电量=%.1f%% 电压=%.1fV 车速=%.2fkm/h 洗消=%s"
|
||
% (a.host, a.port, len(FEEDBACK), a.hz, a.soc, a.volt, a.speed, a.wash))
|
||
sent = 0
|
||
try:
|
||
while a.count == 0 or sent < a.count:
|
||
for can_id, build in FEEDBACK:
|
||
sock.sendto(pack(can_id, build(a)), (a.host, a.port))
|
||
sent += 1
|
||
time.sleep(period)
|
||
except KeyboardInterrupt:
|
||
pass
|
||
print("已发送 %d 轮（每轮 %d 帧）" % (sent, len(FEEDBACK)))
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|