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
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 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_QZSS = 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_GnssId(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_Flags(f'flags_{i}'))
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_QZSS)
# 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_QZSS)
self.disable_gnss(UbxCfgGnss.GNSS_GLONASS)
# self.disable_gnss(UbxCfgGnss.GNSS_IRNSS)
def enable_gnss(self, system):
assert 0 <= system <= UbxCfgGnss.GNSS_IRNSS
pos = self._find_entry(system)
if pos:
field = f'flags_{system}'
self.f._fields[field].enable()
def disable_gnss(self, system):
assert 0 <= system <= UbxCfgGnss.GNSS_IRNSS
pos = self._find_entry(system)
if pos:
field = f'flags_{system}'
self.f._fields[field].disable()
def _find_entry(self, system):
assert 0 <= system <= UbxCfgGnss.GNSS_IRNSS
for i in range(self.f.numConfigBlocks):
field_name = f'gnssId_{i}'
field = self.f.get(field_name)
if field.value == system:
return i