def test_calculation_crc_error2(self):
# Frame with CRC error
# **
# B5 62 01 11 14 00 50 2b d7 11 01 00 00 00 00 00 00 00 01 00 00 4c 03 00 00 00 8e 2b
# hdr | <-- checksum --> | chksum
# Frame looks ok, except ecefVZ, guess 4c should be 00
dut = Checksum()
frame = [
0x01, 0x11, 0x14, 0x00, 0x50, 0x2b, 0xd7, 0x11, 0x01, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x4c, 0x03, 0x00,
0x00, 0x00
]
for byte in frame:
dut.add(byte)
assert dut.matches(0xda, 0xa7) # Checksum for incorrect frame
# Fix faulty byte at position 19: Wrong 0x4C, correct 0x00
dut.reset()
frame[19] = 0x00
for byte in frame:
dut.add(byte)
print(f'{dut._cka:02x} {dut._ckb:02x}')
assert dut.matches(0x8e, 0x2b)
def test_calculation_crc_error1(self):
# Frame with CRC error
# **
# B5 62 01 00 14 00 54 a0 06 0f 00 00 00 00 00 00 00 00 01 00 00 00 03 00 00 00 33 46
# hdr | <-- checksum --> | chksum
# No frame 0x01 0x00 found
# 0x11 missing as ID could explain checksum difference 22bf vs 3346. Check with test
dut = Checksum()
frame = [
0x01, 0x00, 0x14, 0x00, 0x54, 0xa0, 0x06, 0x0f, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x03, 0x00,
0x00, 0x00
]
for byte in frame:
dut.add(byte)
assert dut.matches(0x22, 0xBF) # Checksum for incorrect frame
# Fix faulty byte at position 1: Wrong 0x00, correct 0x11
dut.reset()
frame[1] = 0x11
for byte in frame:
dut.add(byte)
# print(f'{dut._cka:02x} {dut._ckb:02x}')
assert dut.matches(0x33, 0x46)
def test_calculation_crc_error3(self):
# Frame with CRC error
#
# B5 62 01 01 14 00 54 a0 06 ff 02 4e 92 19 0f f6 94 03 a5 2c d0 1b 44 00 00 00 b6 b1
# hdr | <-- checksum --> | chksum
# iTOW of next messages is 54 a0 06 0f instead of .. .. .. ff
dut = Checksum()
frame = [
0x01, 0x01, 0x14, 0x00, 0x54, 0xa0, 0x06, 0xff, 0x02, 0x4e, 0x92,
0x19, 0x0f, 0xf6, 0x94, 0x03, 0xa5, 0x2c, 0xd0, 0x1b, 0x44, 0x00,
0x00, 0x00
]
for byte in frame:
dut.add(byte)
assert dut.matches(0xa6, 0xa1) # Checksum for incorrect frame
# Fix faulty byte at position 19: Wrong 0x4C, correct 0x00
dut.reset()
frame[7] = 0x0F
for byte in frame:
dut.add(byte)
# print(f'{dut._cka:02x} {dut._ckb:02x}')
assert dut.matches(0xb6, 0xb1)
def __init__(self, rx_queue):
super().__init__()
self.rx_queue = rx_queue
self.checksum = Checksum()
self._reset()
self.state = __class__.State.INIT
def __init__(self, crc_error_cid):
super().__init__()
self.crc_error_cid = crc_error_cid
self.rx_queue = list()
self.wait_cids = None
self.checksum = Checksum()
self.frames_rx = 0
self.state = __class__.State.INIT
self._reset()
def test_reset(self):
dut = Checksum()
dut.add(0xF0)
dut.add(0xE0)
dut.reset()
assert dut.matches(0x00, 0x00)
def test_calculation(self):
# B5 62 13 40 18 00 10 00 00 12 E4 07 09 05 06 28 30 00 40 28 EF 0C 0A 00 00 00 00 00 00 00 51 AC
# hdr | <-- checksum --> | chksum
dut = Checksum()
frame = [
0x13,
0x40,
0x18,
0x00,
0x10,
0x00,
0x00,
0x12,
0xE4,
0x07,
0x09,
0x05,
0x06,
0x28,
0x30,
0x00,
0x40,
0x28,
0xEF,
0x0C,
0x0A,
0x00,
0x00,
0x00,
0x00,
0x00,
0x00,
0x00,
]
for byte in frame:
dut.add(byte)
assert dut.matches(0x51, 0xAC)
def __init__(self):
super().__init__()
self.data = bytearray()
# TODO: Do we need checksum as member?
self.checksum = Checksum()
self.f = Fields()
class UbxFrame(object):
CID = UbxCID(0, 0)
NAME = 'UBX'
SYNC_1 = 0xb5
SYNC_2 = 0x62
@classmethod
def construct(cls, data):
obj = cls()
obj.data = data
obj.unpack()
return obj
@classmethod
def MATCHES(cls, cid):
return cls.CID == cid
def __init__(self):
super().__init__()
self.data = bytearray()
# TODO: Do we need checksum as member?
self.checksum = Checksum()
self.f = Fields()
def to_bytes(self):
self._calc_checksum()
msg = bytearray([UbxFrame.SYNC_1, UbxFrame.SYNC_2])
msg.append(self.CID.cls)
msg.append(self.CID.id)
length = len(self.data)
msg.append((length >> 0) % 0xFF)
msg.append((length >> 8) % 0xFF)
msg += self.data
msg.append(self.cka)
msg.append(self.ckb)
return msg
def get(self, name):
return self.f.get(name)
def pack(self):
self.data = self.f.pack()
def unpack(self):
self.f.unpack(self.data)
def _calc_checksum(self):
self.checksum.reset()
self.checksum.add(self.CID.cls)
self.checksum.add(self.CID.id)
length = len(self.data)
self.checksum.add((length >> 0) & 0xFF)
self.checksum.add((length >> 8) & 0xFF)
for d in self.data:
self.checksum.add(d)
self.cka, self.ckb = self.checksum.value()
def __str__(self):
res = f'{self.NAME} {self.CID}'
res += str(self.f)
return res
class UbxParser(object):
"""
Parser that tries to extract UBX frames from arbitrary byte streams
Byte streams can also be NMEA or other frames. Unfortunately,
u-blox frame header also appears in NMEA frames (e.g. version
information). Such data will be detected by a checksum error
TODO: Do more elaborate parsing to filter out such data in advance
"""
class State(Enum):
"""
Parser states
"""
INIT = 1
SYNC = 2
CLASS = 3
ID = 4
LEN1 = 5
LEN2 = 6
DATA = 7
CRC1 = 8
CRC2 = 9
def __init__(self, rx_queue):
super().__init__()
self.rx_queue = rx_queue
self.checksum = Checksum()
self._reset()
self.state = __class__.State.INIT
def process(self, data):
for d in data:
if self.state == __class__.State.INIT:
if d == UbxFrame.SYNC_1:
self.state = __class__.State.SYNC
elif self.state == __class__.State.SYNC:
if d == UbxFrame.SYNC_2:
self._reset()
self.state = __class__.State.CLASS
else:
self.state = __class__.State.INIT
elif self.state == __class__.State.CLASS:
self.msg_class = d
self.checksum.add(d)
self.state = __class__.State.ID
elif self.state == __class__.State.ID:
self.msg_id = d
self.checksum.add(d)
self.state = __class__.State.LEN1
elif self.state == __class__.State.LEN1:
self.msg_len = d
self.checksum.add(d)
self.state = __class__.State.LEN2
elif self.state == __class__.State.LEN2:
self.msg_len = self.msg_len + (d * 256)
self.checksum.add(d)
# TODO: Handle case with len = 0 -> goto CRC directly
# TODO: Handle case with unreasonable size
self.ofs = 0
self.state = __class__.State.DATA
elif self.state == __class__.State.DATA:
self.msg_data.append(d)
self.checksum.add(d)
self.ofs += 1
if self.ofs == self.msg_len:
self.state = __class__.State.CRC1
elif self.state == __class__.State.CRC1:
self.cka = d
self.state = __class__.State.CRC2
elif self.state == __class__.State.CRC2:
self.ckb = d
# if checksum matches received checksum put frame in receive queue
if self.checksum.matches(self.cka, self.ckb):
# Send CID and data as tuple to server
self.rx_queue.put((UbxCID(self.msg_class,
self.msg_id), self.msg_data))
else:
logger.warning(f'checksum error in frame, discarding')
self._reset()
self.state = __class__.State.INIT
def _reset(self):
self.msg_class = 0
self.msg_id = 0
self.msg_len = 0
self.msg_data = bytearray()
self.cka = 0
self.ckb = 0
self.ofs = 0
self.checksum.reset()
class UbxParser(object):
"""
Parser that tries to extract UBX frames from arbitrary byte streams
Byte streams can also be NMEA or other frames. Unfortunately,
u-blox frame header also appears in NMEA frames (e.g. version
information). Such data will be detected by a checksum error
TODO: Do more elaborate parsing to filter out such data in advance
"""
""" Maximum message length supported """
MAX_MESSAGE_LENGTH = 1000
class State(Enum):
""" Parser states """
INIT = 1
SYNC = 2
CLASS = 3
ID = 4
LEN1 = 5
LEN2 = 6
DATA = 7
CRC1 = 8
CRC2 = 9
def __init__(self, crc_error_cid):
super().__init__()
self.crc_error_cid = crc_error_cid
self.rx_queue = list()
self.wait_cids = None
self.checksum = Checksum()
self.frames_rx = 0
self.state = __class__.State.INIT
self._reset()
def restart(self):
self.state = __class__.State.INIT
def set_filter(self, cid):
assert isinstance(cid, UbxCID)
self.wait_cids = [cid] # Put single filter in list
def set_filters(self, cids):
assert isinstance(cids, list)
self.wait_cids = cids
def empty_queue(self):
self.rx_queue.clear()
def packet(self):
# Returns (cid, data) or (None, None)
try:
return self.rx_queue.pop(0)
except IndexError:
# No more frames to de-queue
return (None, None)
def process(self, data):
for d in data:
self._process_byte(d)
def _process_byte(self, data):
if self.state == __class__.State.INIT:
self._state_init(data)
elif self.state == __class__.State.SYNC:
self._state_sync(data)
elif self.state == __class__.State.CLASS:
self._state_class(data)
elif self.state == __class__.State.ID:
self._state_id(data)
elif self.state == __class__.State.LEN1:
self._state_len1(data)
elif self.state == __class__.State.LEN2:
self._state_len2(data)
elif self.state == __class__.State.DATA:
self._state_data(data)
elif self.state == __class__.State.CRC1:
self._state_crc1(data)
elif self.state == __class__.State.CRC2:
self._state_crc2(data)
def _state_init(self, d):
if d == UbxFrame.SYNC_1:
self.state = __class__.State.SYNC
def _state_sync(self, d):
if d == UbxFrame.SYNC_2:
self._reset()
self.state = __class__.State.CLASS
else:
self.state = __class__.State.INIT
def _state_class(self, d):
# TODO: Could add check for SYNC_1, SYNC_2 here, as both are not valid classes or IDs
self.msg_class = d
self.checksum.add(d)
self.state = __class__.State.ID
def _state_id(self, d):
self.msg_id = d
self.checksum.add(d)
self.state = __class__.State.LEN1
def _state_len1(self, d):
self.msg_len = d
self.checksum.add(d)
self.state = __class__.State.LEN2
def _state_len2(self, d):
self.msg_len = self.msg_len + (d * 256)
self.checksum.add(d)
if self.msg_len == 0:
self.state = __class__.State.CRC1
elif self.msg_len > __class__.MAX_MESSAGE_LENGTH:
logger.warning(f'invalid msg len {self.msg_len}')
self.state = __class__.State.INIT
else:
self.ofs = 0
self.state = __class__.State.DATA
def _state_data(self, d):
self.msg_data.append(d)
self.checksum.add(d)
self.ofs += 1
if self.ofs == self.msg_len:
self.state = __class__.State.CRC1
def _state_crc1(self, d):
self.cka = d
self.state = __class__.State.CRC2
def _state_crc2(self, d):
self.ckb = d
# if checksum matches received checksum ..
if self.checksum.matches(self.cka, self.ckb):
self.frames_rx += 1
# .. and frame passes filter ..
cid = UbxCID(self.msg_class, self.msg_id)
if self.wait_cids and cid in self.wait_cids:
# .. queue packet (CID and data)
packet = (cid, self.msg_data)
self.rx_queue.append(packet)
else:
if logger.isEnabledFor(logging.DEBUG):
logger.debug(
f'no match - dropping {cid}, {self.msg_len} bytes')
else:
logger.warning('checksum error in frame, discarding')
logger.warning(
f'{self.msg_class:02x} {self.msg_id:02x} {binascii.hexlify(self.msg_data)}'
)
crc_error_message = (self.crc_error_cid, None)
self.rx_queue.append(crc_error_message)
self.state = __class__.State.INIT
def _reset(self):
self.msg_class = 0
self.msg_id = 0
self.msg_len = 0
self.msg_data = bytearray()
self.cka = 0
self.ckb = 0
self.ofs = 0
self.checksum.reset()
def test_creation(self):
dut = Checksum()
assert dut.matches(0x00, 0x00)