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