def get_bytes(self):
buf = bytes()
# 前导码
buf += self.header[::-1]
#
buf_back = bytes()
# 协议代码
buf_back += struct.pack('<H', self.protocol_code)
# 数据长度
buf_back += struct.pack('<H', 12 + len(self.frame_data))
# 校验码
buf_back += struct.pack('<H', self.checksum2)
# 帧选项
buf_back += struct.pack('<B', self.frame_option)
# 命令代码
buf_back += struct.pack('<B', self.cmd_code)
# 设备地址
buf_back += struct.pack('<H', self.id_485)
# 帧序号
buf_back += struct.pack('<H', self.frame_idx)
# 时间
buf_back += struct.pack('<I', self.unix_time)
# body
buf_back += self.frame_data
# 校验码
checksum = Crc.calc(buf_back)
buf += struct.pack("<H", checksum)
buf += buf_back
#
return buf
def parse_data(data):
# 参考 《VAVE之485协议定义V1.07》
cache = data
msgs = []
while True:
header_pos = cache.find(Msg485.HEADER_485)
if header_pos > -1:
# 找到包头，删除包头前面的数据
cache = cache[header_pos:]
#
if len(cache) >= 10:
# 校验码
checksum = struct.unpack('<H', cache[4: 6])[0]
# 协议代码
protocol_code = struct.unpack('<H', cache[6: 8])[0]
# 数据长度
data_len = struct.unpack('<H', cache[8: 10])[0]
if data_len > 1024:
# 异常包，正常一包数据长度不会超过1k
cache = cache[10:]
continue
else:
if len(cache) >= (10 + data_len):
calc_checksum = Crc.calc(cache[6: 10 + data_len])
if checksum == calc_checksum:
msg = Msg485Recv()
msg.checksum = checksum
msg.protocol_code = protocol_code
msg.data_len = data_len
# 消息数据
msg.checksum2 = struct.unpack('<H', cache[10: 12])[0]
# 帧选项
msg.frame_option = struct.unpack('<B', cache[12: 13])[0]
# 命令代码
msg.cmd_code = struct.unpack('<B', cache[13: 14])[0]
# 设备地址
msg.id_485 = struct.unpack('<H', cache[14: 16])[0]
# 帧序号
msg.frame_idx = struct.unpack('<H', cache[16: 18])[0]
# 帧内容
msg.frame_data = cache[18: 10 + data_len]
msgs.append(msg)
cache = cache[10 + data_len:]
continue
else:
# 异常包，校验不通过
cache = cache[10:]
continue
else:
break
else:
break
else:
# 未找到包头，不能直接清空，保留包头长度-1
if len(cache) >= len(Msg485.HEADER_485):
cache = cache[-len(Msg485.HEADER_485) + 1: len(cache)]
break
return cache, msgs
def __init__(self):
self.crc = Crc()
self.can1 = CanInterface(1)
self.com = Command(self)
self.linear = Linear()
#self.angular = Angular()
self.timcount = 0
self.tim2 = pyb.Timer(2)
self.tim2.init(freq=10)
def __init__(self):
self.crc = Crc()
self.can1 = CanInterface(1)
#self.can2 = CanInterface(2)
# allocate message buffer
self.messages_max = 25
self.messages_size_max = 128
self.messages = []
for i in range(self.messages_max):
self.messages.append(Message(self.crc, self.messages_size_max))
self.message_count = 0
def get_ota_response(self, timeout):
idx = 0
response = bytes()
msg_len = -1
start_time = time.time()
while True:
try:
if time.time() - start_time > timeout:
break
data = self.request.recv(1)
if data is None or len(data) == 0:
continue
if idx < 4:
# 找包头标识 FF FF FF FF
if data[0] == 0xFF:
idx += 1
else:
idx = 0
if idx == 4:
response = bytes(b'\xFF\xFF\xFF\xFF')
elif idx < 10:
# 读取包头
response += data
idx += 1
if idx == 10:
msg_len = struct.unpack('<H', response[8:10])[0]
elif idx < (10 + msg_len):
# 读取消息内容
response += data
idx += 1
if idx == (10 + msg_len):
# crc 校验
checksum = struct.unpack("<H", response[4:6])[0]
calc_checksum = Crc.calc(response[6:])
# 需要确认命令码，ota应答为0x10
msg_cmd = struct.unpack('<B', response[13:14])[0]
if checksum == calc_checksum and msg_cmd == 0x10:
break
else:
# print("checksum err or cmd is not 0x10")
idx = 0
response = bytes()
msg_len = -1
except socket.timeout:
pass
if msg_len != -1 and len(response) == (10 + msg_len):
# 消息内容里的应答数据是从整个包的第18个字节开始
return response[18:]
else:
return None
def parse_data(data):
cache = data
# 参考 《博瑞康木牛对接文档v1.02》
msgs = []
while True:
header_pos = cache.find(MsgJZQ.HEADER_JZQ)
if header_pos > -1:
# 找到包头，删除包头前面的数据
cache = cache[header_pos:]
#
if len(cache) >= 5:
# 数据长度
data_len = struct.unpack('<H', cache[3:5])[0]
if data_len > 1024:
# 异常包，正常一包数据长度不会超过1M
cache = cache[5:]
continue
else:
if len(cache) >= (9 + data_len):
checksum = struct.unpack(
'<H', cache[5 + data_len:7 + data_len])[0]
calc_checksum = Crc.calc(cache[2:5 + data_len])
if checksum == calc_checksum and cache[
7 + data_len:9 +
data_len] == MsgJZQ.TAIL_JZQ:
msg = MsgJZQRecv()
# 线路编号
msg.id_line = struct.unpack('<B',
cache[2:3])[0]
msg.data_len = data_len
msg.data = cache[5:5 + data_len]
msg.checksum = checksum
msgs.append(msg)
cache = cache[9 + data_len:]
continue
else:
# 异常包，校验不通过
cache = cache[5:]
continue
else:
break
else:
break
else:
# 未找到包头，异常数据，直接清空
cache = bytes()
break
return cache, msgs
def send_msg(self,
_device_id,
_frame_id,
_cmd_id=2,
_msg_len=0x0c,
_data=None):
time.sleep(0.01)
buf = bytes()
# 前导码
buf += b'\xff\xff\xff\xff'[::-1]
#
buf_back = bytes()
# 协议代码
buf_back += b'\xc1\x02'[::-1]
# 数据长度
buf_back += struct.pack('<H', _msg_len)
# 消息数据
# 校验码
buf_back += b'\x00\x00'[::-1]
# 帧选项
buf_back += b'\x40'[::-1]
# 命令代码
buf_back += struct.pack('<B', _cmd_id)
# 设备地址
buf_back += struct.pack('<H', _device_id)
# 帧序号
_frame_id %= 0xffff
buf_back += struct.pack('<H', _frame_id)
# 时间
buf_back += struct.pack('<I', 0)
# body
if _data is not None:
buf_back += _data
# 校验码
checksum = Crc.calc(buf_back)
buf += struct.pack("<H", checksum)
buf += buf_back
# send
###############
print("send: ", end=" ")
for b in buf:
print("%02X" % b, end=" ")
print("")
try:
self.ser.write(buf)
except Exception as e:
print(e)
def __init__(self, robo):
self.robo = robo
self.buf = RingBuffer(2048)
self.crc = Crc()
self.proc = Processor()
# load template messages
self.messages_size_max = 128
self.messages = []
self.message_count = 0
with open('command_list.csv','r') as file:
header = 0
row = 0 # translates into message nbr
for line in file:
if header == 0:
header = 1 # ignore
else:
line = line.rstrip('\n').rstrip('\r')
self.messages.append(Message(self.crc, self.messages_size_max))
self.messages[row].mode = 1 # overwrite crc
col = 0
for val in line.split(','):
if col == 1:
self.messages[row].boot = int(val)
elif col == 2:
self.messages[row].freq = int(val)
elif col > 2 and val != '0':
self.messages[row].put(int(val, 16))
col += 1
row += 1
self.message_count = row
# allocate byte arrays for frame buffer
self.data = []
for i in range(9):
self.data.append(bytearray(i))
# allocate frames with references to the byte arrays
self.frame = []
for i in range(9):
self.frame.append([0x201, 0, 0, memoryview(self.data[i])])
self.add(self.messages[11]) #LED ON
def get_bytes(self):
buf = bytes()
#
buf += self.header
#
buf_back = bytes()
#
buf_back += struct.pack('<B', self.id_line)
#
buf_back += struct.pack('<H', self.data_len)
#
buf_back += self.data
#
checksum = Crc.calc(buf_back)
#
buf += buf_back
buf += struct.pack("<H", checksum)
#
buf += self.tail
#
return buf
def decode_bag(self, bag):
if len(bag) > 10:
for i in range(0, len(bag) - 10):
if bag[i] == 0xFF and bag[i + 1] == 0xFF and \
bag[i + 2] == 0xFF and bag[i + 3] == 0xFF:
bag = bag[i:]
# 校验码
checksum = struct.unpack('<H', bag[4:6])[0]
# 协议代码
protocol = struct.unpack('<H', bag[6:8])[0]
# 数据长度
msg_len = struct.unpack('<H', bag[8:10])[0]
if len(bag) >= (10 + msg_len):
calc_checksum = Crc.calc(bag[6:10 + msg_len])
if checksum == calc_checksum:
# 消息数据
msg_checksum = struct.unpack('<H', bag[10:12])[0]
msg_frame = struct.unpack('<B', bag[12:13])[0]
msg_cmd = struct.unpack('<B', bag[13:14])[0]
msg_device_id = struct.unpack('<H', bag[14:16])[0]
msg_frame_id = struct.unpack('<H', bag[16:18])[0]
# 判断消息类型
if msg_cmd == 0x10:
return bag[18:22], bag[10 + msg_len:]
elif msg_cmd == 0x11:
return bag[18:18 + msg_len - 8], bag[10 +
msg_len:]
else:
return None, bag[10 + msg_len:]
else:
for j in range(0, 10 + msg_len):
print("%02X" % bag[j], end=" ")
print("")
print("checksum err %X %X" %
(checksum, calc_checksum))
return None, bag
return None, bag
def create_hashes(self):
self.hashes = []
for i in range(self.register_num):
self.hashes.append(
Crc(32, crc32_polinomials[i], True, 0xffffffff, True,
0xffffffff))
def create_local_hashes(self):
self.hashes = []
for i in range(self.num_hashes):
self.hashes.append(
Crc(32, crc32_polinomials[i], True, 0xffffffff, True,
0xffffffff))