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'],
msgData[0]['Hour'], msgData[0]['Min'], msgData[0]['Sec'])
curDt += datetime.timedelta(microseconds=msgData[0]['Nano']/1e3)
line = curDt.strftime('%Y/%m/%d %H:%M:%S') + '.{:03.0f}'.format(curDt.microsecond/1e3)
line += ' '
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