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
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)
# Enable messages
rate = 1
messageList = [('NAV-PVT', 1), ('NAV-STATUS', 1), ('NAV-SVINFO', 1), ('RXM-RAWX', 1), ('RXM-SFRBX', 1)]
for message, rate in messageList:
print('Enabling {} message...'.format(message))
setMessageRate(ser, ublox, message, rate)
# Configure constellations (CFG-GNSS)
# Configure automobile dynamics (CFG-NAV5)
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
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 += ' '
line += '{:14.9f} {:14.9f} {:10.4f}'.format(msgData[0]['LAT']/1e7, msgData[0]['LON']/1e7, msgData[0]['HEIGHT']/1e3)
line += ' {:3d} {:3d} {:8.4f} {:8.4f} {:8.4f}'.format(5, msgData[0]['NumSV'], msgData[0]['Hacc']/1e3, msgData[0]['Hacc']/1e3, msgData[0]['Vacc']/1e3)
line += ' {:8.4f} {:8.4f} {:8.4f} {:6.2f} {:6.1f}'.format(sdne, sdeu, sdun, age, ratio)
line += '\n'
def printMessage(self, msgTime, msgFormat, msgData):
UbloxMessage.printMessage(msgFormat, msgData, msgTime, fmt='short')