def _calc_crc(self):
"""Calculate the chunk crc."""
bytefield = []
# checksum runs over chunk name and data, so append chunk name first
for c in self._name:
bytefield.append(pybin.ctoi(c))
# append data
bytefield += self._data
# calculate checksum and return
return crc.crc32(bytefield)
def __calc_crc(self, p, n=32, polynomial=0x04C11DB7):
"""
Create n-bit CRC Checksum
:param n the number of bits in the checksum (default is 32)
:param p the bit sequence as a hex string, or int to create the checksum for
For example, bytes 0 to 46 in the 51 byte sensor packet
:param polynomial the bit string of the CRC polynomial to use
Default is the typical CRC-32 polynomial. CRC-4 often uses 0x03
:return The n-bit checksum as a hexadecimal string
"""
if n == 8:
return crc.crc8(p, polynomial, self.crc8_table)
elif n == 32:
csum = crc.crc32(p, polynomial, self.crc32_table)
return [(csum & 0xFF000000) >> 24, (csum & 0x00FF0000) >> 16,
(csum & 0x0000FF00) >> 8, csum & 0x000000FF]
def __check_crc32(self, crc_val, p):
"""
Check crc Checksum with 32 bits
:param crc the 32 bit checksum as a list of bytes (ints) or an int
:param p the list of bytes to compare to the checksum. (This is bytes 0 to 46 in the 53 byte sensor packet)
:param polynomial the bit string of the crc polynomial to use
Default is 0x04C11DB7 which is what we use for n=32
:return True if the checksum matches, False otherwise
"""
if type(p) == int:
p = self.toHex(p)
p = self.toBytesList(p)
if type(crc_val) != int:
crc_val = self.toInt(crc_val)
checksum = crc.crc32(p, table=self.parent.crc32_table)
return checksum == crc_val
def add_chunk_header(file, tag):
global chunk_loc
chunks.append(file)
f = open(file, 'rb')
chunk = f.read()
f.close()
add_word(chunk_loc
) # chunk header starts with the offset of the chunk in the file
add_word(len(chunk)) # then its length
add_word(crc.crc32(chunk)) # then its CRC32
add_ascii(tag) # to finish its tag
fill(8 - len(tag), 0) # fill the space until the string is 8 bytes long
chunk_loc += (len(chunk) + 0x3) & (
~0x3
) # chunks are 4 bytes aligned to prevent potential crashes (with aligned memcpy for example)
def check_crc(self, crcval, p, n=32):
"""
Check crc Checksum with 8 or 32 bits
:param crc the n bit checksum as a list of bytes (ints) or an int
:param p the list of bytes to compare to the checksum. (This is bytes 0 to 46 in the 53 byte sensor packet)
:param polynomial the bit string of the crc polynomial to use
Default is 0x04C11DB7 which is what we use for n=32. For n=8, 0x07 is used
:return True if the checksum matches, False otherwise
"""
if type(p) == int:
p = self.toBytesList(self.toHex(p))
if type(crcval) != int:
crcval = self.to_int(crcval)
if n == 8:
checksum = crc.crc8(p, table=self.crc8_table)
elif n == 32:
checksum = crc.crc32(p, table=self.crc32_table)
return checksum == crcval
timeout = 100
start = time.time()
d = s.read(1)
while d != b'\xAA' and timeout != 0:
d = s.read(1)
timeout -= 1
if timeout == 0:
outer_timeout -= 1
continue
d = codecs.encode(s.read(2), 'hex') #Read 2 bytes (counter and crc)
if len(d) == 4 and int(d[2:], 16) == crc.crc8(
int('aa' + d[:2], 16), table=crc8_table): #Check CRC 8
data = read_in(s, 53)
if len(data) == 53 and to_int(data[49:]) == crc.crc32(
data[:49], table=crc32_table): #Check CRC 32
try:
rid = rep_ids.get(data[36])
except KeyError:
print('Invalid report ID key')
dat = out.sensor_output(None, data, rid, calMatrix)
temp.append(dat.temperature)
atiData.append(atiSensor.read())
diff.append(dat.differential)
sums.append(dat.sum)
pos2.append(p.get_current_joint_position()[2])
pos3.append(p.get_current_joint_position()[3])
pos4.append(p.get_current_joint_position()[4])
pos5.append(p.get_current_joint_position()[5])
jawPos.append(p.get_current_jaw_position())
if x == [False, False]: data.insert(i*2, True) return decode(data) if x == [True, False]: bits.append(False) if x == [False, True]: bits.append(True) return bits if __name__ == "__main__": import struct samples = getMessageSamples() cleaned = clean(samples) figure() t = linspace(0, len(cleaned)/2.4e6, len(cleaned)) tstart = t[(numpy.abs(cleaned) > 0.5).argmax()] tstop = t[-(numpy.abs(cleaned) > 0.5)[::-1].argmax()] vlines(tstart, 0, 1, color='r', linestyles='solid', lw = 10) vlines(tstop, 0, 1, color='k', linestyles='solid', lw = 10) plot(t, numpy.abs(cleaned)) demoded = demod(cleaned) decoded = decode(demoded) print len(decoded)/(tstop-tstart) print decoded[0:-32].tobytes() import crc CRCofMessage = hex(crc.crc32(decoded[0:-32].tobytes()))[2::] CRCfromMessage = ''.join([hex(ord(x))[2::] for x in decoded[-32::].tobytes()]) print CRCofMessage, CRCfromMessage print "good?", CRCofMessage == CRCfromMessage show()
def hexdump(inname, outname=None, width=20):
infile = open(inname, 'r')
contents = infile.read()
infile.close()
if outname == None:
outname = inname + '.b'
output = ''
# check for magic bytes
head = contents[:8]
contents = contents[8:]
for i in range(8):
if not ctoi(head[i:i + 1]) == magic_numbers[i]:
print 'ERROR:',
print '"{}" does not appear to be a PNG file'.format(filename)
close(infile)
return True
for c in head:
output += itos(ctoi(c)) + ' '
output += '; PNG file magic numbers\n'
while len(contents) >= 12:
# each pass will read and print one chunk
output += '\n'
lenfield = contents[:4]
length = ntoi(lenfield) + 12
chunk = contents[:length]
contents = contents[length:]
name = chunk[4:8]
data = chunk[8:-4]
crcfield = chunk[-4:]
crc = ntoi(crcfield)
# verify data integrity
bytefield = []
for c in name:
bytefield.append(ctoi(c))
for c in data:
bytefield.append(ctoi(c))
if crc32(bytefield) == crc:
crc = True
else:
crc = False
# add length to output
for c in lenfield:
output += itos(ctoi(c))
output += ' '
output += '; chunk length: '
output += str(length - 12)
output += ' bytes\n'
# add name to output
for c in name:
output += itos(ctoi(c))
output += ' '
output += '; chunk name: "'
output += name
output += '"\n\n; ##### Begin Chunk Data #####\n'
# add chunk data
counter = 0
for c in data:
output += itos(ctoi(c))
counter += 1
if counter == width:
counter = 0
output += '\n'
else:
output += ' '
if counter != 0:
output += '\n'
output += '; ##### End Chunk Data #####\n\n'
# add crc info
for c in crcfield:
output += itos(ctoi(c))
output += ' '
output += '; '
if crc:
output += 'crc verified'
else:
output += 'data section currupt'
print '"{}" chunk has corrupt data section'.format(name)
output += '\n\n'
outfile = open(outname, 'w')
outfile.write(output)
outfile.close()
def sendMessage(message="hello there, world!", sym=chr(104)):
crcMsg = crc.crc32(message) | 1 << 33
crcStr = hexToByte(hex(crcMsg)[3::][0:-1])
r.sendStringAsk(message + crcStr, ord(sym))
print[hex(ord(x))[2::] for x in message + crcStr]
def hexdump(inname, outname = None, width = 20):
infile = open(inname, 'r')
contents = infile.read()
infile.close()
if outname == None:
outname = inname + '.b'
output = ''
# check for magic bytes
head = contents[:8]
contents = contents[8:]
for i in range(8):
if not ctoi(head[i:i+1]) == magic_numbers[i]:
print 'ERROR:',
print '"{}" does not appear to be a PNG file'.format(filename)
close(infile)
return True
for c in head:
output += itos(ctoi(c)) + ' '
output += '; PNG file magic numbers\n'
while len(contents) >= 12:
# each pass will read and print one chunk
output += '\n'
lenfield = contents[:4]
length = ntoi(lenfield) + 12
chunk = contents[:length]
contents = contents[length:]
name = chunk[4:8]
data = chunk[8:-4]
crcfield = chunk[-4:]
crc = ntoi(crcfield)
# verify data integrity
bytefield = []
for c in name:
bytefield.append(ctoi(c))
for c in data:
bytefield.append(ctoi(c))
if crc32(bytefield) == crc:
crc = True
else:
crc = False
# add length to output
for c in lenfield:
output += itos(ctoi(c))
output += ' '
output += '; chunk length: '
output += str(length - 12)
output += ' bytes\n'
# add name to output
for c in name:
output += itos(ctoi(c))
output += ' '
output += '; chunk name: "'
output += name
output += '"\n\n; ##### Begin Chunk Data #####\n'
# add chunk data
counter = 0
for c in data:
output += itos(ctoi(c))
counter += 1
if counter == width:
counter = 0
output += '\n'
else:
output += ' '
if counter != 0:
output += '\n'
output += '; ##### End Chunk Data #####\n\n'
# add crc info
for c in crcfield:
output += itos(ctoi(c))
output += ' '
output += '; '
if crc:
output += 'crc verified'
else:
output += 'data section currupt'
print '"{}" chunk has corrupt data section'.format(name)
output += '\n\n'
outfile = open(outname, 'w')
outfile.write(output)
outfile.close()
def MyCodeCheck(code):
if code == None or len(code) < 8:
return False
code_sign = struct.unpack('<I', code[-4:])[0]
code_crc = crc.crc32(code[:-4])
return code_crc ^ code_sign == append_sign
def MyCodeSign(code):
return code + struct.pack('<I', crc.crc32(code) ^ append_sign)
def sendMessage(message = "hello there, world!", sym = chr(104)): crcMsg = crc.crc32(message)|1<<33 crcStr = hexToByte(hex(crcMsg)[3::][0:-1]) r.sendStringAsk(message+crcStr, ord(sym)) print [hex(ord(x))[2::] for x in message+crcStr]
def test_cont(s):
calMatrix = np.eye(6)
count = 0
outer_timeout = N_SAMPLES * 10
#Generate byte message with sample rate
hz = toHex(HZ)
print(hz)
while not len(hz) == 4:
hz = '0' + hz
b = b'' + toStr([0x10, int(hz[:2], 16), int(hz[2:], 16)])
#Start timing, write, and read
startt = time.time()
s.write(b)
print('Started transmission')
times = []
while count < N_SAMPLES and outer_timeout != 0:
timeout = 100
start = time.time()
d = s.read(1)
while d != b'\xAA' and timeout != 0:
d = s.read(1)
timeout -= 1
# print('Read and ignored 1 byte: ' + str(d))
if timeout == 0:
outer_timeout -= 1
continue
d = codecs.encode(s.read(2), 'hex') #Read 2 bytes (counter and crc)
# print(d)
if len(d) == 4 and int(d[2:], 16) == crc.crc8(
int('aa' + d[:2], 16), table=crc8_table): #Check CRC 8
data = read_in(s, 53)
if len(data) == 53 and to_int(data[49:]) == crc.crc32(
data[:49], table=crc32_table): #Check CRC 32
try:
rid = rep_ids.get(data[36])
except KeyError:
print('Invalid report ID key')
out.sensor_output(None, data, rid, calMatrix) #.string()
count += 1
else:
print('CRC-32 failed')
else:
print('CRC-8 failed')
outer_timeout -= 1
times.append(time.time() - start)
s.write(b'\x11\x00\x00\xC9') #stop transmission
count = 1
while count < 100 and s.read(1) != '':
s.write(b'\x11\x00\x00\xC9') #stop transmission
count += 1
s.ftdi_fn.ftdi_usb_purge_buffers()
if count == 100:
print('Failed to stop. Still sending data.')
print(time.time() - startt)
return times
def read(self):
self.posData = []
self.cartPosData = []
self.cartOrnData = []
self.effData = []
self.diff = []
self.sums = []
self.temp = []
self.atiData = []
sPos = self.insertion
outer_timeout = 45 * 1000 * 10
while self.insertion >= 0.5 * sPos and (outer_timeout != 0):
timeout = 100
d = self.s.read(1)
while d != b'\xAA' and timeout != 0:
d = self.s.read(1)
timeout -= 1
if timeout == 0:
outer_timeout -= 1
continue
d = codecs.encode(self.s.read(2),
'hex') #Read 2 bytes (counter and crc)
if len(d) == 4 and int(d[2:], 16) == crc.crc8(
int('aa' + d[:2],
16), table=self.crc8_table): #Check CRC 8
data = read_in(self.s, 53)
if len(data) == 53 and to_int(data[49:]) == crc.crc32(
data[:49], table=self.crc32_table): #Check CRC 32
try:
rid = self.rep_ids.get(data[36])
except KeyError:
print('Invalid report ID key')
dat = out.sensor_output(None, data, rid, self.calMatrix)
pos = np.append(self.arm.get_current_joint_position(),
self.arm.get_current_jaw_position())
cartesianPos = self.arm.get_current_position()
cartPos = [float(i) for i in cartesianPos.p]
cartOr = cartesianPos.M.GetQuaternion()
vel = np.append(self.arm.get_current_joint_velocity(),
self.arm.get_current_jaw_velocity())
eff = np.append(self.arm.get_current_joint_effort(),
self.arm.get_current_jaw_effort())
self.posData.append(pos)
self.cartPosData.append(cartPos)
self.cartOrnData.append(cartOr)
self.velData.append(vel)
self.effData.append(eff)
self.atiData.append(self.atiSensor.read())
self.temp.append(dat.temperature)
self.diff.append(dat.differential)
self.sums.append(dat.sum)
else:
print('CRC-32 failed')
else:
print('CRC-8 failed')
if outer_timeout == 0:
print('outer timeout')
self.numdatPoints += len(self.diff)
