def read_msg(self):
"""Low-level message receiving function."""
start = time.time()
while (time.time()-start)<self.timeout:
new_start = time.time()
# Makes sure the buffer has 'size' bytes.
def waitfor(size=1):
read_buf = self.device.read(size)
for _ in range(10):
if len(read_buf) == size:
return read_buf
read_buf += self.device.read(size-len(read_buf))
raise MTException("timeout waiting for message.")
if ord(waitfor())<>0xFA:
continue
# second part of preamble
if ord(waitfor())<>0xFF: # we assume no timeout anymore
continue
# read message id and length of message
#msg = self.device.read(2)
mid, length = struct.unpack('!BB', waitfor(2))
isExtendedLength = False
if length==255: # extended length
length, = struct.unpack('!H', waitfor(2))
isExtendedLength = True
# read contents and checksum
buf = waitfor(length+1)
if (len(buf)<length+1):
continue
checksum = ord(buf[-1])
data = struct.unpack('!%dB'%length, buf[:-1])
if mid == MID.Error:
sys.stderr.write("MT error 0x%02X: %s."%(data[0],
MID.ErrorCodes[data[0]]))
if verbose:
print "MT: Got message id 0x%02X (%s) with %d data bytes: [%s]"%(mid, getMIDName(mid), length,
' '.join("%02X"% v for v in data))
# write to logfile
if self.logfile:
if isExtendedLength:
self.logfile.write(struct.pack('!BBBBH', 0xFA, 0xFF, mid, 0xFF, length))
else:
self.logfile.write(struct.pack('!BBBB', 0xFA, 0xFF, mid, length))
self.logfile.write(buf)
# checksum
chkStart = 0xFF+mid+checksum
if isExtendedLength:
chkStart += 0xFF+(0xFF&(length>>8))+(length&0xFF)
else:
chkStart += length
if (0xFF&sum(data, chkStart)):
sys.stderr.write("invalid checksum; discarding data and "\
"waiting for next message.\n")
continue
return (mid, buf[:-1])
else:
raise MTException("could not find message.")
def read_measurement(self, mode=None, settings=None):
# getting data
mid, data = self.read_msg()
if mid == MID.MTData2:
return self.parse_MTData2(data)
else:
raise MTException(
"Only MTData2 supported, use -f and -m to configure MTi.\n unknown data message: mid=0x%02X (%s)."
% (mid, getMIDName(mid)))
def read_measurement(self, mode=None, settings=None):
# getting data
#data = self.read_data_msg()
mid, data = self.read_msg()
if mid==MID.MTData:
return self.parse_MTData(data, mode, settings)
elif mid==MID.MTData2:
return self.parse_MTData2(data)
else:
raise MTException("unknown data message: mid=0x%02X (%s)."% (mid, getMIDName(mid)))
def read_measurement(self, mode=None, settings=None):
# getting data
#data = self.read_data_msg()
mid, data = self.read_msg()
if mid==MID.MTData:
return self.parse_MTData(data, mode, settings)
elif mid==MID.MTData2:
return self.parse_MTData2(data)
else:
raise MTException("unknown data message: mid=0x%02X (%s)."% (mid, getMIDName(mid)))
def read_msg(self):
"""Low-level message receiving function."""
start = time.time()
while (time.time() - start) < self.timeout:
new_start = time.time()
# Makes sure the buffer has 'size' bytes.
def waitfor(size=1):
while self.device.inWaiting() < size:
if time.time() - new_start >= self.timeout:
rospy.logwarn("timeout waiting for message")
c = self.device.read()
while (not c) and ((time.time() - new_start) < self.timeout):
c = self.device.read()
if not c:
rospy.logwarn("timeout waiting for message")
if ord(c) <> 0xFA:
continue
# second part of preamble
waitfor(3)
if ord(self.device.read()) <> 0xFF: # we assume no timeout anymore
continue
# read message id and length of message
#msg = self.device.read(2)
mid, length = struct.unpack('!BB', self.device.read(2))
if length == 255: # extended length
waitfor(2)
length, = struct.unpack('!H', self.device.read(2))
# read contents and checksum
waitfor(length + 1)
buf = self.device.read(length + 1)
while (len(buf) < length + 1) and (
(time.time() - start) < self.timeout):
buf += self.device.read(length + 1 - len(buf))
if (len(buf) < length + 1):
continue
checksum = ord(buf[-1])
data = struct.unpack('!%dB' % length, buf[:-1])
if mid == MID.Error:
sys.stderr.write("MT error 0x%02X: %s." %
(data[0], MID.ErrorCodes[data[0]]))
if verbose:
print "MT: Got message id 0x%02X (%s) with %d data bytes: [%s]" % (
mid, getMIDName(mid), length, ' '.join("%02X" % v
for v in data))
if 0xFF & sum(data, 0xFF + mid + length + checksum):
sys.stderr.write("invalid checksum; discarding data and "\
"waiting for next message.\n")
continue
return (mid, buf[:-1])
else:
raise MTException("could not find message.")
def read_msg(self):
"""Low-level message receiving function."""
start = time.time()
while (time.time()-start)<self.timeout:
new_start = time.time()
# Makes sure the buffer has 'size' bytes.
def waitfor(size=1):
while self.device.inWaiting() < size:
if time.time()-new_start >= self.timeout:
raise MTException("timeout waiting for message.")
c = self.device.read()
while (not c) and ((time.time()-new_start)<self.timeout):
c = self.device.read()
if not c:
raise MTException("timeout waiting for message.")
if ord(c)<>0xFA:
continue
# second part of preamble
waitfor(3)
if ord(self.device.read())<>0xFF: # we assume no timeout anymore
continue
# read message id and length of message
#msg = self.device.read(2)
mid, length = struct.unpack('!BB', self.device.read(2))
if length==255: # extended length
waitfor(2)
length, = struct.unpack('!H', self.device.read(2))
# read contents and checksum
waitfor(length+1)
buf = self.device.read(length+1)
while (len(buf)<length+1) and ((time.time()-start)<self.timeout):
buf+= self.device.read(length+1-len(buf))
if (len(buf)<length+1):
continue
checksum = ord(buf[-1])
data = struct.unpack('!%dB'%length, buf[:-1])
if mid == MID.Error:
sys.stderr.write("MT error 0x%02X: %s."%(data[0],
MID.ErrorCodes[data[0]]))
if verbose:
print "MT: Got message id 0x%02X (%s) with %d data bytes: [%s]"%(mid, getMIDName(mid), length,
' '.join("%02X"% v for v in data))
if 0xFF&sum(data, 0xFF+mid+length+checksum):
sys.stderr.write("invalid checksum; discarding data and "\
"waiting for next message.\n")
continue
return (mid, buf[:-1])
else:
raise MTException("could not find message.")
def write_msg(self, mid, data=[]):
"""Low-level message sending function."""
length = len(data)
if length>254:
lendat = [0xFF, 0xFF&length, 0xFF&(length>>8)]
else:
lendat = [length]
packet = [0xFA, 0xFF, mid] + lendat + list(data)
packet.append(0xFF&(-(sum(packet[1:]))))
msg = struct.pack('%dB'%len(packet), *packet)
while self.device.read():
#print ".",
pass
self.device.write(msg)
if verbose:
print "MT: Write message id 0x%02X (%s) with %d data bytes: [%s]"%(mid, getMIDName(mid), length,
' '.join("%02X"% v for v in data))
def write_msg(self, mid, data=[]):
"""Low-level message sending function."""
length = len(data)
if length>254:
lendat = [0xFF, 0xFF&length, 0xFF&(length>>8)]
else:
lendat = [length]
packet = [0xFA, 0xFF, mid] + lendat + list(data)
packet.append(0xFF&(-(sum(packet[1:]))))
msg = struct.pack('%dB'%len(packet), *packet)
while self.device.read():
#print ".",
pass
self.device.write(msg)
if verbose:
print "MT: Write message id 0x%02X (%s) with %d data bytes: [%s]"%(mid, getMIDName(mid), length,
' '.join("%02X"% v for v in data))
def read_measurement(self, mode=None, settings=None):
# getting data
mid, data = self.read_msg()
if mid==MID.MTData2:
return self.parse_MTData2(data)
else:
raise MTException("Only MTData2 supported, use -f and -m to configure MTi.\n unknown data message: mid=0x%02X (%s)."% (mid, getMIDName(mid)))
