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