393 lines
17 KiB
Python
393 lines
17 KiB
Python
#!/usr/bin/env python3
|
||
"""车端 CAN 报文模拟器 / 上行抓包器（联调用，非工程代码）。
|
||
|
||
依据《洗消机器人遥控器通信协议20260810.xlsx》sheet RC（分类一/二/三，全部扩展帧、Intel 小端位序）。
|
||
线格式：帧头 0xAA + 32 位 canId + DLC(8) + 8 字节数据 + 校验和 + 帧尾 0xBB，共 16 字节，
|
||
多字节字段按小端排列，与 comm/car.cpp 的 CanFrame 一致。
|
||
|
||
用法示例：
|
||
# 向 UI 的 9001 端口周期发 VCU 反馈，看底盘页电量/电压/车速刷新
|
||
python can_sender.py --soc 85 --volt 360 --speed 2.0
|
||
|
||
# 制造故障：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 = 0xAA # 帧头
|
||
TAIL = 0xBB # 帧尾
|
||
|
||
# 分类一 遥控器控制指令（上行，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):
|
||
# 帧头 + canId 四字节 + DLC + 数据八字节 + 帧尾 求和取低 8 位
|
||
return (HDR + sum((can_id >> (8 * i)) & 0xFF for i in range(4))
|
||
+ dlc + sum(d) + TAIL) & 0xFF
|
||
|
||
|
||
def pack(can_id, d):
|
||
return struct.pack("<BI B 8B B B", HDR, can_id, 8, *d,
|
||
checksum(can_id, 8, d), TAIL)
|
||
|
||
|
||
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 协议表为 8 位 × 0.01（上限 2.55 km/h），与 comm/car.cpp 的读法一致；
|
||
# 早先按 16 位写、并把后续字段整体后移 8 位，会让 rdyStatus/通信故障码错位。
|
||
set_bits(d, 24, 8, clamp(round(a.speed * 100), 0, 255)) # Vehi_spd
|
||
set_bits(d, 32, 2, a.rdy_status) # Vehi_RdyStatus
|
||
set_bits(d, 40, 8, a.comm_fault) # All_Comm_FaultCode
|
||
return d
|
||
|
||
|
||
def fb_vehicle3(a):
|
||
# 底盘状态3：46 个 EPS/制动故障位 + EPS_FaultCode，全 0 即无故障
|
||
d = bytearray(8)
|
||
set_bits(d, 48, 8, a.eps_fault) # EPS_FaultCode
|
||
return d
|
||
|
||
|
||
def fb_vehicle4(a):
|
||
d = bytearray(8)
|
||
set_bits(d, 0, 2, a.gear) # 0=N挡 1=D挡 2=R挡
|
||
set_bits(d, 2, 2, a.parking) # 0未驻车 1左电机 2右电机 3完全驻车
|
||
set_bits(d, 4, 2, a.awd) # 0二驱 1四驱 2四驱锁定
|
||
set_bits(d, 8, 16, a.steer) # Steering_Angle
|
||
# ALIGN 是状态字节：0x11=对中失败、0xEE/0x55=成功。给 0 会被判成“方向盘未对中”
|
||
set_bits(d, 24, 8, a.align) # Angle_alignment_status
|
||
set_bits(d, 32, 8, a.brake_r) # Brake_Right_Mot_fault
|
||
set_bits(d, 40, 8, a.brake_l) # Brake_Left_Mot_fault
|
||
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, 6, 1, 1 if a.level_a_low else 0) # A_LiquidLevel_Low
|
||
set_bits(d, 7, 1, 1 if a.level_b_low else 0) # B_LiquidLevel_Low
|
||
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_VEHICLE_FB3, fb_vehicle3), (ID_VEHICLE_FB4, fb_vehicle4),
|
||
(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 + 16 <= len(data):
|
||
hdr, cid, dlc = struct.unpack_from("<BIB", data, off)
|
||
if hdr == HDR and dlc == 8 and data[off + 15] == TAIL:
|
||
body = data[off + 6:off + 14]
|
||
cks = data[off + 14]
|
||
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 += 16
|
||
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=9000,
|
||
help="发送目标端口 = UI 的 kCanRxPort（默认 9000）；--listen 抓上行时改 3001")
|
||
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=2.0, help="km/h，精度 0.01，协议只给 8 位（上限 2.55）")
|
||
p.add_argument("--no-handbrake", action="store_true")
|
||
p.add_argument("--rdy-status", type=int, default=2, help="底盘 ready：0初始/1上电完成/2就绪")
|
||
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")
|
||
# 底盘状态3/4
|
||
p.add_argument("--eps-fault", type=int, default=0, help="EPS 故障码，0=无")
|
||
p.add_argument("--gear", type=int, default=0, help="0=N挡 1=D挡 2=R挡")
|
||
p.add_argument("--parking", type=int, default=0, help="0未驻车 1左电机 2右电机 3完全驻车")
|
||
p.add_argument("--awd", type=int, default=0, help="0二驱 1四驱 2四驱锁定")
|
||
p.add_argument("--steer", type=int, default=0, help="方向盘角度")
|
||
p.add_argument("--align", type=int, default=0x55, help="0x11对中失败 / 0xEE、0x55成功；别给 0")
|
||
p.add_argument("--brake-r", type=int, default=0, help="右制动电机故障码")
|
||
p.add_argument("--brake-l", type=int, default=0, help="左制动电机故障码")
|
||
# 机械臂
|
||
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-low", action="store_true", help="A组分/清水液位低 → 首页液位行红闪")
|
||
p.add_argument("--level-b-low", action="store_true", help="B组分/泡沫液位低 → 首页液位行红闪")
|
||
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()
|