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')