def unframe(buffer):
""" Decode frame and return data """
# Remove terminating 0
if buffer and buffer[-1] == 0:
buffer.pop()
if IS_PY2:
buffer = bytes(buffer)
try:
decoded = cobs.decode(buffer)
except cobs.DecodeError as err:
log.warning("MSG Decode error {}".format(err))
return False
if not decoded:
# Padding/Fill frame only, don't do anything
return None
expected_crc = decoded[-1]
data = decoded[:-1]
data_crc = crc_func(data)
if IS_PY2:
expected_crc = ord(expected_crc)
if data_crc != expected_crc:
log.warning(
"CRC ERR: Sender crc: {:02x} Received data crc: {:02x}".format(
expected_crc, data_crc))
return False
return data
def recoverpacket(self, pkt):
"""De-construct a packet, check it's CRC and return a struct
containing the fields."""
# check the header and footer is still here
if pkt[0] != int.from_bytes(self.protocol_header, byteorder='big'):
return None
if pkt[len(pkt) - 1] != int.from_bytes(self.protocol_header,
byteorder='big'):
return None
# strip away the 0x00 header and de-cobs/r it
recv_packet = cobsr.decode(pkt[1:-1])
# parse through the structure
try:
pkt_struct = self.message_format.parse(recv_packet)
except Exception:
return None
# verify the hash code
chk = hashlib.sha256(pkt_struct.fields.data).digest()[0:4]
if chk != pkt_struct.checksum:
return None
return pkt_struct.fields.value
def test_zeros(self):
for length in xrange(520):
test_string = '\x00' * length
encoded = cobsr.encode(test_string)
expected_encoded = '\x01' * (length + 1)
self.assertEqual(encoded, expected_encoded,
"encoding zeros failed for length %d" % length)
decoded = cobsr.decode(encoded)
self.assertEqual(decoded, test_string,
"decoding zeros failed for length %d" % length)
def read():
string = ser.read_until(
zeroByte) # read until the COBS packet ending delimiter is found
n = len(string)
if n > 0:
decodeStr = string[:
-1] # take everything except the trailing zero byte, b'\x00'
res = cobs.decode(decodeStr) # recover binary data encoded on Arduino
n_binary = len(res)
if (n_binary == expectedBytes):
return res
def parse_packet(packet):
if len(packet) < 4:
return None, None
#print "Receiving: " + binascii.b2a_hex(packet)
if len(packet) < 18:
packet = cobsr.decode(packet.strip("\x00"))
# Telemetry packet
try:
pk_fields = struct.unpack("<BBBBBHH", packet[3:])
pk_field_names = ("RSSI", "Remote RSSI", "TX Buffer Length",
"Noise", "Remote Noise", "RX Errors",
"RX Errors Fixed")
pk_data = dict(zip(pk_field_names, pk_fields))
return ("rfd900", pk_data)
except Exception:
#print "Failed to parse packet: " + binascii.b2a_hex(packet) + "\n"
#raise
return (None, None)
else:
# State packet
try:
log_data = plog.ParameterLog.deserialize(packet)
pk_data = dict(
(p.parameter_type.name, p.values) for p in log_data
if isinstance(p, plog.DataParameter))
refp = log_data.find_by(
device_id=0,
parameter_type=plog.ParameterType.FCS_PARAMETER_KEY_VALUE)
if refp and refp.key == plog.FCS_PARAMETER_KEY_WAYPOINT:
pk_data["_reference_waypoint"] = plog.extract_waypoint(refp.value)
return ("state", pk_data)
except Exception:
print "Failed to parse packet: " + repr(packet)
#raise
return (None, None)
def test_random(self):
try:
for _test_num in xrange(self.NUM_TESTS):
length = random.randint(0, self.MAX_LENGTH)
test_string = ''.join(
chr(random.randint(0, 255)) for x in xrange(length))
encoded = cobsr.encode(test_string)
self.assertTrue(
'\x00' not in encoded,
"encoding contains zero byte(s):\noriginal: %s\nencoded: %s"
% (repr(test_string), repr(encoded)))
self.assertTrue(
len(encoded) <= len(test_string) + 1 +
(len(test_string) // 254),
"encoding too big:\noriginal: %s\nencoded: %s" %
(repr(test_string), repr(encoded)))
decoded = cobsr.decode(encoded)
self.assertEqual(
decoded, test_string,
"encoding and decoding random data failed:\noriginal: %s\ndecoded: %s"
% (repr(test_string), repr(decoded)))
except KeyboardInterrupt:
pass
def deserialize(cls, data):
data = cobsr.decode(data.strip('\x00'))
data_crc = struct.pack("<L" , binascii.crc32(data[:-4]) & 0xFFFFFFFF)
if data[-4:] != data_crc:
raise ValueError("CRC mismatch: provided %s, calculated %s" %
(binascii.b2a_hex(data[-4:]),
binascii.b2a_hex(data_crc)))
log_type, _, tick = struct.unpack("<BHH", data[0:5])
data = data[5:]
result = cls(log_type=LogType(log_type), tick=tick)
while len(data) > 4:
param, tail = Parameter.deserialize(data)
if param:
result.append(param)
if data == tail:
break
data = tail
return result
def deserialize(cls, data):
data = cobsr.decode(data.strip('\x00'))
data_crc = struct.pack("<L", binascii.crc32(data[:-4]) & 0xFFFFFFFF)
if data[-4:] != data_crc:
raise ValueError(
"CRC mismatch: provided %s, calculated %s" %
(binascii.b2a_hex(data[-4:]), binascii.b2a_hex(data_crc)))
log_type, _, tick = struct.unpack("<BHH", data[0:5])
data = data[5:]
result = cls(log_type=LogType(log_type), tick=tick)
while len(data) > 4:
param, tail = Parameter.deserialize(data)
if param:
result.append(param)
if data == tail:
break
data = tail
return result
def test_decode_predefined_encodings(self):
for (test_string,
expected_encoded_string) in self.predefined_encodings:
decoded = cobsr.decode(expected_encoded_string)
self.assertEqual(test_string, decoded)
def print_c_data(self):
print ''.join(('\\x%02x' % ord(c)) for c in
cobsr.decode(self.serialize())[:-4])
def print_c_data(self):
print ''.join(
('\\x%02x' % ord(c)) for c in cobsr.decode(self.serialize())[:-4])
