def poll(self,
messageType,
length=0,
data=[],
printMessage=False,
timeout=1,
delay=0):
logging.info('Polling for {}...'.format(messageType))
self.clearReceiveBuffer()
self.sendMessage(messageType, length, data)
if delay > 0:
time.sleep(delay)
msgType, data, remainder = self.receiveMessage(timeout=timeout)
if msgType != messageType:
raise Exception(
'Response was of a different type! Got {} instead of {}!'.
format(msgType, messageType))
if remainder is not None:
logging.debug('Parsing remainder...')
msgTypeR, dataR, remainder = UbloxMessage.parse(remainder)
if messageType.startswith('CFG-') and msgTypeR == 'ACK-ACK':
if self.checkAck(msgTypeR, dataR, messageType):
logging.info('Config message ACKed by ublox.')
if printMessage:
UbloxMessage.printMessage(msgType, data)
return data
def parse(self):
index = self.buffer.find(b'\xb5\x62')
if index >= 0:
self.start += index
result = UbloxMessage.validate(self.buffer[self.start:])
if result['valid']:
msgFormat, msgData, remainder = UbloxMessage.parse(
self.buffer[self.start:])
self.buffer = remainder if remainder is not None else b''
self.start = 0
self.handle_message(msgFormat, msgData)
else:
# Invalid message, move past sync bytes
if result['lengthMatch']:
self.buffer = self.buffer[self.start + 2:]
# Discard all but the last byte
else:
self.buffer = self.buffer[-1:]
self.start = 0
def decodeNmeaBuffer(self, buf):
# This assumes the data buffer is NMEA data and looks for messages
messages = UbloxMessage.decodeNmeaBuffer(buf)
for message in messages:
if len(message) == 0:
continue
start = message.find(',')
self.callback(message[:start], message)
def extractMessages(data):
messages = []
while data is not None:
msgFormat, msgData, remainder = UbloxMessage.parse(data, raw=True)
print('{} message of length {}'.format(msgFormat, len(msgData)))
messages.append((msgFormat, msgData))
data = remainder
return messages
def parse(self):
self.logger.debug('in UbloxReader.parse()')
if len(self.buffer) < 8:
self.logger.debug('UbloxReader.parse(): not enough data in buffer')
return
index = self.buffer.find(b'\xb5\x62')
if index >= 0:
self.start += index
msgTime = time.time()
self.logger.debug('UbloxReader.parse(): sending for validation')
result = UbloxMessage.validate(self.buffer[self.start:])
if result['valid']:
rawMessage = self.buffer[self.start:]
self.logger.debug('UbloxReader.parse(): sending to UbloxMessage.parse()')
msgFormat, msgData, remainder = UbloxMessage.parse(rawMessage)
rawMessage = rawMessage[:len(rawMessage) - len(remainder)] if remainder is not None else rawMessage[:len(rawMessage)]
self.buffer = remainder if remainder is not None else b''
self.start = 0
if msgFormat is not None:
self.logger.debug('UbloxReader.parse(): sending to UbloxReader.handleMessage()')
self.handleMessage(msgTime, msgFormat, msgData, rawMessage)
return
else:
# Invalid message, move past sync bytes
if result['lengthMatch'] or ((result['length'] is not None) and (result['length'] > 4096)):
if result['lengthMatch']:
self.logger.debug('UbloxReader.parse(): invalid message in buffer, moving past sync')
else:
self.logger.debug('UbloxReader.parse(): invalid length ({}) - enforcing max length of 4096 bytes'.format(result['length']))
self.buffer = self.buffer[self.start+2:]
return
else:
self.logger.debug('Ublox.parse(): Header indicates a message of length {}, buffer only has {} bytes'.format(result['length'], len(self.buffer)))
return
# Discard all but the last byte
else:
self.logger.debug('UbloxReader.parse(): could not find sync in buffer, discarding all but the last byte')
self.buffer = self.buffer[-1:]
self.start = 0
return
def poll(self, msgFormat, ser):
length = 0
data = []
message = UbloxMessage.buildMessage(msgFormat, length, data)
self.pollResult = None
self.pollTarget = msgFormat
ser.write(message)
while self.pollResult is None:
time.sleep(0.01)
return self.pollResult
def receiveMessage(self, timeout=1):
bytesAvailable = self.waitForMessage(timeout=timeout)
if bytesAvailable == 0:
raise Exception('Failed to get response!')
else:
logging.debug(
'Bytes available for read: {}'.format(bytesAvailable))
message = self.readBytes(bytesAvailable)
msgType, data, remainder = UbloxMessage.parse(message)
if remainder is not None:
logging.debug('Extra data in message buffer! ({})'.format(
len(remainder)))
return msgType, data, remainder
def reset(self, startType, clear=None, mode='hw'):
if clear is None:
if startType == 'hot':
navBbrMask = 0
elif startType == 'warm':
navBbrMask = 1
elif startType == 'cold':
navBbrMask = 0xff
else:
navBbrMask = UbloxMessage.buildMask(
clear, ubloxMessage.navBbrMaskShiftDict)
resetMode = ubloxMessage.resetModeDict[mode]
logging.info('Sending restart command... this will not be ACKed.')
self.sendMessage("CFG-RST", 4, {
'nav_bbr': navBbrMask,
'Reset': resetMode
})
def sendMessage(self, ser, msgFormat, length, data):
message = UbloxMessage.buildMessage(msgFormat, length, data)
ser.write(message)
def printMessage(self, msgTime, msgFormat, msgData):
UbloxMessage.printMessage(msgFormat, msgData, msgTime, fmt='short')
if args.debug:
logging.basicConfig(level=logging.DEBUG)
elif args.verbose:
logging.basicConfig(level=logging.INFO)
else:
logging.basicConfig(level=logging.ERROR)
if args.clear is None:
if args.startType == 'hot':
navBbrMask = 0
elif args.startType == 'warm':
navBbrMask = 1
elif args.startType == 'cold':
navBbrMask = 0xff
else:
navBbrMask = UbloxMessage.buildMask(args.clear,
UbloxMessage.navBbrMaskShiftDict)
resetMode = resetModeDict[args.mode]
ser = serial.Serial(args.device, 115200, timeout=1)
with serial.threaded.ReaderThread(ser, UbloxReader) as protocol:
msgFormat, msgData = protocol.poll(ser, 'MON-VER')
UbloxMessage.printMessage(msgFormat,
msgData,
header=datetime.datetime.now().strftime(
'[%Y-%m-%d %H:%M:%S.%f]\n'))
print('\nSending reset...')
protocol.sendMessage(ser, "CFG-RST", 4, {
'nav_bbr': navBbrMask,
'-l',
action='store_true',
help='Keep sending requests in a loop')
parser.add_argument('--verbose', '-v', action='store_true')
parser.add_argument('--debug', action='store_true')
args = parser.parse_args()
if args.debug:
logging.basicConfig(level=logging.DEBUG)
elif args.verbose:
logging.basicConfig(level=logging.INFO)
else:
logging.basicConfig(level=logging.ERROR)
ser = serial.Serial(args.device, 115200, timeout=1)
with serial.threaded.ReaderThread(ser, UbloxReader) as ublox:
ublox.printMessageFlag = True
msgFormat, msgData = ublox.poll(ser, 'CFG-PRT')
UbloxMessage.printMessage(msgFormat, msgData)
ublox.sendConfig(ser, msgFormat, 20, msgData)
ublox.saveStreamFlag = True
while 1:
try:
time.sleep(1)
except KeyboardInterrupt:
break
loop.quit()
elif ty == "ACK-NACK":
print('\nFailed to restore settings!')
loop.quit()
else:
elapsed = time.time() - lastStateTransitionTime
if elapsed > 1:
print('\n*** Configuration load request failed!')
loop.quit()
import sys; sys.exit(1)
return True
if __name__=='__main__':
import argparse
parser = argparse.ArgumentParser()
parser.add_argument('settings', nargs='+', choices=(clearMaskShiftDict.keys() + ['all', 'none']), help='Specify the settings to be reset to default. \'all\' will reset all settings and \'none\' will save none.')
parser.add_argument('--device', '-d', help='Specify the serial port device to communicate with. e.g. /dev/ttyO5')
args = parser.parse_args()
if args.device is not None:
t = ubx.Parser(callback, device=args.device)
else:
t = ubx.Parser(callback)
clearMask = UbloxMessage.buildMask(args.settings, clearMaskShiftDict)
print('Restoring default configuration...')
t.send("CFG-CFG", 12, {'clearMask': clearMask, 'saveMask': 0, 'loadMask': clearMask})
lastStateTransitionTime = time.time()
loop.run()
def sendMessage(self, messageType, length, data):
rawMessage = UbloxMessage.buildMessage(messageType, length, data)
self.sendRawMessage(rawMessage)
action='store_true',
help='Keep sending requests in a loop')
parser.add_argument('--verbose', '-v', action='store_true')
parser.add_argument('--debug', action='store_true')
args = parser.parse_args()
if args.debug:
logging.basicConfig(level=logging.DEBUG)
elif args.verbose:
logging.basicConfig(level=logging.INFO)
else:
logging.basicConfig(level=logging.ERROR)
ser = serial.Serial(args.device, 115200, timeout=1)
with serial.threaded.ReaderThread(ser, UbloxReader) as protocol:
try:
while True:
msgFormat, msgData = protocol.poll(ser, 'NAV-PVT')
UbloxMessage.printMessage(
msgFormat,
msgData,
header=datetime.datetime.now().strftime(
'[%Y-%m-%d %H:%M:%S.%f]\n'))
if not args.loop:
break
time.sleep(0.1)
except KeyboardInterrupt:
pass
parser = argparse.ArgumentParser()
parser.add_argument('input')
args = parser.parse_args()
inputDirectory, tail = os.path.split(args.input)
f = open(args.input, 'rb')
data = f.read()
outputFile = open(os.path.join(inputDirectory, 'ublox_solution.pos'), 'wt')
outputFile.write('% UTC latitude(deg) longitude(deg) height(m) Q ns sdn(m) sde(m) sdu(m) sdne(m) sdeu(m) sdun(m) age(s) ratio\n')
start = 0
numMessages = 0
numNavPvtMessages = 0
while start < (len(data) - 8):
rawMessage, msgClass, msgId, length, start = UbloxMessage.getMessageFromBuffer(data, start)
if rawMessage is not None:
payload = rawMessage[6:length+6]
try:
msgFormat, msgData = UbloxMessage.decode(msgClass, msgId, length, payload)
except ValueError:
continue
UbloxMessage.printMessage(msgFormat, msgData, None, fmt='short')
numMessages += 1
if msgFormat == 'NAV-PVT':
sdne = sdeu = sdun = 99.9999
age = ratio = 0.
curDt = datetime.datetime(msgData[0]['Year'], msgData[0]['Month'], msgData[0]['Day'],
def send(self, clid, length, payload):
logging.debug("Sending UBX packet of type %s: %s" % (clid, payload))
stream = UbloxMessage.buildMessage(clid, length, payload)
self.sendraw(stream)
def parseUbx(buf):
msgFormat, msgData, remainder = UbloxMessage.parse(buf, raw=True)
print('\nUBX {} | Length: {}'.format(msgFormat, len(msgData)))
output = {'messageType': 'UBX-{}'.format(msgFormat), 'payload': msgData}
return output, remainder
def parse(self, data, useRawCallback=False):
self.buffer += data
buffer_offset = 0
# Minimum packet length is 8
while len(self.buffer) >= buffer_offset + 8:
# Find the beginning of a UBX message
start = self.buffer.find(chr(SYNC1) + chr(SYNC2), buffer_offset)
# Could not find message - keep data because there may be a whole or partial NMEA message
# Consider limiting max buffer size
if start == -1:
return True
# Message shorter than minimum length - return and wait for additional data
# Consider limiting max buffer size
if start + 8 > len(self.buffer):
return True
# Decode header - message class, id, and length
(cl, id, length) = struct.unpack("<BBH",
self.buffer[start + 2:start + 6])
# Check that there is enough data in the buffer to match the length
# If not, return and wait for additional data
if len(self.buffer) < start + length + 8:
return True
# Validate checksum - if fail, skip past the sync
if self.checksum(
self.buffer[start + 2:start + length +
6]) != struct.unpack(
"<BB", self.buffer[start + length +
6:start + length + 8]):
buffer_offset = start + 2
continue
# At this point, we should have a valid message at the start position
# Handle data prior to UBX message
if start > 0:
logging.debug("Discarded data not UBX %s" %
repr(self.buffer[:start]))
# Attempt to decode NMEA on discarded data
self.decodeNmeaBuffer(self.buffer[:start])
if length == 0:
logging.warning(
'Zero length packet of class {}, id {}!'.format(
hex(cl), hex(id)))
else:
# Decode UBX message
try:
msgFormat, data = UbloxMessage.decode(
cl, id, length,
self.buffer[start + 6:start + length + 6])
except ValueError:
data = None
pass
if data is not None:
logging.debug("Got UBX packet of type %s: %s" %
(msgFormat, data))
self.callback(msgFormat, data)
if useRawCallback and (self.rawCallback is not None):
self.rawCallback(self.buffer[:start + length + 8])
# Discard packet
self.buffer = self.buffer[start + length + 8:]
buffer_offset = 0
with serial.threaded.ReaderThread(ser, UbloxReader) as ublox:
if args.output is not None:
ublox.saveFileName = os.path.join(args.output, 'ublox')
ublox.logger = logger
ublox.setSaveInterval(args.interval)
ublox.printMessageFlag = False
ublox.userHandler = messageHandler
if args.configure:
logger.info('*** Configuring receiver...')
# Set measurement rate to 1 Hz during config to prevent problems
logger.info('Setting measurement rate to 1 Hz...')
ublox.sendConfig(ser, 'CFG-RATE', 6, {'Meas': 1000, 'Nav': 1, 'Time': 1})
# Reset to default config
clearMask = UbloxMessage.buildMask(['msgConf'], clearMaskShiftDict)
logger.info('Restoring message configuration...')
ublox.sendConfig(ser, 'CFG-CFG', 12, {'clearMask': clearMask, 'saveMask': 0, 'loadMask': clearMask})
# Set power management settings
logger.info('Setting power management to full power...')
ublox.sendConfig(ser, 'CFG-PMS', 8, {'Version': 0, 'PowerSetupValue': 0, 'Period': 0, 'OnTime': 0})
# Disable NMEA output - UBX only
logger.info('Polling for port config (CFG-PRT)...')
msgFormat, msgData = ublox.poll(ser, 'CFG-PRT')
UbloxMessage.printMessage(msgFormat, msgData)
logger.info('Disabling NMEA output (CFG-PRT)...')
msgData[1]["Out_proto_mask"] = 1
ublox.sendConfig(ser, msgFormat, 20, msgData)
parser.add_argument('--startType', choices=['hot', 'warm', 'cold'], default='cold', help='Specify the start type. This controls what data is cleared. Use the \'clear\' option to specify individual sections to clear.')
parser.add_argument('--clear', '-c', choices=list(ubloxMessage.navBbrMaskShiftDict.keys()) + ['all', 'none'], nargs='+', default=None, help='Specify the data structures to clear. This overrides \'startType\'.')
parser.add_argument('--mode', '-m', choices=ubloxMessage.resetModeDict.keys(), default='hw', help='Specify the restart mode.\nsw: Controlled software reset\nswGnssOnly: Controlled software reset (GNSS Only)\nhw: Hardware reset (Watchdog) immediately\nhwShutdown: Hardware reset (Watchdog) after shutdown\ngnssStop: Controlled GNSS stop\ngnssStart: Controlled GNSS start')
args = parser.parse_args()
logging.basicConfig(level=logging.WARNING)
#logging.basicConfig(level=logging.DEBUG)
if args.clear is None:
if args.startType == 'hot':
navBbrMask = 0
elif args.startType == 'warm':
navBbrMask = 1
elif args.startType == 'cold':
navBbrMask = 0xff
else:
navBbrMask = UbloxMessage.buildMask(args.clear, UbloxMessage.navBbrMaskShiftDict)
resetMode = ubloxMessage.resetModeDict[args.mode]
with SMBusWrapper(args.bus) as bus:
ublox = UbloxI2C(bus)
data = ublox.poll('MON-VER', printMessage=True)
print('\nSending reset...')
ublox.sendConfig("CFG-RST", 4, {'nav_bbr': navBbrMask, 'Reset': resetMode})
