def write_hexfile(self, filename, generate_crc=None):
max_write_addr = 0x6800
crc_addr = max_write_addr - 2
hexfile = intelhex.IntelHex()
hexfile.padding = 0xff
with open(filename) as f:
hexfile.loadhex(f)
print(hexfile.maxaddr(), max_write_addr - 2)
if generate_crc == None and hexfile.maxaddr() < max_write_addr - 2:
generate_crc = True
if generate_crc:
if hexfile.maxaddr() > max_write_addr - 2:
raise HexfileTooBig("hexfile too big")
crc = crc16.crc16(hexfile, max_write_addr - 2)
hexfile[crc_addr + 0] = hi(crc)
hexfile[crc_addr + 1] = lo(crc)
else:
if hexfile.maxaddr() > max_write_addr:
raise HexfileTooBig("hexfile too big")
expected_crc = crc16.crc16(hexfile, max_write_addr - 2)
got_crc = (hexfile[crc_addr + 0] << 8) | (hexfile[crc_addr + 1])
if expected_crc != got_crc:
raise BadCRC16("Invalid crc16 include inhexfile. Expected 0x{:x}, got 0x{:x}" \
.format(expected_crc, got_crc))
# hexfile.dump()
for i in range(MAX_PROGRAMMABLE_PAGE + 1):
self.cmd_erase_page(i)
# a = [hex(hexfile[i]) for i in range(1, max_write_addr)]
# print(a, hex(len(a)))
self.cmd_write_bytes(
0x0001, array('B', [hexfile[i] for i in range(1, max_write_addr)]))
self.cmd_write_bytes(0x0000, array('B', [hexfile[0]]))
def SetSpeed(spd):
print("Speed set to:", spd)
speed = (int(spd).to_bytes(4, 'big', signed=True))
ser.write((b'\x01\x10\x18\x04\x00\x02\x04'+speed)
+ crc16.crc16((b'\x01\x10\x18\x04\x00\x02\x04'+speed))['lower']
+ crc16.crc16((b'\x01\x10\x18\x04\x00\x02\x04'+speed))['upper'])
ser.read(200)
def authenticate(rfidInput):
packetcrc = struct.unpack('<H',rfidInput.pop())[0]
scanned_card = struct.unpack('<L', ''.join(rfidInput[2]))[0]
mac_addr = []
for octet in rfidInput[2]:
mac_addr.append("%02x"%ord(octet))
mac_addr = ":".join(mac_addr)
device_salt = dob_get_salt(mac_addr)
calculate_crc = []
tokens_list = []
with token_lock:
calculate_crc.append(crc16.crc16(''.join(rfidInput)+device_salt+tokens[0]))
calculate_crc.append(crc16.crc16(''.join(rfidInput)+device_salt+tokens[1]))
# Do a copy not DON'T point to the data
tokens_list.append(tokens[0])
tokens_list.append(tokens[1])
if packetcrc in calculate_crc:
if packetcrc == calculate_crc[0]:
tokens_list.pop(1)
else:
tokens_list.pop(0)
if dob_check_user(mac_addr, scanned_card):
resp = response(rfidInput[0], 'Ok')
else:
resp = response(rfidInput[0], 'Fail')
else:
return {'response':response(rfidInput[0], 'ChecksumFail')}
resp.append(struct.pack('<H', crc16.crc16(''.join(resp)+device_salt+tokens_list[0])))
return {'response': resp}
def hello(rfidInput):
# dnl = struct.unpack('<B',rfidInput[1])[0] # device name length
# packetcrc = struct.unpack('<H',''.join(rfidInput[-2:]))[0]
device_name = rfidInput[1]
version_number = ord(rfidInput[3])
mac_addr = []
for octet in rfidInput[2]:
mac_addr.append("%02x"%ord(octet))
mac_addr = ":".join(mac_addr)
device_salt = dob_get_salt(mac_addr)
debugPrint( "device name", device_name )
debugPrint( "mac addr" , mac_addr )
debugPrint( "Version Number", version_number )
packetcrc = struct.unpack('<H',rfidInput.pop())[0]
calculate_crc = []
with token_lock:
calculate_crc.append(crc16.crc16(''.join(rfidInput)+device_salt+tokens[0]))
calculate_crc.append(crc16.crc16(''.join(rfidInput)+device_salt+tokens[1]))
if packetcrc in calculate_crc:
if dob_hello_check(device_name, mac_addr, version_number):
return {'response':response(rfidInput[0], 'Ok')}
else:
return {'response':response(rfidInput[0], 'Unknown')}
else:
return {'response':response(rfidInput[0], 'ChecksumFail')}
def Move(pos):
print("Moving to:", pos)
position = (ConvertSteps(float(pos)).to_bytes(4, 'big', signed=True))
ser.write((b'\x01\x10\x18\x00\x00\x04\x08\x00\x00\x00\x01'+position)
+ crc16.crc16((b'\x01\x10\x18\x00\x00\x04\x08\x00\x00\x00\x01'+position))['lower']
+ crc16.crc16((b'\x01\x10\x18\x00\x00\x04\x08\x00\x00\x00\x01'+position))['upper'])
ser.read(200)
Execute()
def CheckAlarm():
ser.write(b'\x01\x03\x00\x7F\x00\x01'
+ crc16.crc16(b'\x01\x03\x00\x7F\x00\x01')['lower']
+ crc16.crc16(b'\x01\x03\x00\x7F\x00\x01')['upper'])
if int.from_bytes(ser.read(200)[3:5], 'big') & 0b10000000 == 0b10000000:
print("Alarm ON!")
return 1
else:
return 0
def test_auth_CRC(self):
mac_add = ''
for octet in self.mac_address.split(":"):
mac_add += struct.pack('<B', int(octet, 16))
send_auth = ['@', mac_add, struct.pack('<L', self.allowed_cards_list[0])]
send_auth.append(struct.pack('<H', crc16.crc16(''.join(send_auth) + self.device_salt + tokens[0])))
auth = authenticate(send_auth)['response']
self.assertEqual(struct.unpack('<H', auth[-1])[0], crc16.crc16(''.join(auth[:-1]) + self.device_salt + tokens[0]))
def GoRight():
print("Going right...")
ser.write(b'\x01\x10\x00\x7C\x00\x02\x04\x00\x00\x80\x00'
+ crc16.crc16(b'\x01\x10\x00\x7C\x00\x02\x04\x00\x00\x80\x00')['lower']
+ crc16.crc16(b'\x01\x10\x00\x7C\x00\x02\x04\x00\x00\x80\x00')['upper'])
ser.read(200)
CheckReady()
ser.read(200)
CheckMove()
ser.read(200)
CheckAlarm()
ser.read(200)
def Stop():
print("Stopping...")
ser.write(b'\x01\x10\x00\x7C\x00\x02\x04\x00\x00\x00\x00'
+ crc16.crc16(b'\x01\x10\x00\x7C\x00\x02\x04\x00\x00\x00\x00')['lower']
+ crc16.crc16(b'\x01\x10\x00\x7C\x00\x02\x04\x00\x00\x00\x00')['upper'])
ser.read(200)
CheckReady()
ser.read(200)
CheckMove()
ser.read(200)
CheckAlarm()
ser.read(200)
def getToken():
print "Requesting Token"
print ":".join("{0:x}".format(ord(c)) for c in umsgpack.packb(['T']))
s.send(umsgpack.packb(['T']))
token = umsgpack.unpackb(s.recv(1024))
print "Token Recived:",token
crc = token.pop()
if struct.unpack('<H', crc)[0] == crc16.crc16(''.join(token)):
print "Token Valid - CRC passed"
else:
print "Token INVALID", crc16.crc16(token[1]), struct.unpack('<H', crc)[0]
return token[1]
def getToken():
print "Requesting Token"
print ":".join("{0:x}".format(ord(c)) for c in umsgpack.packb(['T']))
s.send(umsgpack.packb(['T']))
token = umsgpack.unpackb(s.recv(1024))
print "Token Recived:", token
crc = token.pop()
if struct.unpack('<H', crc)[0] == crc16.crc16(''.join(token)):
print "Token Valid - CRC passed"
else:
print "Token INVALID", crc16.crc16(token[1]), struct.unpack('<H',
crc)[0]
return token[1]
def readpacket(self, timeout=None):
total_read = 4
try:
initial_read = self.read(total_read)
except serial.SerialException as e:
#print ('readpacket() : Exception =', e)
if sys.version_info < (3, 0):
sys.exc_clear()
return None
if initial_read != []:
all_data = initial_read
if ord(initial_read[0]) == 1:
command = initial_read[3]
data_number = struct.unpack('<H', initial_read[1:3])[0]
if data_number > 6:
toread = abs(data_number - 6)
second_read = self.read(toread)
all_data += second_read
total_read += toread
out = second_read
else:
out = ''
suffix = self.read(2)
if len(suffix) < 2:
raise constants.Error('Packet header too short!')
sent_crc = struct.unpack('<H', suffix)[0]
local_crc = crc16.crc16(all_data, 0, total_read)
if sent_crc != local_crc:
raise constants.CrcError("readpacket Failed CRC check")
num1 = total_read + 2
return ReadPacket(command, out)
else:
raise constants.Error('Error reading packet header!')
def readpacket(self, timeout=None):
total_read = 4
initial_read = self.read(total_read)
if initial_read != []:
all_data = initial_read
if ((sys.version_info.major > 2) and (initial_read[0] == 1)) or \
((sys.version_info.major <= 2) and ord(initial_read[0]) == 1):
command = initial_read[3]
data_number = struct.unpack('<H', initial_read[1:3])[0]
if data_number > 6:
toread = abs(data_number - 6)
second_read = self.read(toread)
all_data += second_read
total_read += toread
out = second_read
else:
out = ''
suffix = self.read(2)
if len(suffix) < 2:
raise constants.Error('Packet header too short!')
sent_crc = struct.unpack('<H', suffix)[0]
local_crc = crc16.crc16(all_data, 0, total_read)
if sent_crc != local_crc:
raise constants.CrcError("readpacket Failed CRC check")
return ReadPacket(command, out)
else:
raise constants.Error('Error reading packet header!')
return None
def ComposePacket(self, command, payload=None):
assert self._packet is None
self._packet = bytearray(b"\x01") # OFFSET_SOF
self._packet += b"\x00" # OFFSET_LENGTH
self._packet += b"\x00"
self._packet += command.to_bytes(1, byteorder='little', signed=False) # OFFSET_CMD
if payload and len(payload):
for xx in payload:
if type(xx) == str:
data = bytes(xx, 'utf8')
elif type(xx) == bytes:
data = xx
elif type(xx) == bytearray:
data = bytes(xx)
elif type(xx) == int:
data = xx.to_bytes(4, byteorder='little', signed=False)
else:
print ('ComposePacket : unknown type =', type(xx))
raise NotImplementedError
self._packet += data
# Set the length
theLength = len(self._packet) + 2
self._packet[self.OFFSET_LENGTH] = theLength & 0xff
self._packet[self.OFFSET_LENGTH+1] = (theLength >> 8) & 0xff
# Append the CRC
crc = crc16.crc16(self._packet, 0, len(self._packet))
self._packet += crc.to_bytes(2, byteorder='little', signed=False)
def AppendCrc(self):
self.SetLength()
ps = self.PacketString()
crc = crc16.crc16(ps, 0, len(ps))
for x in struct.pack('H', crc):
# Added chr() function to ensure successful appending
self._packet.append(chr(x))
def ReadDatabasePage(self, record_type, page):
record_type_index = constants.RECORD_TYPES.index(record_type)
try:
self.WriteCommand(
constants.READ_DATABASE_PAGES,
(chr(record_type_index), struct.pack('I', page), chr(1)))
packet = self.readpacket()
except Exception as e:
#print ('ReadDatabasePage() Exception =', e)
if sys.version_info < (3, 0):
sys.exc_clear()
return []
if packet is None:
return []
assert ord(packet.command) == 1
# first index (uint), numrec (uint), record_type (byte), revision (byte),
# page# (uint), r1 (uint), r2 (uint), r3 (uint), ushort (Crc)
header_format = '<2IcB4IH'
header_data_len = struct.calcsize(header_format)
header = struct.unpack_from(header_format, packet.data)
header_crc = crc16.crc16(packet.data[:header_data_len - 2])
assert header_crc == header[-1]
assert ord(header[2]) == record_type_index
assert header[4] == page
packet_data = packet.data[header_data_len:]
return self.ParsePage(header, packet_data)
def build(self):
data = struct.pack('BBBB', self.dest, self.source, self.flags,
self.ptype)
data += self.payload
data += struct.pack('<H', crc16.crc16(data))
return data
def write_page(self, page, data):
if len(data) > FLASH_PAGE_SIZE:
raise LoaderError, "data is larger than page size"
crc = crc16(data)
self.fp.write('write %x %x %x\r\n' % (page, len(data), crc))
self.fp.write(data)
self.fp.flush()
self.wait()
def test_heartBeat(self):
mac_add = ''
for octet in self.mac_address.split(":"):
mac_add += str(int(octet, 16))
send_heartBeat = ['B', mac_add]
send_heartBeat.append(struct.pack('<H', crc16.crc16(''.join(send_heartBeat))))
b = heartBeat(send_heartBeat)['response']
self.failUnless(self.checkResponce('B',b))
def test_sendHello(self):
device_len = len(self.device_name)
mac_add = ''
for octet in self.mac_address.split(":"):
mac_add += struct.pack('<B', int(octet, 16))
send_hello = ['H', self.device_name, mac_add, chr(self.version_num)]
send_hello.append(struct.pack('<H', crc16.crc16(''.join(send_hello) + self.device_salt + tokens[0])))
h = hello(send_hello)['response']
self.failUnless(self.checkResponce('H',h))
def write(self, data):
data = translate_to_robot(data)
if data == False:
self.user_sock.write("Invalid command or parameters")
return
checksum = crc16.crc16(self.address + data)
checksum = struct.pack(">H", checksum)
data = self.address + data + checksum
#print "send", repr(data)
socketlib.Socket.write(self, data)
def test_auth_deny(self):
mac_add = ''
for octet in self.mac_address.split(":"):
mac_add += str(int(octet, 16))
for card in self.deny_cards_list:
send_auth = ['@', mac_add, struct.pack('<L', card)]
send_auth.append(struct.pack('<H', crc16.crc16(''.join(send_auth) + self.device_salt + tokens[0])))
auth = authenticate(send_auth)['response']
self.failIf(self.checkResponce('@',auth))
def __init__(self):
rospy.loginfo(rospy.get_name() + ": Start")
# static parameters
self.update_rate = 200 # set update frequency [Hz]
self.count = 0
# status parameters
self.position_ok = Int8()
self.position_ok = 0
self.last_heard = 0.0
self.last_nmea_sent = 0.0
# get parameters
self.output_format = rospy.get_param("~output_format", "nmea")
self.serial_device = rospy.get_param("~serial_device", "/dev/gt_pos_robot")
self.serial_baudrate = rospy.get_param("~serial_baudrate", 57600)
self.robot_id = rospy.get_param("~robot_id", "robot")
self.lat_offset = rospy.get_param("~latitude_offset", 55.47038)
self.lon_offset = rospy.get_param("~longitude_offset", 10.32966)
self.alt_offset = rospy.get_param("~altitude_offset", 0.0)
# get topic names
self.topic_nmea_from_gt_pos_sub = rospy.get_param("~nmea_from_gt_pos_sub",'/fmData/nmea_from_gt_pos')
self.topic_position_ok_pub = rospy.get_param("~position_ok_pub",'/fmData/robot_position_ok')
self.topic_gpgga_pub = rospy.get_param("~gpgga_pub",'/fmData/robot_gpgga')
# setup publishers
self.position_ok_pub = rospy.Publisher(self.topic_position_ok_pub, Int8, queue_size=0)
self.gpgga = StringStamped()
self.gpgga_pub = rospy.Publisher(self.topic_gpgga_pub, StringStamped, queue_size=0)
# configure and open serial device
ser_error = False
try:
self.ser_dev = serial.Serial(self.serial_device, self.serial_baudrate)
except Exception as e:
rospy.logerr(rospy.get_name() + ": Unable to open serial device: %s" % self.serial_device)
ser_error = True
if ser_error == False:
self.slip = slip_protocol()
self.crc = crc16()
# instantiate utmconv class
self.uc = utmconv()
(self.utm_hem, self.utm_z, self.utm_l, self.utm_e, self.utm_n) = self.uc.geodetic_to_utm (self.lat_offset, self.lon_offset)
# setup subscription topic callbacks
rospy.Subscriber(self.topic_nmea_from_gt_pos_sub, nmea, self.on_nmea_msg)
# call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
def parse(self, data):
data = bytearray(data)
if len(data) < 6:
return None
self.dest, self.source, self.flags, self.ptype = struct.unpack_from(
'BBBB', data)
self.payload = data[4:-2]
return crc16.crc16(data) == 0
def sign_set_text_frame(p, session):
frame_id = ord(p.data[0])
rev = ord(p.data[1])
font = ord(p.data[2])
colour = ord(p.data[3])
flash = ord(p.data[4])
length = ord(p.data[5])
text = str(p.data[6:-2])
crc = str(p.data[-2:]).encode('hex')
offset = crc16.crc16(p.mi + p.data)
if offset:
session.logger.warning('Text frame CRC error: offset = %d' % offset)
session.reject(p.mi, '\x04')
return
if frame_id == 0:
session.logger.warning('Text frame ID 0 is invalid')
session.reject(p.mi, '\x02')
return
if len(text) != length:
session.logger.warning('Specified text length does not match data')
session.reject(p.mi, '\x16')
return
try:
session.server.vms.set_text_frame(
frame_id, rev, font, colour,
flash, length, text)
except Exception as e:
if 'FontException' in e.message[1]:
session.logger.warning('Invalid font')
session.reject(p.mi, '\x0b')
return
elif 'ColourException' in e.message[1]:
session.logger.warning('Invalid colour')
session.reject(p.mi, '\x0c')
return
elif 'ConspicuityException' in e.message[1]:
session.logger.warning('Invalid flash pattern')
session.reject(p.mi, '\x11')
return
else:
session.logger.error('Unrecognised exception')
session.reject(p.mi, '\x02')
session.logger.info('Successfully set sign text frame id %d' % frame_id)
status_reply = rtastatus.generate_sign_status(p, session)
session.send_packet(0x06, status_reply)
def read_page(self, page):
self.fp.write('read %x\r\n' % page)
self.wait()
reply = self.fp.read(FLASH_PAGE_SIZE + 6)
if len(reply) != FLASH_PAGE_SIZE + 6:
raise LoaderError, \
"Reply to short for read command, length = %d" % len(reply)
data = reply[:FLASH_PAGE_SIZE]
crc = int(reply[FLASH_PAGE_SIZE:], 16)
if crc16(data) != crc:
raise LoaderCRCError, "CRC16 error"
return data
def sign_set_text_frame(p, session):
frame_id = ord(p.data[0])
rev = ord(p.data[1])
font = ord(p.data[2])
colour = ord(p.data[3])
flash = ord(p.data[4])
length = ord(p.data[5])
text = str(p.data[6:-2])
crc = str(p.data[-2:]).encode('hex')
offset = crc16.crc16(p.mi + p.data)
if offset:
session.logger.warning('Text frame CRC error: offset = %d' % offset)
session.reject(p.mi, '\x04')
return
if frame_id == 0:
session.logger.warning('Text frame ID 0 is invalid')
session.reject(p.mi, '\x02')
return
if len(text) != length:
session.logger.warning('Specified text length does not match data')
session.reject(p.mi, '\x16')
return
try:
session.server.vms.set_text_frame(frame_id, rev, font, colour, flash,
length, text)
except Exception as e:
if 'FontException' in e.message[1]:
session.logger.warning('Invalid font')
session.reject(p.mi, '\x0b')
return
elif 'ColourException' in e.message[1]:
session.logger.warning('Invalid colour')
session.reject(p.mi, '\x0c')
return
elif 'ConspicuityException' in e.message[1]:
session.logger.warning('Invalid flash pattern')
session.reject(p.mi, '\x11')
return
else:
session.logger.error('Unrecognised exception')
session.reject(p.mi, '\x02')
session.logger.info('Successfully set sign text frame id %d' % frame_id)
status_reply = rtastatus.generate_sign_status(p, session)
session.send_packet(0x06, status_reply)
def __init__(self, data):
self.data = data[:-4] + binascii.a2b_hex(data[-4:])
offset = crc16.crc16(self.data)
if offset:
raise crc16.CRCError('Offset = %d' % offset)
self.control = self.data[0]
if self.data[0] == '\x15':
self.nr = int(struct.unpack(">B",str(binascii.a2b_hex(self.data[1:3])))[0])
self.addr = int(struct.unpack(">B",str(binascii.a2b_hex(self.data[3:5])))[0])
else:
self.ns = int(struct.unpack(">B",str(binascii.a2b_hex(self.data[1:3])))[0])
self.nr =int(struct.unpack(">B",str(binascii.a2b_hex(self.data[3:5])))[0])
self.addr = int(struct.unpack(">B",str(binascii.a2b_hex(self.data[5:7])))[0])
self.mi = binascii.a2b_hex(self.data[8:10])
self.data = binascii.a2b_hex(self.data[10:-2])
def ReadDatabasePage(self, record_type, page):
record_type_index = constants.RECORD_TYPES.index(record_type)
self.WriteCommand(constants.READ_DATABASE_PAGES, (chr(record_type_index), struct.pack('I', page), chr(1)))
packet = self.readpacket()
assert ord(packet.command) == 1
# first index (uint), numrec (uint), record_type (byte), revision (byte),
# page# (uint), r1 (uint), r2 (uint), r3 (uint), ushort (Crc)
header_format = '<2I2c4IH'
header_data_len = struct.calcsize(header_format)
header = struct.unpack_from(header_format, packet.data)
header_crc = crc16.crc16(packet.data[:header_data_len-2])
assert header_crc == header[-1]
assert ord(header[2]) == record_type_index
assert header[4] == page
packet_data = packet.data[header_data_len:]
return self.ParsePage(header, packet_data)
def ValidateCRC(self,buff,ln,key):
valid = False
op = opmanager.OpcodePacket()
opcode = op.getname(self.opcode)
if opcode in {'OP_SessionRequest', 'OP_SessionResponse', 'OP_OutOfSession'}:
valid = True
else:
comp_crc = crc16.crc16(buff,ln-2,key)
packet_crc = socket.ntohs(buff[ln-2:])
if packet_crc and comp_crc != packet_crc:
print("CRC mismatch!")
quit()
return valid
def __init__(self):
rospy.loginfo(rospy.get_name() + ": Start")
#Get parameters
#self.got_dev = rospy.get_param("~got_device", "/dev/drone_cage_got_in")
#self.aq_dev = rospy.get_param("~aq_device", "/dev/drone_cage_aq_out")
#Serial communications
serial_error = False
try:
self.serialIn = serial.Serial(
port="/dev/ttyUSB0",
baudrate=57600) #(port = self.got_dev, baudrate = 57600)
except Exception as e:
rospy.logerr(
rospy.get_name() +
": Unable to open serial device") # %s" % self.got_dev)
serial_error = True
if serial_error == False:
try:
self.serialOut = serial.Serial(
port="/dev/ttyUSB1",
baudrate=57600) #(port = self.aq_dev, baudrate = 57600)
except Exception as e:
rospy.logerr(
rospy.get_name() +
": Unable to open serial device") # %s" % self.aq_dev)
serial_error = True
if serial_error == False:
#Position conversion class
self.uc = utmconv()
#Crc calculation class
self.crc = crc16()
#Center positions
self.zeroEasting = 590734.045671
self.zeroNorthing = 6136563.68892
#Update frequency
self.rate = 200 #5Hz
self.r = rospy.Rate(self.rate)
self.updater()
def handle_request(self, req):
if len(req) < 3: return
self.last_update = time.time()
data = req[0:-2]
checksum = struct.unpack(">H", req[-2:])[0]
if crc16.crc16(data) != checksum:
translated = "CHECKSUM MISMATCH: " + repr(req)
print "CHECKSUM MISMATCH"
return
if req[1] == cmds.get('debug') :
debug_handler(data)
return
translated = translate_from_robot(data)
if translated == False: return
if self.user_sock.is_open():
self.user_sock.write(translated)
def ReadDatabasePage(self, record_type, page):
record_type_index = constants.RECORD_TYPES.index(record_type)
self.WriteCommand(constants.READ_DATABASE_PAGES,
(chr(record_type_index), struct.pack('I', page), chr(1)))
packet = self.readpacket()
assert ord(packet.command) == 1
# first index (uint), numrec (uint), record_type (byte), revision (byte),
# page# (uint), r1 (uint), r2 (uint), r3 (uint), ushort (Crc)
header_format = '<2I2c4IH'
header_data_len = struct.calcsize(header_format)
header = struct.unpack_from(header_format, packet.data)
header_crc = crc16.crc16(packet.data[:header_data_len-2])
assert header_crc == header[-1]
assert ord(header[2]) == record_type_index
assert header[4] == page
packet_data = packet.data[header_data_len:]
return self.ParsePage(header, packet_data)
def ack(self):
overhead = binascii.b2a_hex(
struct.pack('>BB', self.nr, self.addr)).upper()
data = '\x06' + overhead
crc = crc16.crc16(data)
crc = binascii.b2a_hex(struct.pack('>H', crc)).upper()
data = data + crc + '\x03'
self.logger.info('TX: Protocol ACK NR=%d' % self.nr)
if type(self.con) is socket.socket:
new_data = self.con.send(data)
else:
new_data = self.con.write(data)
if self.tx_led is not None:
try:
with open(self.tx_led, 'w') as led_file:
led_file.write('1')
except:
pass
def ack(self):
overhead = binascii.b2a_hex(struct.pack('>BB', self.nr,
self.addr)).upper()
data = '\x06' + overhead
crc = crc16.crc16(data)
crc = binascii.b2a_hex(struct.pack('>H', crc)).upper()
data = data + crc + '\x03'
self.logger.info('TX: Protocol ACK NR=%d' % self.nr)
if type(self.con) is socket.socket:
new_data = self.con.send(data)
else:
new_data = self.con.write(data)
if self.tx_led is not None:
try:
with open(self.tx_led, 'w') as led_file:
led_file.write('1')
except:
pass
def upload(self, filename, data, crc, compressed = False):
"""This method uploads a file to the server.
@param filename: file name of the file to download.
@param data: the contents of the file being uploaded in base64 format.
@param crc: the CRC-16-IBM (CRC16) Cyclic Redundancy Check for the data.
@param compressed: if True the data will be decompressed with zlib.
"""
# decode the base64 encoded string
data_ = base64.b64decode(data)
# decompress the data if needed
if compressed:
data_ = zlib.decompress(data)
# make sure we recieved what we were expecting by computing the crc value
if crc16(data_) != crc:
self.log.debug('FileTransfer.upload: crc values did not match for request %s' % filename)
raise JSONRPCAssertionError('crc value does not match')
cfg = getConfig(file)
ul_plugins = dict([[x.__class__.__name__, x] for x in self.upload_plugins])
for plugin in cfg['plugins']:
if ul_plugins.has_key(plugin):
plugin = ul_plugins[plugin]
if plugin.handles(cfg['head'], cfg['tail']):
plugin.upload(cfg['head'], cfg['tail'], data_, cfg['rootpath'])
break
else:
self.log.debug("FileTransfer.upload: Skipping plugin %s, not found" % plugin)
else: # default upload handling, just save the data
path = os.path.normpath(os.path.join(cfg['rootpath'], cfg['tail']))
if not os.path.exists(path):
raise ApplicationError('IOError: No such file or directory: %s' % filename)
if not os.isfile(path):
raise ApplicationError('IOError: %s is a directory' % filename)
f = open(path, 'wb')
f.write(data_)
f.close()
def __init__(self, data):
self.data = data[:-4] + binascii.a2b_hex(data[-4:])
offset = crc16.crc16(self.data)
if offset:
raise crc16.CRCError('Offset = %d' % offset)
self.control = self.data[0]
if self.data[0] == '\x15':
self.nr = int(
struct.unpack(">B", str(binascii.a2b_hex(self.data[1:3])))[0])
self.addr = int(
struct.unpack(">B", str(binascii.a2b_hex(self.data[3:5])))[0])
else:
self.ns = int(
struct.unpack(">B", str(binascii.a2b_hex(self.data[1:3])))[0])
self.nr = int(
struct.unpack(">B", str(binascii.a2b_hex(self.data[3:5])))[0])
self.addr = int(
struct.unpack(">B", str(binascii.a2b_hex(self.data[5:7])))[0])
self.mi = binascii.a2b_hex(self.data[8:10])
self.data = binascii.a2b_hex(self.data[10:-2])
def send_packet(self, mi, data):
overhead = binascii.b2a_hex(
struct.pack('>BBB', self.ns, self.nr, self.addr)).upper()
mi = binascii.b2a_hex(struct.pack('>B', mi)).upper()
data = binascii.b2a_hex(data).upper()
data = '\x01' + overhead + '\x02' + mi + data
crc = crc16.crc16(data)
crc = binascii.b2a_hex(struct.pack('>H', crc)).upper()
data = data + crc + '\x03'
self.logger.info('TX: %s' % repr(data))
if type(self.con) is socket.socket:
new_data = self.con.send(data)
else:
new_data = self.con.write(data)
if self.tx_led is not None:
try:
with open(self.tx_led, 'w') as led_file:
led_file.write('1')
except:
pass
def send_packet(self, mi, data):
overhead = binascii.b2a_hex(
struct.pack('>BBB', self.ns, self.nr, self.addr)).upper()
mi = binascii.b2a_hex(struct.pack('>B', mi)).upper()
data = binascii.b2a_hex(data).upper()
data = '\x01' + overhead + '\x02' + mi + data
crc = crc16.crc16(data)
crc = binascii.b2a_hex(struct.pack('>H', crc)).upper()
data = data + crc + '\x03'
self.logger.info('TX: %s' % repr(data))
if type(self.con) is socket.socket:
new_data = self.con.send(data)
else:
new_data = self.con.write(data)
if self.tx_led is not None:
try:
with open(self.tx_led, 'w') as led_file:
led_file.write('1')
except:
pass
def download(self, file, compress = False):
"""This method is used to request a file from the server.
@param file: path of the file to download.
@param compress: if True the contents of the file will be compressed
with zlib before sending.
@return: a JSON-RPC Object with keys data, crc, and compressed.
"""
cfg = getConfig(file)
dl_plugins = dict([[x.__class__.__name__, x] for x in self.download_plugins])
for plugin in cfg['plugins']:
if dl_plugins.has_key(plugin):
plugin = dl_plugins[plugin]
if plugin.handles(cfg['head'], cfg['tail']):
data = plugin.download(cfg['head'], cfg['tail'], cfg['rootpath'])
break
else:
self.log.debug("FileTransfer.download: Skipping plugin %s, not found" % plugin)
else: # default download handling, just send the file
path = os.path.normpath(os.path.join(cfg['rootpath'], cfg['tail']))
if not os.path.exists(path):
raise ApplicationError('IOError: No such file or directory: %s' % file)
if not os.isfile(path):
raise ApplicationError('IOError: %s is a directory' % file)
data = open(path, 'rb').read()
# compute the crc of the data
crc = crc16(data)
# compress the data if so requested
if compress:
data = zlib.compress(data)
# encode the data in base64
data = base64.b64encode(data)
return {'data': data, 'crc': crc}
def lookupStringId(str):
global numStrings
id = numStrings
numStrings += 1
a = array.array('B')
a.fromstring(str)
crc = crc16.crc16(a)
filename = '/tmp/triplestore/stringid%04X' % crc
if not os.path.exists(filename):
stringToId = { str: id }
else:
inf = open(filename)
stringToId = cPickle.load(inf)
inf.close()
if stringToId.has_key(str):
# already got this string
return stringToId[str]
stringToId[str] = id
outf = open(filename, 'w')
cPickle.dump(stringToId, outf)
outf.close()
crc = int(((id >> 16) ^ id) & 0xffff)
filename = '/tmp/triplestore/idstring%04X' % crc
if not os.path.exists(filename):
idToString = { id: str }
else:
inf = open(filename)
idToString = cPickle.load(inf)
inf.close()
idToString[id] = str
outf = open(filename, 'w')
cPickle.dump(idToString, outf)
outf.close()
return id
def lookupStringId(str):
global numStrings
id = numStrings
numStrings += 1
a = array.array('B')
a.fromstring(str)
crc = crc16.crc16(a)
filename = '/tmp/triplestore/stringid%04X' % crc
if not os.path.exists(filename):
stringToId = {str: id}
else:
inf = open(filename)
stringToId = cPickle.load(inf)
inf.close()
if stringToId.has_key(str):
# already got this string
return stringToId[str]
stringToId[str] = id
outf = open(filename, 'w')
cPickle.dump(stringToId, outf)
outf.close()
crc = int(((id >> 16) ^ id) & 0xffff)
filename = '/tmp/triplestore/idstring%04X' % crc
if not os.path.exists(filename):
idToString = {id: str}
else:
inf = open(filename)
idToString = cPickle.load(inf)
inf.close()
idToString[id] = str
outf = open(filename, 'w')
cPickle.dump(idToString, outf)
outf.close()
return id
def readpacket(self, timeout=None):
total_read = 4
initial_read = self.read(total_read)
all_data = initial_read
if ord(initial_read[0]) == 1:
command = initial_read[3]
data_number = struct.unpack('<H', initial_read[1:3])[0]
if data_number > 6:
toread = abs(data_number - 6)
second_read = self.read(toread)
all_data += second_read
total_read += toread
out = second_read
else:
out = ''
suffix = self.read(2)
sent_crc = struct.unpack('<H', suffix)[0]
local_crc = crc16.crc16(all_data, 0, total_read)
if sent_crc != local_crc:
raise constants.CrcError("readpacket Failed CRC check")
num1 = total_read + 2
return ReadPacket(command, out)
else:
raise constants.Error('Error reading packet header!')
def readpacket(self, timeout=None):
total_read = 4
initial_read = self.read(total_read)
all_data = initial_read
if ord(initial_read[0]) == 1:
command = initial_read[3]
data_number = struct.unpack('<H', initial_read[1:3])[0]
if data_number > 6:
toread = abs(data_number-6)
second_read = self.read(toread)
all_data += second_read
total_read += toread
out = second_read
else:
out = ''
suffix = self.read(2)
sent_crc = struct.unpack('<H', suffix)[0]
local_crc = crc16.crc16(all_data, 0, total_read)
if sent_crc != local_crc:
raise constants.CrcError("readpacket Failed CRC check")
num1 = total_read + 2
return ReadPacket(command, out)
else:
raise constants.Error('Error reading packet header!')
def sign_set_graphics_frame(p, session):
frame_id = ord(p.data[0])
rev = ord(p.data[1])
height = ord(p.data[2])
width = ord(p.data[3])
colour = ord(p.data[4])
flash = ord(p.data[5])
length = struct.unpack(">H", p.data[6:8])[0]
data = str(p.data[8:-2])
crc = str(p.data[-2:]).encode('hex')
offset = crc16.crc16(p.mi + p.data)
if offset:
session.logger.warning('Graphics frame CRC error: offset = %d' %
offset)
session.reject(p.mi, '\x04')
return
if frame_id == 0:
session.logger.warning('Graphics frame ID 0 is invalid')
session.reject(p.mi, '\x02')
return
if len(data) != length:
session.logger.warning('Specified data length does not match data')
session.reject(p.mi, '\x16')
return
try:
session.server.vms.set_graphics_frame(frame_id, rev,
(width, height), colour, flash,
data.encode('hex'))
except Exception as e:
try:
error_message = e.message[1]
except:
import traceback
session.logger.error('Unrecognised exception')
session.logger.error(traceback.format_exc())
session.reject(p.mi, '\x02')
else:
if 'SizeException' in e.message[1]:
session.logger.warning('Frame size incorrect')
session.reject(p.mi, '\x16')
return
elif 'ColourException' in e.message[1]:
session.logger.warning('Invalid colour')
session.reject(p.mi, '\x0c')
return
elif 'ConspicuityException' in e.message[1]:
session.logger.warning('Invalid flash pattern')
session.reject(p.mi, '\x11')
return
else:
import traceback
session.logger.error('Unrecognised exception')
session.logger.error(traceback.format_exc())
session.reject(p.mi, '\x02')
session.logger.info('Successfully set sign graphics frame id %d' %
frame_id)
status_reply = rtastatus.generate_sign_status(p, session)
session.send_packet(0x06, status_reply)
import os
from crc16 import crc16
files = [f for f in os.listdir()]
files.sort()
for file in files:
if file in ['.git', '.gitignore', '.idea', '__pycache__']:
continue
s = os.stat(file)
try:
with open(file, 'rb') as f:
h = crc16(f.read())
print('{: <25} {: >6} {: >4}'.format(file, h, s[6]))
except:
print('{: <25} ERROR {: >4}'.format(file, s[6]))
def sign_set_graphics_frame(p, session):
frame_id = ord(p.data[0])
rev = ord(p.data[1])
height = ord(p.data[2])
width = ord(p.data[3])
colour = ord(p.data[4])
flash = ord(p.data[5])
length = struct.unpack(">H", p.data[6:8])[0]
data = str(p.data[8:-2])
crc = str(p.data[-2:]).encode('hex')
offset = crc16.crc16(p.mi + p.data)
if offset:
session.logger.warning(
'Graphics frame CRC error: offset = %d' % offset)
session.reject(p.mi, '\x04')
return
if frame_id == 0:
session.logger.warning('Graphics frame ID 0 is invalid')
session.reject(p.mi, '\x02')
return
if len(data) != length:
session.logger.warning('Specified data length does not match data')
session.reject(p.mi, '\x16')
return
try:
session.server.vms.set_graphics_frame(
frame_id, rev, (width, height), colour,
flash, data.encode('hex'))
except Exception as e:
try:
error_message = e.message[1]
except:
import traceback
session.logger.error('Unrecognised exception')
session.logger.error(traceback.format_exc())
session.reject(p.mi, '\x02')
else:
if 'SizeException' in e.message[1]:
session.logger.warning('Frame size incorrect')
session.reject(p.mi, '\x16')
return
elif 'ColourException' in e.message[1]:
session.logger.warning('Invalid colour')
session.reject(p.mi, '\x0c')
return
elif 'ConspicuityException' in e.message[1]:
session.logger.warning('Invalid flash pattern')
session.reject(p.mi, '\x11')
return
else:
import traceback
session.logger.error('Unrecognised exception')
session.logger.error(traceback.format_exc())
session.reject(p.mi, '\x02')
session.logger.info(
'Successfully set sign graphics frame id %d' % frame_id)
status_reply = rtastatus.generate_sign_status(p, session)
session.send_packet(0x06, status_reply)
def do(self):
# Check if input file exists
if not os.path.isfile(self.hex_file.get()):
showerror(message="Файла \"{0}\" не существует".format(
self.hex_file.get()))
return
else:
self.settings['hex_file'] = self.hex_file.get()
# Verify specified red key ID
red_key_id = self.red_key.get().strip()
if not self.check_hex_value(red_key_id, num_digits=8):
showerror(
message=
"Требуется ИД красного ключа в формате HHHHHHHH, где H - шестнадцатеричная цифра"
)
return
else:
self.settings['red_key'] = self.red_key.get()
# Ask output file name
new_hex_file = filedialog.asksaveasfilename(filetypes=(('hex-файл',
'*.hex'),
('все файлы',
'*.*')))
if not new_hex_file:
return
if new_hex_file[-4:] != '.hex': # add proper extension if not specified
new_hex_file = new_hex_file + '.hex'
app_size = 0x0000 # application size in bytes
crc_line_no = [None, None
] # number of hex line with MSB and LSB respectively
new_hex_lines = [] # buffer for hex line before writing in file
line_cnt = 0 # current hex line number
bin_dump = b'' # binary dump to help to calculate crc
last_add = 0x0000 # address of the last byte written to binary dump
# Read line-by-line input hex and write it out with red key has replaced
with open(self.hex_file.get()) as hf:
for line in hf.readlines():
parsed_hexline = self.read_hexline(
line
) # parse hex line to find out hex address and hex record type
# If record with data
if parsed_hexline["record_type"] == 0x00:
# If record contains red key bytes
if (RED_KEY_ADD <= parsed_hexline["address"] +
parsed_hexline["data_len"] - 1
and parsed_hexline["address"] <=
RED_KEY_ADD + RED_KEY_LEN - 1):
# Calc index of the first character containing red key bytes
col_beg = 9 + (RED_KEY_ADD -
parsed_hexline["address"] if RED_KEY_ADD
>= parsed_hexline["address"] else 0) * 2
# Calc index of the last character containing red key bytes
col_end = 9 + (
RED_KEY_ADD + RED_KEY_LEN - 1 -
parsed_hexline["address"] if RED_KEY_ADD +
RED_KEY_LEN < parsed_hexline["address"] +
parsed_hexline["data_len"] - 1 else
parsed_hexline["data_len"] - 1) * 2 + 1
# Calc index of the first character in red_key_id met in hex record
col_red_key_beg = 0 if RED_KEY_ADD >= parsed_hexline["address"]\
else (parsed_hexline["address"] - RED_KEY_ADD) * 2
# Calc index of the last character in red_key_id met in hex record
col_red_key_end = col_red_key_beg + col_end - col_beg
# Compile new hex line with new red key
line = line[:col_beg] + red_key_id[col_red_key_beg:(
col_red_key_end + 1)] + line[(col_end + 1):]
# Recalculate check sum
parsed_hexline = self.read_hexline(
line, strict_check_sum=False)
# Insert corrected check sum
line = line[:-3] + (
hex(parsed_hexline["check_sum_actual"] +
0x100)[-2:]).upper() + '\n'
# If record contains application size
for i in range(AP_SIZE_LEN):
if parsed_hexline[
"address"] <= AP_SIZE_ADD + i <= parsed_hexline[
"address"] + parsed_hexline["data_len"] - 1:
app_size += parsed_hexline["data"][
AP_SIZE_ADD + i -
parsed_hexline["address"]] * (1 << (i * 8))
# If record contains CRC
for i in range(CRC_LEN):
if parsed_hexline[
"address"] <= CRC_ADD + i <= parsed_hexline[
"address"] + parsed_hexline["data_len"] - 1:
crc_line_no[i] = line_cnt
# Anyway add all data bytes to bin dump to recalculate CRC further
bin_dump += b'\xFF' * (parsed_hexline["address"] -
last_add - 1)
bin_dump += parsed_hexline["data"]
last_add = parsed_hexline["address"] + parsed_hexline[
"data_len"] - 1
new_hex_lines.append(line)
line_cnt += 1
# Recalculate CRC
crc = 0xFFFF
for i in range(app_size):
if not (0x0080 <= i < 0x00C0):
if i < len(bin_dump):
crc = crc16(crc, bin_dump[i])
else:
crc = crc16(crc, 0xFF) # pad with 0xFF
# Insert new CRC byte-by-byte
for i in range(CRC_LEN):
line = new_hex_lines[crc_line_no[i]]
parsed_hexline = self.read_hexline(line)
col_beg = 9 + ((CRC_ADD + i) - parsed_hexline["address"]) * 2
line = line[:col_beg] + (hex(crc >> (
(1 - i) * 8) & 0xFF + 0x10000)).upper()[-2:] + line[(col_beg +
2):]
parsed_hexline = self.read_hexline(
line, strict_check_sum=False) # get new check sum
line = line[:-3] + (hex(parsed_hexline["check_sum_actual"] +
0x100)[-2:]).upper() + '\n'
new_hex_lines[crc_line_no[i]] = line
# Write in the file
with open(new_hex_file, 'w') as nhf:
for line in new_hex_lines:
nhf.write(line)
def get_modbus_response(self):
modbus_response = struct.pack('<19B3H12xB20xl12x3B22x2lhl2h2l50x', self.__yibiaoid, 0x03, 0xAC, 0x00,0, self.__zidong, 0, 0x0A, 0x01, 0x0A, 0x01, 0x0A, 0x01, 0x0A, 0x01, 0x0F, 0x28, 0x46, 0x64, 0, 0, 0xFFFF, 1, int(self.__shengyuliuliang * 10), self.__kaidu, self.__kaixiang, self.__duandian, \
int(self.__shunshiliuliang * 100), int(self.__yali * 1000), int(self.__wendu * 10), int(self.__leijiliuliang * 10), int(self.__pinlv * 10), 0, 0, int(self.__midu * 1000))
checksum = struct.pack('H', crc16.crc16(modbus_response))
return modbus_response + checksum
def AppendCrc(self):
self.SetLength()
ps = self.PacketString()
crc = crc16.crc16(ps, 0, len(ps))
for x in struct.pack('H', crc):
self._packet.append(x)
def calculate_crc(self):
return crc16.crc16(self.raw_data[:-2])
def test_sendToken_crc(self):
t = sendToken(['T'])['response']
crc = t.pop()
self.assertEqual(crc16.crc16(''.join(t)), struct.unpack('<H', crc)[0])
else:
repeat = 1
firmware.append(Sample(fname, role, weight, repeat))
return firmware
if __name__ == "__main__":
output = sys.stdout
if sys.argv < 2:
print >> sys.stderr, 'Syntax: %s <fw.in>' % sys.argv[0]
sys.exit(1)
samples = parse_fwin(sys.argv[1])
descr = ''
pageno = 1
for sample in samples:
sample.page = pageno
pageno += sample.pages()
descr += sample.tobin()
data = SIGNATURE
data += struct.pack('<BH', len(samples), crc16(descr))
data += descr
data = pad_page(data)
output.write(data)
for sample in samples:
data = pad_page(sample.wave.frames)
output.write(data)
try:
portReader = PortReader()
portReader.start()
except (KeyboardInterrupt, SystemExit):
sys.exit()
SLAVE_ADDRESS = 1
MF_READ_INPUT_REGISTER = 4
data = bytearray(8)
data[0] = SLAVE_ADDRESS
data[1] = MF_READ_INPUT_REGISTER
data[2] = 0
data[3] = 0
data[4] = 0
data[5] = 1
crc = crc16(0xFFFF, data, 6)
data[6] = crc & 0xFF
data[7] = crc >> 8
port.write(data)
sys.stdin.readline()
portReader.stop()
sys.exit()
print "Testing... Hello"
# ------------ Start Hello Message ------------
# Request a token
token = getToken()
# Send Hello Message
device_len = struct.pack('<B', len(device_name))
#print len(device_name)
mac_add = ''
for octet in mac_address.split(":"):
mac_add += struct.pack('<B', int(octet, 16))
v_num = struct.pack('<B', version_num)
send_hello = ['H', device_name, mac_add, v_num]
send_hello.append(struct.pack('<H', crc16.crc16(''.join(send_hello) + device_salt + token)))
print send_hello
print ":".join("{0:x}".format(ord(c)) for c in umsgpack.packb(send_hello))
s.send(umsgpack.packb(send_hello))
hello = umsgpack.unpackb(s.recv(1024))
if checkResponce('H', hello):
print "Hello, PASS"
else:
print "Hello, FAIL"
# ------------ Send a Heartbeat ------------
print "\nTesting... HeartBeat"
def calculate_crc(self):
return crc16.crc16(self.raw_data[:-2])
def sendToken(rfidInput):
global token_lock, tokens
t = ['T']
with token_lock:
t.append(tokens[0])
return {'response':['T',t[1],struct.pack('<H', crc16.crc16(''.join(t)))]}
def AppendCrc(self):
self.SetLength()
ps = self.PacketString()
crc = crc16.crc16(ps, 0, len(ps))
for x in struct.pack('H', crc):
self._packet.append(x)
