sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
openpilot is an open source driver assistance system. openpilot performs the functions of Automated Lane Centering and Adaptive Cruise Control for over 200 supported car makes and models.
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
139 Commits
120 Branches
82 Tags
5.0 GiB
 
 
 
 
 
 
Tag: Branch: Tree: 9a411ebf32
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '9a411ebf32'
${ noResults }
openpilot_comma/selfdrive/locationd/ublox.py

995 lines
31 KiB
Raw Blame History

#!/usr/bin/env python
'''
UBlox binary protocol handling
Copyright Andrew Tridgell, October 2012
Released under GNU GPL version 3 or later
WARNING: This code has originally intended for
ublox version 7, it has been adapted to work
for ublox version 8, not all functions may work.
'''
import struct
import time, os
# protocol constants
PREAMBLE1 = 0xb5
PREAMBLE2 = 0x62
# message classes
CLASS_NAV = 0x01
CLASS_RXM = 0x02
CLASS_INF = 0x04
CLASS_ACK = 0x05
CLASS_CFG = 0x06
CLASS_MON = 0x0A
CLASS_AID = 0x0B
CLASS_TIM = 0x0D
CLASS_ESF = 0x10
# ACK messages
MSG_ACK_NACK = 0x00
MSG_ACK_ACK = 0x01
# NAV messages
MSG_NAV_POSECEF = 0x1
MSG_NAV_POSLLH = 0x2
MSG_NAV_STATUS = 0x3
MSG_NAV_DOP = 0x4
MSG_NAV_SOL = 0x6
MSG_NAV_PVT = 0x7
MSG_NAV_POSUTM = 0x8
MSG_NAV_VELNED = 0x12
MSG_NAV_VELECEF = 0x11
MSG_NAV_TIMEGPS = 0x20
MSG_NAV_TIMEUTC = 0x21
MSG_NAV_CLOCK = 0x22
MSG_NAV_SVINFO = 0x30
MSG_NAV_AOPSTATUS = 0x60
MSG_NAV_DGPS = 0x31
MSG_NAV_DOP = 0x04
MSG_NAV_EKFSTATUS = 0x40
MSG_NAV_SBAS = 0x32
MSG_NAV_SOL = 0x06
# RXM messages
MSG_RXM_RAW = 0x15
MSG_RXM_SFRB = 0x11
MSG_RXM_SFRBX = 0x13
MSG_RXM_SVSI = 0x20
MSG_RXM_EPH = 0x31
MSG_RXM_ALM = 0x30
MSG_RXM_PMREQ = 0x41
# AID messages
MSG_AID_ALM = 0x30
MSG_AID_EPH = 0x31
MSG_AID_ALPSRV = 0x32
MSG_AID_AOP = 0x33
MSG_AID_DATA = 0x10
MSG_AID_ALP = 0x50
MSG_AID_DATA = 0x10
MSG_AID_HUI = 0x02
MSG_AID_INI = 0x01
MSG_AID_REQ = 0x00
# CFG messages
MSG_CFG_PRT = 0x00
MSG_CFG_ANT = 0x13
MSG_CFG_DAT = 0x06
MSG_CFG_EKF = 0x12
MSG_CFG_ESFGWT = 0x29
MSG_CFG_CFG = 0x09
MSG_CFG_USB = 0x1b
MSG_CFG_RATE = 0x08
MSG_CFG_SET_RATE = 0x01
MSG_CFG_NAV5 = 0x24
MSG_CFG_FXN = 0x0E
MSG_CFG_INF = 0x02
MSG_CFG_ITFM = 0x39
MSG_CFG_MSG = 0x01
MSG_CFG_NAVX5 = 0x23
MSG_CFG_NMEA = 0x17
MSG_CFG_NVS = 0x22
MSG_CFG_PM2 = 0x3B
MSG_CFG_PM = 0x32
MSG_CFG_RINV = 0x34
MSG_CFG_RST = 0x04
MSG_CFG_RXM = 0x11
MSG_CFG_SBAS = 0x16
MSG_CFG_TMODE2 = 0x3D
MSG_CFG_TMODE = 0x1D
MSG_CFG_TPS = 0x31
MSG_CFG_TP = 0x07
MSG_CFG_GNSS = 0x3E
MSG_CFG_ODO = 0x1E
# ESF messages
MSG_ESF_MEAS = 0x02
MSG_ESF_STATUS = 0x10
# INF messages
MSG_INF_DEBUG = 0x04
MSG_INF_ERROR = 0x00
MSG_INF_NOTICE = 0x02
MSG_INF_TEST = 0x03
MSG_INF_WARNING = 0x01
# MON messages
MSG_MON_SCHD = 0x01
MSG_MON_HW = 0x09
MSG_MON_HW2 = 0x0B
MSG_MON_IO = 0x02
MSG_MON_MSGPP = 0x06
MSG_MON_RXBUF = 0x07
MSG_MON_RXR = 0x21
MSG_MON_TXBUF = 0x08
MSG_MON_VER = 0x04
# TIM messages
MSG_TIM_TP = 0x01
MSG_TIM_TM2 = 0x03
MSG_TIM_SVIN = 0x04
MSG_TIM_VRFY = 0x06
# port IDs
PORT_DDC = 0
PORT_SERIAL1 = 1
PORT_SERIAL2 = 2
PORT_USB = 3
PORT_SPI = 4
# dynamic models
DYNAMIC_MODEL_PORTABLE = 0
DYNAMIC_MODEL_STATIONARY = 2
DYNAMIC_MODEL_PEDESTRIAN = 3
DYNAMIC_MODEL_AUTOMOTIVE = 4
DYNAMIC_MODEL_SEA = 5
DYNAMIC_MODEL_AIRBORNE1G = 6
DYNAMIC_MODEL_AIRBORNE2G = 7
DYNAMIC_MODEL_AIRBORNE4G = 8
#reset items
RESET_HOT = 0
RESET_WARM = 1
RESET_COLD = 0xFFFF
RESET_HW = 0
RESET_SW = 1
RESET_SW_GPS = 2
RESET_HW_GRACEFUL = 4
RESET_GPS_STOP = 8
RESET_GPS_START = 9
class UBloxError(Exception):
'''Ublox error class'''
def __init__(self, msg):
Exception.__init__(self, msg)
self.message = msg
class UBloxAttrDict(dict):
'''allow dictionary members as attributes'''
def __init__(self):
dict.__init__(self)
def __getattr__(self, name):
try:
return self.__getitem__(name)
except KeyError:
raise AttributeError(name)
def __setattr__(self, name, value):
if self.__dict__.has_key(name):
# allow set on normal attributes
dict.__setattr__(self, name, value)
else:
self.__setitem__(name, value)
def ArrayParse(field):
'''parse an array descriptor'''
arridx = field.find('[')
if arridx == -1:
return (field, -1)
alen = int(field[arridx + 1:-1])
fieldname = field[:arridx]
return (fieldname, alen)
class UBloxDescriptor:
'''class used to describe the layout of a UBlox message'''
def __init__(self,
name,
msg_format,
fields=[],
count_field=None,
format2=None,
fields2=None):
self.name = name
self.msg_format = msg_format
self.fields = fields
self.count_field = count_field
self.format2 = format2
self.fields2 = fields2
def unpack(self, msg):
'''unpack a UBloxMessage, creating the .fields and ._recs attributes in msg'''
msg._fields = {}
# unpack main message blocks. A comm
formats = self.msg_format.split(',')
buf = msg._buf[6:-2]
count = 0
msg._recs = []
fields = self.fields[:]
for fmt in formats:
size1 = struct.calcsize(fmt)
if size1 > len(buf):
raise UBloxError("%s INVALID_SIZE1=%u" % (self.name, len(buf)))
f1 = list(struct.unpack(fmt, buf[:size1]))
i = 0
while i < len(f1):
field = fields.pop(0)
(fieldname, alen) = ArrayParse(field)
if alen == -1:
msg._fields[fieldname] = f1[i]
if self.count_field == fieldname:
count = int(f1[i])
i += 1
else:
msg._fields[fieldname] = [0] * alen
for a in range(alen):
msg._fields[fieldname][a] = f1[i]
i += 1
buf = buf[size1:]
if len(buf) == 0:
break
if self.count_field == '_remaining':
count = len(buf) / struct.calcsize(self.format2)
if count == 0:
msg._unpacked = True
if len(buf) != 0:
raise UBloxError("EXTRA_BYTES=%u" % len(buf))
return
size2 = struct.calcsize(self.format2)
for c in range(count):
r = UBloxAttrDict()
if size2 > len(buf):
raise UBloxError("INVALID_SIZE=%u, " % len(buf))
f2 = list(struct.unpack(self.format2, buf[:size2]))
for i in range(len(self.fields2)):
r[self.fields2[i]] = f2[i]
buf = buf[size2:]
msg._recs.append(r)
if len(buf) != 0:
raise UBloxError("EXTRA_BYTES=%u" % len(buf))
msg._unpacked = True
def pack(self, msg, msg_class=None, msg_id=None):
'''pack a UBloxMessage from the .fields and ._recs attributes in msg'''
f1 = []
if msg_class is None:
msg_class = msg.msg_class()
if msg_id is None:
msg_id = msg.msg_id()
msg._buf = ''
fields = self.fields[:]
for f in fields:
(fieldname, alen) = ArrayParse(f)
if not fieldname in msg._fields:
break
if alen == -1:
f1.append(msg._fields[fieldname])
else:
for a in range(alen):
f1.append(msg._fields[fieldname][a])
try:
# try full length message
fmt = self.msg_format.replace(',', '')
msg._buf = struct.pack(fmt, *tuple(f1))
except Exception:
# try without optional part
fmt = self.msg_format.split(',')[0]
msg._buf = struct.pack(fmt, *tuple(f1))
length = len(msg._buf)
if msg._recs:
length += len(msg._recs) * struct.calcsize(self.format2)
header = struct.pack('<BBBBH', PREAMBLE1, PREAMBLE2, msg_class, msg_id, length)
msg._buf = header + msg._buf
for r in msg._recs:
f2 = []
for f in self.fields2:
f2.append(r[f])
msg._buf += struct.pack(self.format2, *tuple(f2))
msg._buf += struct.pack('<BB', *msg.checksum(data=msg._buf[2:]))
def format(self, msg):
'''return a formatted string for a message'''
if not msg._unpacked:
self.unpack(msg)
ret = self.name + ': '
for f in self.fields:
(fieldname, alen) = ArrayParse(f)
if not fieldname in msg._fields:
continue
v = msg._fields[fieldname]
if isinstance(v, list):
ret += '%s=[' % fieldname
for a in range(alen):
ret += '%s, ' % v[a]
ret = ret[:-2] + '], '
elif isinstance(v, str):
ret += '%s="%s", ' % (f, v.rstrip(' \0'))
else:
ret += '%s=%s, ' % (f, v)
for r in msg._recs:
ret += '[ '
for f in self.fields2:
v = r[f]
ret += '%s=%s, ' % (f, v)
ret = ret[:-2] + ' ], '
return ret[:-2]
# list of supported message types.
msg_types = {
(CLASS_ACK, MSG_ACK_ACK):
UBloxDescriptor('ACK_ACK', '<BB', ['clsID', 'msgID']),
(CLASS_ACK, MSG_ACK_NACK):
UBloxDescriptor('ACK_NACK', '<BB', ['clsID', 'msgID']),
(CLASS_CFG, MSG_CFG_USB):
UBloxDescriptor('CFG_USB', '<HHHHHH32s32s32s', [
'vendorID', 'productID', 'reserved1', 'reserved2', 'powerConsumption', 'flags',
'vendorString', 'productString', 'serialNumber'
]),
(CLASS_CFG, MSG_CFG_PRT):
UBloxDescriptor('CFG_PRT', '<BBHIIHHHH', [
'portID', 'reserved0', 'txReady', 'mode', 'baudRate', 'inProtoMask', 'outProtoMask',
'reserved4', 'reserved5'
]),
(CLASS_CFG, MSG_CFG_CFG):
UBloxDescriptor('CFG_CFG', '<III,B',
['clearMask', 'saveMask', 'loadMask', 'deviceMask']),
(CLASS_CFG, MSG_CFG_RXM):
UBloxDescriptor('CFG_RXM', '<BB',
['reserved1', 'lpMode']),
(CLASS_CFG, MSG_CFG_RST):
UBloxDescriptor('CFG_RST', '<HBB', ['navBbrMask ', 'resetMode', 'reserved1']),
(CLASS_CFG, MSG_CFG_SBAS):
UBloxDescriptor('CFG_SBAS', '<BBBBI',
['mode', 'usage', 'maxSBAS', 'scanmode2', 'scanmode1']),
(CLASS_CFG, MSG_CFG_GNSS):
UBloxDescriptor('CFG_GNSS', '<BBBB',
['msgVer', 'numTrkChHw', 'numTrkChUse',
'numConfigBlocks'], 'numConfigBlocks', '<BBBBI',
['gnssId', 'resTrkCh', 'maxTrkCh', 'reserved1', 'flags']),
(CLASS_CFG, MSG_CFG_RATE):
UBloxDescriptor('CFG_RATE', '<HHH', ['measRate', 'navRate', 'timeRef']),
(CLASS_CFG, MSG_CFG_MSG):
UBloxDescriptor('CFG_MSG', '<BB6B', ['msgClass', 'msgId', 'rates[6]']),
(CLASS_NAV, MSG_NAV_POSLLH):
UBloxDescriptor('NAV_POSLLH', '<IiiiiII',
['iTOW', 'Longitude', 'Latitude', 'height', 'hMSL', 'hAcc', 'vAcc']),
(CLASS_NAV, MSG_NAV_VELNED):
UBloxDescriptor('NAV_VELNED', '<IiiiIIiII', [
'iTOW', 'velN', 'velE', 'velD', 'speed', 'gSpeed', 'heading', 'sAcc', 'cAcc'
]),
(CLASS_NAV, MSG_NAV_DOP):
UBloxDescriptor('NAV_DOP', '<IHHHHHHH',
['iTOW', 'gDOP', 'pDOP', 'tDOP', 'vDOP', 'hDOP', 'nDOP', 'eDOP']),
(CLASS_NAV, MSG_NAV_STATUS):
UBloxDescriptor('NAV_STATUS', '<IBBBBII',
['iTOW', 'gpsFix', 'flags', 'fixStat', 'flags2', 'ttff', 'msss']),
(CLASS_NAV, MSG_NAV_SOL):
UBloxDescriptor('NAV_SOL', '<IihBBiiiIiiiIHBBI', [
'iTOW', 'fTOW', 'week', 'gpsFix', 'flags', 'ecefX', 'ecefY', 'ecefZ', 'pAcc',
'ecefVX', 'ecefVY', 'ecefVZ', 'sAcc', 'pDOP', 'reserved1', 'numSV', 'reserved2'
]),
(CLASS_NAV, MSG_NAV_PVT):
UBloxDescriptor('NAV_PVT', '<IHBBBBBBIiBBBBiiiiIIiiiiiIIH6BihH', [
'iTOW', 'year', 'month', 'day', 'hour', 'min', 'sec', 'valid', 'tAcc', 'nano',
'fixType', 'flags', 'flags2', 'numSV', 'lon', 'lat', 'height', 'hMSL', 'hAcc', 'vAcc',
'velN', 'velE', 'velD', 'gSpeed', 'headMot', 'sAcc', 'headAcc', 'pDOP',
'reserverd1[6]', 'headVeh', 'magDec', 'magAcc'
]),
(CLASS_NAV, MSG_NAV_POSUTM):
UBloxDescriptor('NAV_POSUTM', '<Iiiibb',
['iTOW', 'East', 'North', 'Alt', 'Zone', 'Hem']),
(CLASS_NAV, MSG_NAV_SBAS):
UBloxDescriptor('NAV_SBAS', '<IBBbBBBBB', [
'iTOW', 'geo', 'mode', 'sys', 'service', 'cnt', 'reserved01', 'reserved02',
'reserved03'
], 'cnt', 'BBBBBBhHh', [
'svid', 'flags', 'udre', 'svSys', 'svService', 'reserved1', 'prc', 'reserved2', 'ic'
]),
(CLASS_NAV, MSG_NAV_POSECEF):
UBloxDescriptor('NAV_POSECEF', '<IiiiI', ['iTOW', 'ecefX', 'ecefY', 'ecefZ', 'pAcc']),
(CLASS_NAV, MSG_NAV_VELECEF):
UBloxDescriptor('NAV_VELECEF', '<IiiiI', ['iTOW', 'ecefVX', 'ecefVY', 'ecefVZ',
'sAcc']),
(CLASS_NAV, MSG_NAV_TIMEGPS):
UBloxDescriptor('NAV_TIMEGPS', '<IihbBI',
['iTOW', 'fTOW', 'week', 'leapS', 'valid', 'tAcc']),
(CLASS_NAV, MSG_NAV_TIMEUTC):
UBloxDescriptor('NAV_TIMEUTC', '<IIiHBBBBBB', [
'iTOW', 'tAcc', 'nano', 'year', 'month', 'day', 'hour', 'min', 'sec', 'valid'
]),
(CLASS_NAV, MSG_NAV_CLOCK):
UBloxDescriptor('NAV_CLOCK', '<IiiII', ['iTOW', 'clkB', 'clkD', 'tAcc', 'fAcc']),
(CLASS_NAV, MSG_NAV_DGPS):
UBloxDescriptor('NAV_DGPS', '<IihhBBH',
['iTOW', 'age', 'baseId', 'baseHealth', 'numCh', 'status', 'reserved1'],
'numCh', '<BBHff', ['svid', 'flags', 'ageC', 'prc', 'prrc']),
(CLASS_NAV, MSG_NAV_SVINFO):
UBloxDescriptor('NAV_SVINFO', '<IBBH', ['iTOW', 'numCh', 'globalFlags',
'reserved2'], 'numCh', '<BBBBBbhi',
['chn', 'svid', 'flags', 'quality', 'cno', 'elev', 'azim', 'prRes']),
(CLASS_RXM, MSG_RXM_SVSI):
UBloxDescriptor('RXM_SVSI', '<IhBB', ['iTOW', 'week', 'numVis', 'numSV'], 'numSV',
'<BBhbB', ['svid', 'svFlag', 'azim', 'elev', 'age']),
(CLASS_RXM, MSG_RXM_EPH):
UBloxDescriptor('RXM_EPH', '<II , 8I 8I 8I',
['svid', 'how', 'sf1d[8]', 'sf2d[8]', 'sf3d[8]']),
(CLASS_AID, MSG_AID_EPH):
UBloxDescriptor('AID_EPH', '<II , 8I 8I 8I',
['svid', 'how', 'sf1d[8]', 'sf2d[8]', 'sf3d[8]']),
(CLASS_AID, MSG_AID_HUI):
UBloxDescriptor('AID_HUI', '<Iddi 6h 8f I',
['health', 'utcA0', 'utcA1', 'utcTOW', 'utcWNT', 'utcLS', 'utcWNF',
'utcDN', 'utcLSF', 'utcSpare', 'klobA0', 'klobA1', 'klobA2', 'klobA3',
'klobB0', 'klobB1', 'klobB2', 'klobB3', 'flags']),
(CLASS_AID, MSG_AID_AOP):
UBloxDescriptor('AID_AOP', '<B47B , 48B 48B 48B',
['svid', 'data[47]', 'optional0[48]', 'optional1[48]',
'optional1[48]']),
(CLASS_RXM, MSG_RXM_RAW):
UBloxDescriptor('RXM_RAW', '<dHbBB3B', [
'rcvTow', 'week', 'leapS', 'numMeas', 'recStat', 'reserved1[3]'
], 'numMeas', '<ddfBBBBHBBBBBB', [
'prMes', 'cpMes', 'doMes', 'gnssId', 'svId', 'reserved2', 'freqId', 'locktime', 'cno',
'prStdev', 'cpStdev', 'doStdev', 'trkStat', 'reserved3'
]),
(CLASS_RXM, MSG_RXM_SFRB):
UBloxDescriptor('RXM_SFRB', '<BB10I', ['chn', 'svid', 'dwrd[10]']),
(CLASS_RXM, MSG_RXM_SFRBX):
UBloxDescriptor('RXM_SFRBX', '<8B', ['gnssId', 'svid', 'reserved1', 'freqId', 'numWords',
'reserved2', 'version', 'reserved3'], 'numWords', 'I', ['dwrd']),
(CLASS_AID, MSG_AID_ALM):
UBloxDescriptor('AID_ALM', '<II', '_remaining', 'I', ['dwrd']),
(CLASS_RXM, MSG_RXM_ALM):
UBloxDescriptor('RXM_ALM', '<II , 8I', ['svid', 'week', 'dwrd[8]']),
(CLASS_CFG, MSG_CFG_ODO):
UBloxDescriptor('CFG_ODO', '<B3BBB6BBB2BBB2B', [
'version', 'reserved1[3]', 'flags', 'odoCfg', 'reserverd2[6]', 'cogMaxSpeed',
'cogMaxPosAcc', 'reserved3[2]', 'velLpGain', 'cogLpGain', 'reserved[2]'
]),
(CLASS_CFG, MSG_CFG_NAV5):
UBloxDescriptor('CFG_NAV5', '<HBBiIbBHHHHBBIII', [
'mask', 'dynModel', 'fixMode', 'fixedAlt', 'fixedAltVar', 'minElev', 'drLimit',
'pDop', 'tDop', 'pAcc', 'tAcc', 'staticHoldThresh', 'dgpsTimeOut', 'reserved2',
'reserved3', 'reserved4'
]),
(CLASS_CFG, MSG_CFG_NAVX5):
UBloxDescriptor('CFG_NAVX5', '<HHIBBBBBBBBBBHIBBBBBBHII', [
'version', 'mask1', 'reserved0', 'reserved1', 'reserved2', 'minSVs', 'maxSVs',
'minCNO', 'reserved5', 'iniFix3D', 'reserved6', 'reserved7', 'reserved8',
'wknRollover', 'reserved9', 'reserved10', 'reserved11', 'usePPP', 'useAOP',
'reserved12', 'reserved13', 'aopOrbMaxErr', 'reserved3', 'reserved4'
]),
(CLASS_MON, MSG_MON_HW):
UBloxDescriptor('MON_HW', '<IIIIHHBBBBIB25BHIII', [
'pinSel', 'pinBank', 'pinDir', 'pinVal', 'noisePerMS', 'agcCnt', 'aStatus', 'aPower',
'flags', 'reserved1', 'usedMask', 'VP[25]', 'jamInd', 'reserved3', 'pinInq', 'pullH',
'pullL'
]),
(CLASS_MON, MSG_MON_HW2):
UBloxDescriptor('MON_HW2', '<bBbBB3BI8BI4B', [
'ofsI', 'magI', 'ofsQ', 'magQ', 'cfgSource', 'reserved1[3]', 'lowLevCfg',
'reserved2[8]', 'postStatus', 'reserved3[4]'
]),
(CLASS_MON, MSG_MON_SCHD):
UBloxDescriptor('MON_SCHD', '<IIIIHHHBB', [
'tskRun', 'tskSchd', 'tskOvrr', 'tskReg', 'stack', 'stackSize', 'CPUIdle', 'flySly',
'ptlSly'
]),
(CLASS_MON, MSG_MON_VER):
UBloxDescriptor('MON_VER', '<30s10s,30s', ['swVersion', 'hwVersion', 'romVersion'],
'_remaining', '30s', ['extension']),
(CLASS_TIM, MSG_TIM_TP):
UBloxDescriptor('TIM_TP', '<IIiHBB',
['towMS', 'towSubMS', 'qErr', 'week', 'flags', 'reserved1']),
(CLASS_TIM, MSG_TIM_TM2):
UBloxDescriptor('TIM_TM2', '<BBHHHIIIII', [
'ch', 'flags', 'count', 'wnR', 'wnF', 'towMsR', 'towSubMsR', 'towMsF', 'towSubMsF',
'accEst'
]),
(CLASS_TIM, MSG_TIM_SVIN):
UBloxDescriptor('TIM_SVIN', '<IiiiIIBBH', [
'dur', 'meanX', 'meanY', 'meanZ', 'meanV', 'obs', 'valid', 'active', 'reserved1'
]),
(CLASS_INF, MSG_INF_ERROR):
UBloxDescriptor('INF_ERR', '<18s', ['str']),
(CLASS_INF, MSG_INF_DEBUG):
UBloxDescriptor('INF_DEBUG', '<18s', ['str'])
}
class UBloxMessage:
'''UBlox message class - holds a UBX binary message'''
def __init__(self):
self._buf = ""
self._fields = {}
self._recs = []
self._unpacked = False
self.debug_level = 1
def __str__(self):
'''format a message as a string'''
if not self.valid():
return 'UBloxMessage(INVALID)'
type = self.msg_type()
if type in msg_types:
return msg_types[type].format(self)
return 'UBloxMessage(UNKNOWN %s, %u)' % (str(type), self.msg_length())
def as_dict(self):
'''format a message as a string'''
if not self.valid():
return 'UBloxMessage(INVALID)'
type = self.msg_type()
if type in msg_types:
return msg_types[type].format(self)
return 'UBloxMessage(UNKNOWN %s, %u)' % (str(type), self.msg_length())
def __getattr__(self, name):
'''allow access to message fields'''
try:
return self._fields[name]
except KeyError:
if name == 'recs':
return self._recs
raise AttributeError(name)
def __setattr__(self, name, value):
'''allow access to message fields'''
if name.startswith('_'):
self.__dict__[name] = value
else:
self._fields[name] = value
def have_field(self, name):
'''return True if a message contains the given field'''
return name in self._fields
def debug(self, level, msg):
'''write a debug message'''
if self.debug_level >= level:
print(msg)
def unpack(self):
'''unpack a message'''
if not self.valid():
raise UBloxError('INVALID MESSAGE')
type = self.msg_type()
if not type in msg_types:
raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf)))
msg_types[type].unpack(self)
return self._fields, self._recs
def pack(self):
'''pack a message'''
if not self.valid():
raise UBloxError('INVALID MESSAGE')
type = self.msg_type()
if not type in msg_types:
raise UBloxError('Unknown message %s' % str(type))
msg_types[type].pack(self)
def name(self):
'''return the short string name for a message'''
if not self.valid():
raise UBloxError('INVALID MESSAGE')
type = self.msg_type()
if not type in msg_types:
raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf)))
return msg_types[type].name
def msg_class(self):
'''return the message class'''
return ord(self._buf[2])
def msg_id(self):
'''return the message id within the class'''
return ord(self._buf[3])
def msg_type(self):
'''return the message type tuple (class, id)'''
return (self.msg_class(), self.msg_id())
def msg_length(self):
'''return the payload length'''
(payload_length, ) = struct.unpack('<H', self._buf[4:6])
return payload_length
def valid_so_far(self):
'''check if the message is valid so far'''
if len(self._buf) > 0 and ord(self._buf[0]) != PREAMBLE1:
return False
if len(self._buf) > 1 and ord(self._buf[1]) != PREAMBLE2:
self.debug(1, "bad pre2")
return False
if self.needed_bytes() == 0 and not self.valid():
if len(self._buf) > 8:
self.debug(1, "bad checksum len=%u needed=%u" % (len(self._buf),
self.needed_bytes()))
else:
self.debug(1, "bad len len=%u needed=%u" % (len(self._buf), self.needed_bytes()))
return False
return True
def add(self, bytes):
'''add some bytes to a message'''
self._buf += bytes
while not self.valid_so_far() and len(self._buf) > 0:
'''handle corrupted streams'''
self._buf = self._buf[1:]
if self.needed_bytes() < 0:
self._buf = ""
def checksum(self, data=None):
'''return a checksum tuple for a message'''
if data is None:
data = self._buf[2:-2]
#cs = 0
ck_a = 0
ck_b = 0
for i in data:
ck_a = (ck_a + ord(i)) & 0xFF
ck_b = (ck_b + ck_a) & 0xFF
return (ck_a, ck_b)
def valid_checksum(self):
'''check if the checksum is OK'''
(ck_a, ck_b) = self.checksum()
#d = self._buf[2:-2]
(ck_a2, ck_b2) = struct.unpack('<BB', self._buf[-2:])
return ck_a == ck_a2 and ck_b == ck_b2
def needed_bytes(self):
'''return number of bytes still needed'''
if len(self._buf) < 6:
return 8 - len(self._buf)
return self.msg_length() + 8 - len(self._buf)
def valid(self):
'''check if a message is valid'''
return len(self._buf) >= 8 and self.needed_bytes() == 0 and self.valid_checksum()
class UBlox:
'''main UBlox control class.
port can be a file (for reading only) or a serial device
'''
def __init__(self, port, baudrate=115200, timeout=0, panda=False, grey=False):
self.serial_device = port
self.baudrate = baudrate
self.use_sendrecv = False
self.read_only = False
self.debug_level = 0
if panda:
from panda import Panda, PandaSerial
self.panda = Panda()
# resetting U-Blox module
self.panda.set_esp_power(0)
time.sleep(0.1)
self.panda.set_esp_power(1)
time.sleep(0.5)
# can't set above 9600 now...
self.baudrate = 9600
self.dev = PandaSerial(self.panda, 1, self.baudrate)
self.baudrate = 460800
print "upping baud:",self.baudrate
self.send_nmea("$PUBX,41,1,0007,0003,%u,0" % self.baudrate)
time.sleep(0.1)
self.dev = PandaSerial(self.panda, 1, self.baudrate)
elif grey:
import zmq
from selfdrive.services import service_list
import selfdrive.messaging as messaging
class BoarddSerial(object):
def __init__(self):
context = zmq.Context()
self.ubloxRaw = messaging.sub_sock(context, service_list['ubloxRaw'].port)
self.buf = ""
def read(self, n):
for msg in messaging.drain_sock(self.ubloxRaw, len(self.buf) < n):
self.buf += msg.ubloxRaw
ret = self.buf[:n]
self.buf = self.buf[n:]
return ret
def write(self, dat):
pass
self.dev = BoarddSerial()
else:
if self.serial_device.startswith("tcp:"):
import socket
a = self.serial_device.split(':')
destination_addr = (a[1], int(a[2]))
self.dev = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.dev.connect(destination_addr)
self.dev.setblocking(1)
self.dev.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1)
self.use_sendrecv = True
elif os.path.isfile(self.serial_device):
self.read_only = True
self.dev = open(self.serial_device, mode='rb')
else:
import serial
self.dev = serial.Serial(
self.serial_device,
baudrate=self.baudrate,
dsrdtr=False,
rtscts=False,
xonxoff=False,
timeout=timeout)
self.logfile = None
self.log = None
self.preferred_dynamic_model = None
self.preferred_usePPP = None
self.preferred_dgps_timeout = None
def close(self):
'''close the device'''
self.dev.close()
self.dev = None
def set_debug(self, debug_level):
'''set debug level'''
self.debug_level = debug_level
def debug(self, level, msg):
'''write a debug message'''
if self.debug_level >= level:
print(msg)
def set_logfile(self, logfile, append=False):
'''setup logging to a file'''
if self.log is not None:
self.log.close()
self.log = None
self.logfile = logfile
if self.logfile is not None:
if append:
mode = 'ab'
else:
mode = 'wb'
self.log = open(self.logfile, mode=mode)
def set_preferred_dynamic_model(self, model):
'''set the preferred dynamic model for receiver'''
self.preferred_dynamic_model = model
if model is not None:
self.configure_poll(CLASS_CFG, MSG_CFG_NAV5)
def set_preferred_dgps_timeout(self, timeout):
'''set the preferred DGPS timeout for receiver'''
self.preferred_dgps_timeout = timeout
if timeout is not None:
self.configure_poll(CLASS_CFG, MSG_CFG_NAV5)
def set_preferred_usePPP(self, usePPP):
'''set the preferred usePPP setting for the receiver'''
if usePPP is None:
self.preferred_usePPP = None
return
self.preferred_usePPP = int(usePPP)
self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5)
def nmea_checksum(self, msg):
d = msg[1:]
cs = 0
for i in d:
cs ^= ord(i)
return cs
def write(self, buf):
'''write some bytes'''
if not self.read_only:
if self.use_sendrecv:
return self.dev.send(buf)
return self.dev.write(buf)
def read(self, n):
'''read some bytes'''
if self.use_sendrecv:
import socket
try:
return self.dev.recv(n)
except socket.error:
return ''
return self.dev.read(n)
def send_nmea(self, msg):
if not self.read_only:
s = msg + "*%02X" % self.nmea_checksum(msg) + "\r\n"
self.write(s)
def set_binary(self):
'''put a UBlox into binary mode using a NMEA string'''
if not self.read_only:
print("try set binary at %u" % self.baudrate)
self.send_nmea("$PUBX,41,0,0007,0001,%u,0" % self.baudrate)
self.send_nmea("$PUBX,41,1,0007,0001,%u,0" % self.baudrate)
self.send_nmea("$PUBX,41,2,0007,0001,%u,0" % self.baudrate)
self.send_nmea("$PUBX,41,3,0007,0001,%u,0" % self.baudrate)
self.send_nmea("$PUBX,41,4,0007,0001,%u,0" % self.baudrate)
self.send_nmea("$PUBX,41,5,0007,0001,%u,0" % self.baudrate)
def disable_nmea(self):
''' stop sending all types of nmea messages '''
self.send_nmea("$PUBX,40,GSV,1,1,1,1,1,0")
self.send_nmea("$PUBX,40,GGA,0,0,0,0,0,0")
self.send_nmea("$PUBX,40,GSA,0,0,0,0,0,0")
self.send_nmea("$PUBX,40,VTG,0,0,0,0,0,0")
self.send_nmea("$PUBX,40,TXT,0,0,0,0,0,0")
self.send_nmea("$PUBX,40,RMC,0,0,0,0,0,0")
def seek_percent(self, pct):
'''seek to the given percentage of a file'''
self.dev.seek(0, 2)
filesize = self.dev.tell()
self.dev.seek(pct * 0.01 * filesize)
def special_handling(self, msg):
'''handle automatic configuration changes'''
if msg.name() == 'CFG_NAV5':
msg.unpack()
sendit = False
pollit = False
if self.preferred_dynamic_model is not None and msg.dynModel != self.preferred_dynamic_model:
msg.dynModel = self.preferred_dynamic_model
sendit = True
pollit = True
if self.preferred_dgps_timeout is not None and msg.dgpsTimeOut != self.preferred_dgps_timeout:
msg.dgpsTimeOut = self.preferred_dgps_timeout
self.debug(2, "Setting dgpsTimeOut=%u" % msg.dgpsTimeOut)
sendit = True
# we don't re-poll for this one, as some receivers refuse to set it
if sendit:
msg.pack()
self.send(msg)
if pollit:
self.configure_poll(CLASS_CFG, MSG_CFG_NAV5)
if msg.name() == 'CFG_NAVX5' and self.preferred_usePPP is not None:
msg.unpack()
if msg.usePPP != self.preferred_usePPP:
msg.usePPP = self.preferred_usePPP
msg.mask = 1 << 13
msg.pack()
self.send(msg)
self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5)
def receive_message(self, ignore_eof=False):
'''blocking receive of one ublox message'''
msg = UBloxMessage()
while True:
n = msg.needed_bytes()
b = self.read(n)
if not b:
if ignore_eof:
time.sleep(0.01)
continue
if len(msg._buf) > 0:
self.debug(1, "dropping %d bytes" % len(msg._buf))
return None
msg.add(b)
if self.log is not None:
self.log.write(b)
self.log.flush()
if msg.valid():
self.special_handling(msg)
return msg
def receive_message_noerror(self, ignore_eof=False):
'''blocking receive of one ublox message, ignoring errors'''
try:
return self.receive_message(ignore_eof=ignore_eof)
except UBloxError as e:
print(e)
return None
except OSError as e:
# Occasionally we get hit with 'resource temporarily unavailable'
# messages here on the serial device, catch them too.
print(e)
return None
def send(self, msg):
'''send a preformatted ublox message'''
if not msg.valid():
self.debug(1, "invalid send")
return
if not self.read_only:
self.write(msg._buf)
def send_message(self, msg_class, msg_id, payload):
'''send a ublox message with class, id and payload'''
msg = UBloxMessage()
msg._buf = struct.pack('<BBBBH', 0xb5, 0x62, msg_class, msg_id, len(payload))
msg._buf += payload
(ck_a, ck_b) = msg.checksum(msg._buf[2:])
msg._buf += struct.pack('<BB', ck_a, ck_b)
self.send(msg)
def configure_solution_rate(self, rate_ms=200, nav_rate=1, timeref=0):
'''configure the solution rate in milliseconds'''
payload = struct.pack('<HHH', rate_ms, nav_rate, timeref)
self.send_message(CLASS_CFG, MSG_CFG_RATE, payload)
def configure_message_rate(self, msg_class, msg_id, rate):
'''configure the message rate for a given message'''
payload = struct.pack('<BBB', msg_class, msg_id, rate)
self.send_message(CLASS_CFG, MSG_CFG_SET_RATE, payload)
def configure_port(self, port=1, inMask=3, outMask=3, mode=2240, baudrate=None):
'''configure a IO port'''
if baudrate is None:
baudrate = self.baudrate
payload = struct.pack('<BBH8BHHBBBB', port, 0xff, 0, 0, 0, 0, 0, 0, 0, 0, 0, inMask,
outMask, 0, 0, 0, 0)
self.send_message(CLASS_CFG, MSG_CFG_PRT, payload)
def configure_loadsave(self, clearMask=0, saveMask=0, loadMask=0, deviceMask=0):
'''configure configuration load/save'''
payload = struct.pack('<IIIB', clearMask, saveMask, loadMask, deviceMask)
self.send_message(CLASS_CFG, MSG_CFG_CFG, payload)
def configure_poll(self, msg_class, msg_id, payload=''):
'''poll a configuration message'''
self.send_message(msg_class, msg_id, payload)
def configure_poll_port(self, portID=None):
'''poll a port configuration'''
if portID is None:
self.configure_poll(CLASS_CFG, MSG_CFG_PRT)
else:
self.configure_poll(CLASS_CFG, MSG_CFG_PRT, struct.pack('<B', portID))
def configure_min_max_sats(self, min_sats=4, max_sats=32):
'''Set the minimum/maximum number of satellites for a solution in the NAVX5 message'''
payload = struct.pack('<HHIBBBBBBBBBBHIBBBBBBHII', 0, 4, 0, 0, 0, min_sats, max_sats,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
self.send_message(CLASS_CFG, MSG_CFG_NAVX5, payload)
def module_reset(self, set, mode):
''' Reset the module for hot/warm/cold start'''
payload = struct.pack('<HBB', set, mode, 0)
self.send_message(CLASS_CFG, MSG_CFG_RST, payload)