def clear_diagnosic_info(self, id, sub_function=0, fill_data=0x00):
"""
14 清除诊断信息服务
parm:
id->接收方地址(物理地址或功能地址)
sub_function->子功能(该参数具体类型待定)
fill_data->数据填充
return:
14服务报文帧
"""
# ALL DTCs
if sub_function == 0:
frame = Frame(id_=id,
data=[
0x04, 0x14, 0xFF, 0xFF, 0xFF, fill_data,
fill_data, fill_data
],
dlc=8,
flags=0)
# ALL emission-related DTCs
if sub_function == 1:
frame = Frame(id_=id,
data=[
0x04, 0x14, 0x00, 0x00, 0x00, fill_data,
fill_data, fill_data
],
dlc=8,
flags=0)
return frame
def read_data_by_did(self, id, DID_dic={}):
"""
22 通过DID读取数据
parm:
id->接收方地址(物理地址或功能地址)
DID_dic->字典，举例DID_dic = {"VIN":{"DID_high":0xF1,"DID_low":0x90}}
return:
22服务报文帧
"""
if len(DID_dic) < 4:
i = 0
DID = [0x00, 0x00, 0x00, 0x00, 0x00, 0x00]
for key in DID_dic:
DID[i] = DID_dic[key]["DID_high"]
DID[i + 1] = DID_dic[key]["DID_low"]
i += 2
frame = Frame(id_=id,
data=[
len(DID_dic) * 2 + 1, 0x22, DID[0], DID[1],
DID[2], DID[3], DID[4], DID[5]
],
dlc=8,
flags=0)
return frame
else:
# 需要分多帧，还会涉及到流控，有待研究
pass
def __send_reset_zero_level_signal(self):
# COB-id: 0x60A, Byte 1: initiating download, 3 empty bytes, index: 0x2020, sub index: 2
frame = Frame(id_=1546,
data=[int('00101111', 2), 32, 32, 2, 1, 0, 0, 0],
dlc=8,
flags=0)
self.channel.write(frame)
def write_can_message(self, cobid, data, flag=0, timeout=None):
"""Combining writing functions for different |CAN| interfaces
Parameters
----------
cobid : :obj:`int`
|CAN| identifier
data : :obj:`list` of :obj:`int` or :obj:`bytes`
Data bytes
flag : :obj:`int`, optional
Message flag (|RTR|, etc.). Defaults to zero.
timeout : :obj:`int`, optional
|SDO| write timeout in milliseconds. When :data:`None` or not
given an infinit timeout is used.
"""
if self.__interface == 'Kvaser':
if timeout is None:
timeout = 0xFFFFFFFF
frame = Frame(id_=cobid, data=data, timestamp=None
) #, flags=canlib.MessageFlag.EXT) # from tutorial
self.__ch.writeWait(frame, timeout) #
elif self.__interface == 'AnaGate':
if not self.__ch.deviceOpen:
self.logger.notice('Reopening AnaGate CAN interface')
self.__ch.write(cobid, data, flag)
else:
msg = can.Message(arbitration_id=cobid,
data=data,
is_extended_id=False,
is_error_frame=False)
try:
self.__ch.send(msg, timeout)
except: # can.CanError:
self.logger.error("An Error occurred, The bus is not active")
def sendMessage(threadName, ch, number):
wb = xw.Book("./uds_test2.xlsx")
sht = wb.sheets("1_1_DiagAndCommMgtFuncUnit")
# 初始化开始行数
i = 11
# send_data = sht.range("K11").value
# for n in range(len(send_data)):
# send_data[n] = int(send_data[n],16)
while i < 11 + number:
if str(r.get("test"), encoding="utf8") == "start":
if str(r.get("status"),
encoding="utf8") == "%s%s" % ("start", str(i)):
# 向第一个线程传递当前行数值
r.set("line_number", str(i))
send_data = sht.range("%s%s" % ("k", str(i))).value
send_data = send_data.split(" ")
for n in range(len(send_data)):
send_data[n] = int(send_data[n], 16)
frame = Frame(id_=0x79D,
data=[
send_data[0], send_data[1], send_data[2],
send_data[3], send_data[4], send_data[5],
send_data[6], send_data[7]
],
dlc=8,
flags=0)
ch.write(frame)
i = i + 1
# r.set("test","stop")
r.set("test", "stop")
def sendMessage(ch,
frame=Frame(
id_=0x79D,
data=[0x02, 0x10, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00],
dlc=8,
flags=0)):
ch.write(frame)
def __set_angle_data_interval(self, ms):
if 0 <= ms < 256:
# COB-id: 0x60A, Byte 1: initiating download, 2 empty bytes, index: 0x1808, sub index: 5
frame = Frame(id_=1546,
data=[int('00101011', 2), 8, 24, 5, ms, 0, 0, 0],
dlc=8,
flags=0)
self.channel.write(frame)
def setSteerCMD(self, value):
self.steer = value
frame = Frame(id_=0x152,
data=(self.frame152()).to_bytes(8,
byteorder="little",
signed=False))
self.ch.write(frame)
time.sleep(0.001)
def setBrakeCMD(self, value):
value = value if value < 30000 else 30000
self.brake = value
frame = Frame(id_=0x152,
data=(self.frame152()).to_bytes(8,
byteorder="little",
signed=False))
self.ch.write(frame)
time.sleep(0.001)
def write_reg(self, block: Enum, address: int, value: int):
super().write_reg(block, address, value)
if value > 32767:
payload = struct.pack(">biH", block.value, address, value)
else:
payload = struct.pack(">bih", block.value, address, value)
send_frame = Frame(id_=0x0100 + self.can_bus_unit_id, data=payload, dlc=len(payload), flags=0)
self._hw_interface_can_bus.write(send_frame)
def setAccelCMD(self, value):
value = value if value < 2000 else 2000
self.accel = value
frame = Frame(id_=0x152,
data=(self.frame152()).to_bytes(8,
byteorder="little",
signed=False))
#print(frame)
self.ch.write(frame)
time.sleep(0.001)
def put_bytes(self, data, timeout_ms): # protected
from canlib import canlib, Frame
view = memoryview(data)
for i in range(0, len(view), 8):
try:
self.__can.writeWait(Frame(id_=self.__message_id,
data=view[i:i + 8]),
timeout=self._put_short_timeout)
except canlib.CanError:
break
def write_block(self, block: Enum, address: int, block_data: np.ndarray):
super().write_block(block, address, block_data)
length = len(block_data)
payload = struct.pack(">biH", block.value, address, length)
send_frame = Frame(id_=0x0300 + self.can_bus_unit_id, data=payload, dlc=len(payload), flags=0)
self._hw_interface_can_bus.write(send_frame)
time.sleep(0.01)
start_offset = 0
while start_offset < length:
data = block_data[start_offset:start_offset + 4]
wr_data = np.array(data, '>i2')
wr_data_bytes = wr_data.tobytes()
# offsets for command are in int indexes
send_frame = Frame(id_=0x0300 + self.can_bus_unit_id, data=wr_data_bytes, dlc=8, flags=0)
self._hw_interface_can_bus.write(send_frame)
start_offset += len(data)
if start_offset % (64 * 8) == 0:
time.sleep(0.05)
def send_receive_sig(self):
self.toggled = 0
t = 0.1
frame = Frame(id_=528,
data=bytearray(b'\x00\x00'),
flags=canlib.MessageFlag.STD)
self.ch.write(frame)
time.sleep(t)
frame = Frame(id_=528,
data=bytearray(b'\x01\x00'),
flags=canlib.MessageFlag.STD)
self.ch.write(frame)
time.sleep(t)
frame = Frame(id_=528,
data=bytearray(b'\x00\x00'),
flags=canlib.MessageFlag.STD)
self.ch.write(frame)
time.sleep(1)
self.toggled = 1
def process_sdo_write(self, msg, timeout=42):
"""Process a SDO write request.
Abort transfer if not expedited.
Parameters
----------
msg : :obj:`list` of :obj:`int`
CAN data. Must have a length of 8 bytes.
timeout : :obj:`int`, optional
SDO timeout in milliseconds
"""
cmd = msg[0]
index = int.from_bytes([msg[1], msg[2]], 'little')
subindex = msg[3]
n = (cmd >> 2) & 0b11
datasize = 4 - n
data = int.from_bytes(msg[4:(4 + datasize)], 'little')
cobid = coc.COBID.SDO_TX + self.__nodeId
ret = [0 for i in range(8)]
# Check if command specifier known
if cmd not in [0x23, 0x27, 0x2b, 0x2f]:
self.logger.error('Unkown command specifier')
ret = self.sdo_abort_message([msg[2], msg[1]], msg[3], SAC.COMMAND)
# Check if object exists
elif index not in self.__od:
self.logger.error('Object does not exist.')
ret = self.sdo_abort_message([msg[2], msg[1]], msg[3],
SAC.NO_OBJECT)
# Check if subindex exists
elif subindex not in self.__od[index]:
self.logger.error('Subindex does not exist.')
ret = self.sdo_abort_message([msg[2], msg[1]], msg[3],
SAC.SUBINDEX)
# Check access attribute
elif self.__od[index][subindex].attribute in [
coc.ATTR.RO, coc.ATTR.CONST
]:
self.logger.error('No write access')
ret = self.sdo_abort_message([msg[2], msg[1]], msg[3], SAC.ACCESS)
else:
self.logger.notice(f'Writing value {data:X} on '
f'{index:X}:{subindex:X}.')
self.__od[index][subindex].value = data
ret[0] = 0x60
ret[1:4] = msg[1:4]
ret[4:] = [0 for i in range(4)]
if index == 0x2000 and subindex in range(1, 5):
scb = subindex - 1
self.logger.notice(f'SCB{scb}: Setting connections.')
for i in range(16):
pspp_idx = 0x2200 + scb * 16 + i
self.__od[pspp_idx][2].value = bool((data >> i) & 1)
self.__ch.writeWait(Frame(cobid, ret), timeout)
def setAngularSpeed(self, value):
value = value if value < 255 else 255
self.angular = value << 40
#self.data = self.andbytes(self.data, (aliveVar << 40).to_bytes(8, byteorder="little", signed=False))
frame = Frame(id_=0x150,
data=(self.angular).to_bytes(8,
byteorder="little",
signed=False))
#print(frame)
self.ch.write(frame)
time.sleep(0.001)
def command():
while True:
cmd = s.recv(36)
frameId = (bytes_to_int(cmd[24:28])-2**29)
# ch0.writeFrame(id_=frameId,data=bytearray(bytes_to_int(cmd[28:36])))
print(frameId)
print(enums.MessageFlag.EXT)
f = Frame(id_=-2**29+frameId,data=(cmd[28:36]),flags=enums.MessageFlag.EXT)
print(f.id)
ch0.write(f)
print(bytes_to_int((cmd[28:36])))
sys.stdout.flush()
def setBrakeCMD(self, value):
value = value if value < 30000 else 30000
self.brake = value
frame = Frame(id_=0x152,
data=(self.frame152()).to_bytes(8,
byteorder="little",
signed=False))
try:
self.ch.write(frame)
except:
self.node.get_logger().fatal("dbw_cmd_node : can send failed")
time.sleep(0.001)
def security_access(self, id, sub_function, *key):
"""
27 安全访问
这个服务暂时还没想好怎么写
"""
if sub_function % 2 == 0: # 发送key
pass
else: # 请求种子
frame = Frame(
id_=id,
data=[0x02, 0x27, sub_function, 0x00, 0x00, 0x00, 0x00, 0x00],
dlc=8,
flags=0)
return frame
def setSteerCMD(self, value):
if abs(value) >= 440:
Hsigned = -1 if value < 0 else 1
value = Hsigned * 440
self.steer = value
frame = Frame(id_=0x152,
data=(self.frame152()).to_bytes(8,
byteorder="little",
signed=False))
try:
self.ch.write(frame)
except:
self.node.get_logger().fatal("dbw_cmd_node : can send failed")
time.sleep(0.001)
def security_access_calculate_key(self, id, seed):
"""
用于通过seed计算key
"""
# seed_length = len(seed)
"""
这里写key计算算法
"""
frame = Frame(id_=id,
data=[0x06, 0x27, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00],
dlc=8,
flags=0)
return frame
def setAngularSpeed(self, value):
value = value if value < 255 else 255
self.angular = value << 40
#self.data = self.andbytes(self.data, (aliveVar << 40).to_bytes(8, byteorder="little", signed=False))
frame = Frame(id_=0x150,
data=(self.angular).to_bytes(8,
byteorder="little",
signed=False))
#print(frame)
try:
self.ch.write(frame)
except:
self.node.get_logger().fatal("dbw_cmd_node : can send failed")
time.sleep(0.001)
def send_flow_control_frame(self, id, STmin=0x14):
"""
发送流控帧
parm:
id->接收地址
STmin->帧最小间隔时间(默认20ms)
rules:
ISO15765
"""
frame = Frame(id_=id,
data=[0x30, 0x00, STmin, 0x00, 0x00, 0x00, 0x00, 0x00],
dlc=8,
flags=0)
# 发送流控帧
self.ch.write(frame)
def __set_slope_level(self, msg):
try:
msg = msg.replace(",", ".")
slope = float(msg.split(":", 1)[1])
ba = bytearray(struct.pack("f", slope))
frame = Frame(id_=1546,
data=[
int('00100011', 2), 32, 32, 1, ba[0], ba[1],
ba[2], ba[3]
],
dlc=8,
flags=0)
self.channel.write(frame)
except ValueError:
pass
def setGearCMD(self, value):
cmd = [0, 5, 6, 7]
if not value in cmd:
#Rogue Data
return -1
#print(value)
self.gear = value
frame = Frame(id_=0x152,
data=(self.frame152()).to_bytes(8,
byteorder="little",
signed=False))
self.ch.write(frame)
time.sleep(0.001)
def AliveSender(self):
aliveVar = 0
while True:
if aliveVar == 256: aliveVar = 0
self.data_150 += (aliveVar << 8)
self.data_150 += (self.angular)
frame = Frame(id_=0x150,
data=(self.data_150).to_bytes(8,
byteorder="little",
signed=False))
self.ch.write(frame)
time.sleep(0.001)
aliveVar += 1
#print(frame)
self.data_150 = 0
def analysis(self, ID, Bytearry_list, DBC):
'''
:param ID:报文ID；
:param Bytearry_list:bytearry格式的数据列表；
:param DBC:加载的DBC文件；
:return:返回一个字典{信号名：数值, ...}
'''
signals_value = {}
# signals_unit = {}
frame = Frame(id_=ID, data=Bytearry_list)
Message = DBC.interpret(frame)
for Signal in Message:
signals_value.setdefault(
Signal.name, '{}'.format(Signal.value)
) # Signal.phys, Signal.raw, Signal value,Signal.unit
# signals_unit.setdefault(Signal.name, '{}'.format(Signal.unit))#这个返回信号名和单位{信号名：单位, ...}
return signals_value #[signals_value, signals_unit]
def setGearCMD(self, value):
cmd = [0, 5, 6, 7]
if not value in cmd:
#Rogue Data
return -1
#print(value)
self.gear = value
frame = Frame(id_=0x152,
data=(self.frame152()).to_bytes(8,
byteorder="little",
signed=False))
try:
self.ch.write(frame)
except:
self.node.get_logger().fatal("dbw_cmd_node : can send failed")
time.sleep(0.001)
def session(self, id, sub_function=0x01, fill_data=0x00):
"""
10 会话服务
parm:
sub_function->子功能(该参数具体类型待定)
id->接收方地址(物理地址或功能地址)
fill_data->数据填充
return:
10服务报文帧
"""
frame = Frame(id_=id,
data=[
0x02, 0x10, sub_function, fill_data, fill_data,
fill_data, fill_data, fill_data
],
dlc=8,
flags=0)
return frame
def read_reg(self, block: Enum, address: int, expected_res: ExpResType):
super().read_reg(block, address, expected_res)
payload = struct.pack(">bi", block.value, address)
send_frame = Frame(id_=0x0200 + self.can_bus_unit_id, data=payload, dlc=len(payload), flags=0)
self._hw_interface_can_bus.write(send_frame)
time.sleep(0.1) # time.sleep(0.01) # this delay (0.1) is necessary when using long cable (?)
res = -1
for retry in range(5):
try:
response = self._hw_interface_can_bus.read()
if response.id == 0x0280 + self.can_bus_unit_id and response.dlc == 2:
res = int(response.data[0]) << 8 | int(response.data[1])
break
else:
time.sleep(0.01)
except Exception as ex:
res = -1
time.sleep(0.01)
return np.int16(res)
