def unpack(self):
# Dynamically build fields based on message length
self.f = Fields()
self.f.add(U1('msgVer'))
self.f.add(U1('numTrkChHw'))
self.f.add(U1('numTrkChUse'))
self.f.add(U1('numConfigBlocks'))
# Extract upto this place to read number of config blocks
super().unpack()
# TODO: Check extra length against numConfigBlocks
# TODO: raise on mismatch
"""
extra_length = len(self.data) - 40
extra_info = int(extra_length / 30)
"""
# TODO: check nested fields -> unit test
for i in range(self.f.numConfigBlocks):
self.f.add(U1(f'gnssId_{i}'))
self.f.add(U1(f'resTrkCh_{i}'))
self.f.add(U1(f'maxTrkCh_{i}'))
self.f.add(Padding(1, f'res1_{i}'))
self.f.add(X4(f'flags_{i}')) # Bit 0: enable
super().unpack()
def __init__(self):
super().__init__()
self.f = Fields()
self.f.add(U4('timeTag'))
self.f.add(X2('flags'))
self.f.add(U2('id'))
self.f.add(X4('data'))
def __init__(self):
super().__init__()
self.f = Fields()
self.f.add(U4('iTow'))
self.f.add(U1('version'))
self.f.add(X1('bitfield0'))
self.f.add(Padding(2, 'res1'))
self.f.add(I4('yaw')) # 1e-1 -> more likely 1e-2
self.f.add(I2('pitch')) # 1e-1
self.f.add(I2('roll')) # 1e-1
def __init__(self):
super().__init__()
self.f = Fields()
self.f.add(U4('iTow'))
self.f.add(U1('version'))
self.f.add(U1_Flags('flags'))
self.f.add(U1('error'))
self.f.add(Padding(1, 'res1'))
self.f.add(U4('yaw')) # 1e-2, 0..+360
self.f.add(I2('pitch')) # 1e-2, -90..+90
self.f.add(I2('roll')) # 1e-2, -180..180
def __init__(self):
super().__init__()
self.f = Fields()
self.f.add(U4('iTow'))
self.f.add(U1_GpsFix('gpsFix'))
self.f.add(X1_Flags('flags'))
self.f.add(X1('fixStat'))
self.f.add(X1('flags2'))
self.f.add(U4('ttff'))
self.f.add(U4('msss'))
def unpack(self):
# Dynamically build fields based on message length
self.f = Fields()
self.f.add(CH(30, 'swVersion'))
self.f.add(CH(10, 'hwVersion'))
extra_length = len(self.data) - 40
extra_info = int(extra_length / 30)
for i in range(extra_info):
self.f.add(CH(30, f'extension_{i}'))
super().unpack()
def test_chars_invalid_length(self):
u = Fields()
u.add(CH(5, 'string'))
with pytest.raises(ValueError):
u.unpack(bytearray.fromhex('31 32')) # Too few bytes
with pytest.raises(ValueError):
u.unpack(bytearray.fromhex('31 32 33 34')) # Too few bytes
def test_chars_too_long(self):
u = Fields()
u.add(CH(6, 'string'))
u.string = '1234567' # One byte too long
with pytest.raises(ValueError):
u.pack()
class UbxMonVer(UbxMonVer_):
def __init__(self):
super().__init__()
# fields defined in unpack as they are dynamic
def unpack(self):
# Dynamically build fields based on message length
self.f = Fields()
self.f.add(CH(30, 'swVersion'))
self.f.add(CH(10, 'hwVersion'))
extra_length = len(self.data) - 40
extra_info = int(extra_length / 30)
for i in range(extra_info):
self.f.add(CH(30, f'extension_{i}'))
super().unpack()
def test_set_field3(self):
u = Fields()
u.add(I4('test'))
u.add(U1('val'))
u.test = 0x12345678
u.val = 0xFF
assert u._fields['test'].value == 0x12345678
assert u._fields['val'].value == 0xFF
assert u.test == 0x12345678
assert u.val == 0xFF
def unpack(self):
# Dynamically build fields based on message length
self.f = Fields()
self.f.add(U1('version'))
self.f.add(U1('numConfigs'))
self.f.add(Padding(2, 'res1'))
# Extract upto this place to read number of level arm configurations
super().unpack()
assert self.f.numConfigs <= 5
# Build final list
for lever in range(self.f.numConfigs):
self.f.add(U1_LeverArmType(f'leverArmType_{lever}'))
self.f.add(Padding(1, f'res2_{lever}'))
self.f.add(I2(f'leverArmX_{lever}'))
self.f.add(I2(f'leverArmY_{lever}'))
self.f.add(I2(f'leverArmZ_{lever}'))
super().unpack()
def test_no_duplicate_fields(self):
u = Fields()
u.add(I4('test1'))
# Must fail as name 'test1' is already used
with pytest.raises(KeyError):
u.add(I4('test1'))
def test_chars_padding_added(self):
u = Fields()
u.add(CH(6, 'string'))
u.string = '1234'
data = u.pack()
assert data == bytearray.fromhex('31 32 33 34 00 00')
def test_basic_pack(self):
u = Fields()
u.add(CH(4, 'string'))
u.string = '1234'
data = u.pack()
assert data == bytearray.fromhex("31 32 33 34")
def unpack(self):
# Dynamically build fields based on message length
self.f = Fields()
self.f.add(U4('iTow'))
self.f.add(U1('version'))
self.f.add(X1_InitStatus1('initStatus1'))
self.f.add(X1_InitStatus2('initStatus2'))
self.f.add(Padding(5, 'res1'))
self.f.add(U1_FusionMode('fusionMode'))
self.f.add(Padding(2, 'res2'))
self.f.add(U1('numSens'))
# Extract upto this place to read number of sensors
super().unpack()
# Build final list
for sensor in range(self.f.numSens):
self.f.add(X1_SensStatus1(f'sensStatus1_{sensor}'))
self.f.add(X1_SensStatus2(f'sensStatus2_{sensor}'))
self.f.add(U1(f'freq_{sensor}'))
self.f.add(X1(f'faults_{sensor}'))
super().unpack()
def test_set_field2(self):
u = Fields()
u.add(U1('cmd'))
u.add(U1('val'))
u.cmd = 1
u.val = 3
assert u._fields['cmd'].value == 1
assert u._fields['val'].value == 3
def test_basic_unpack(self):
u = Fields()
u.add(CH(4, 'string'))
u.unpack(bytearray.fromhex('41 42 43 44'))
assert u.string == 'ABCD'
def test_chars_invalid_unicode(self):
u = Fields()
u.add(CH(2, 'string'))
with pytest.raises(ValueError):
u.unpack(bytearray.fromhex('d8 00')) # UTF16 reserved code point
def test_set_field1(self):
u = Fields()
u.add(U1('cmd'))
u.cmd = 1
assert u._fields['cmd'].value == 1
def test_chars_trailing_zeroes_removed(self):
u = Fields()
u.add(CH(10, 'string'))
u.unpack(bytearray.fromhex('41 42 43 44 45 00 00 00 00 00'))
assert u.string == 'ABCDE'
class UbxCfgGnss(UbxCfgGnss_):
GNSS_GPS = 0 # US GPS, worldwide
GNSS_SBAS = 1 # Satellite Based Augmentation System
GNSS_Galileo = 2 # European
GNSS_BeiDou = 3 # Chinese
GNSS_IMES = 4 # Japanese Indoor System
GNSS_GZSS = 5 # Japanse, Focus Japan, also Australia
GNSS_GLONASS = 6 # Russian
GNSS_IRNSS = 7 # Indian, not (yet) supported on NEO-M8
def __init__(self):
super().__init__()
# fields defined in unpack as they are dynamic
def unpack(self):
# Dynamically build fields based on message length
self.f = Fields()
self.f.add(U1('msgVer'))
self.f.add(U1('numTrkChHw'))
self.f.add(U1('numTrkChUse'))
self.f.add(U1('numConfigBlocks'))
# Extract upto this place to read number of config blocks
super().unpack()
# TODO: Check extra length against numConfigBlocks
# TODO: raise on mismatch
"""
extra_length = len(self.data) - 40
extra_info = int(extra_length / 30)
"""
# TODO: check nested fields -> unit test
for i in range(self.f.numConfigBlocks):
self.f.add(U1(f'gnssId_{i}'))
self.f.add(U1(f'resTrkCh_{i}'))
self.f.add(U1(f'maxTrkCh_{i}'))
self.f.add(Padding(1, f'res1_{i}'))
self.f.add(X4(f'flags_{i}')) # Bit 0: enable
super().unpack()
def gps_glonass(self):
self.enable_gnss(UbxCfgGnss.GNSS_GPS)
self.enable_gnss(UbxCfgGnss.GNSS_SBAS)
self.enable_gnss(UbxCfgGnss.GNSS_GLONASS)
self.disable_gnss(UbxCfgGnss.GNSS_Galileo)
self.disable_gnss(UbxCfgGnss.GNSS_BeiDou)
self.disable_gnss(UbxCfgGnss.GNSS_IMES)
self.disable_gnss(UbxCfgGnss.GNSS_GZSS)
# self.disable_gnss(UbxCfgGnss.GNSS_IRNSS)
def gps_galileo_beidou(self):
self.enable_gnss(UbxCfgGnss.GNSS_GPS)
self.enable_gnss(UbxCfgGnss.GNSS_SBAS)
self.enable_gnss(UbxCfgGnss.GNSS_Galileo)
self.enable_gnss(UbxCfgGnss.GNSS_BeiDou)
self.disable_gnss(UbxCfgGnss.GNSS_IMES)
self.disable_gnss(UbxCfgGnss.GNSS_GZSS)
self.disable_gnss(UbxCfgGnss.GNSS_GLONASS)
# self.disable_gnss(UbxCfgGnss.GNSS_IRNSS)
def enable_gnss(self, system):
assert 0 <= system <= 7
# TODO: Do not assume system == field entry, rather
# lookup gnssId
field = f'flags_{system}'
flag = self.f._fields[field].value
# print(f'{flag:08x}')
flag |= 1
self.f._fields[field].value = flag
def disable_gnss(self, system):
assert 0 <= system <= 7
# TODO: Do not assume system == field entry, rather
# lookup gnssId
field = f'flags_{system}'
flag = self.f._fields[field].value
# print(f'{flag:08x}')
flag &= ~1
self.f._fields[field].value = flag
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
def __init__(self):
super().__init__()
self.data = bytearray()
# TODO: Do we need checksum as member?
self.checksum = Checksum()
self.f = Fields()
def test_pack1(self):
u = Fields()
u.add(I4('test'))
u.add(U1('val'))
u.add(Padding(3, 'res1'))
u.add(U4('test2'))
u.test = 0x76543210
u.val = 0xAF
u.test2 = 0xcafebabe
data = u.pack()
print(data)
assert data == bytearray.fromhex('10 32 54 76 AF 00 00 00 be ba fe ca')
def test_pack2(self):
u = Fields()
u.add(I4('test'))
u.add(U1('val'))
u.add(Padding(2, 'res1'))
u.test = 0x76543210
u.val = 0xAF
data = u.pack()
assert data == bytearray.fromhex('10 32 54 76 AF 00 00')
class UbxEsfAlg(UbxEsfAlg_):
def __init__(self):
super().__init__()
self.f = Fields()
self.f.add(U4('iTow'))
self.f.add(U1('version'))
self.f.add(X1('bitfield0'))
self.f.add(Padding(2, 'res1'))
self.f.add(I4('yaw')) # 1e-1 -> more likely 1e-2
self.f.add(I2('pitch')) # 1e-1
self.f.add(I2('roll')) # 1e-1
# TODO: Check if properly documented.
# 30c74a22 01 00 0000 64000000 c800 2c01
# Does not report proper yaw, pitch, roll values when manually configured
# yaw displays roll
# pitch displays pitch
# roll displays 0 (yaw?)
def unpack(self):
import binascii
print(binascii.hexlify(self.data))
return super().unpack()
def test_creation(self):
u = Fields()
u.add(U1('cmd'))
class UbxCfgEsfla(UbxCfgEsfla_):
"""
Reponse frame to poll request
Contains all lever arm configurations
"""
def __init__(self):
super().__init__()
# fields defined in unpack()
def unpack(self):
# Dynamically build fields based on message length
self.f = Fields()
self.f.add(U1('version'))
self.f.add(U1('numConfigs'))
self.f.add(Padding(2, 'res1'))
# Extract upto this place to read number of level arm configurations
super().unpack()
assert self.f.numConfigs <= 5
# Build final list
for lever in range(self.f.numConfigs):
self.f.add(U1_LeverArmType(f'leverArmType_{lever}'))
self.f.add(Padding(1, f'res2_{lever}'))
self.f.add(I2(f'leverArmX_{lever}'))
self.f.add(I2(f'leverArmY_{lever}'))
self.f.add(I2(f'leverArmZ_{lever}'))
super().unpack()
def lever_arm(self, armType):
data = None
for lever in range(self.f.numConfigs):
x = self.f.get(f'leverArmType_{lever}')
if x.value == armType:
data = {
'x': self.f.get(f'leverArmX_{lever}').value,
'y': self.f.get(f'leverArmY_{lever}').value,
'z': self.f.get(f'leverArmZ_{lever}').value
}
break
return data
