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 ReqConfiguration(self):
"""Ask for the current configuration of the MT device.
Assume the device is in Config state."""
data_ack = self.write_ack(MID.SetOutputConfiguration,())
config = []
try:
for i in range(len(data_ack)/4):
config.append(struct.unpack('!HH', data_ack[i*4:i*4+4]))
except struct.error:
raise MTException("could not parse configuration.")
return config
def SetCurrentScenario(self, scenario_id):
"""Sets the XKF scenario to use.
Assume the device is in Config state."""
data = self.ReqAvailableScenarios()
availableSc = numpy.array(data)
validateSc = availableSc == str(scenario_id)
if validateSc.any():
self.write_ack(MID.SetCurrentScenario, (0x00, scenario_id&0xFF))
print "Set to scenario:%2d"%scenario_id
else:
raise MTException("not an available XKF scenario")
def parse_status(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # Status Byte
o['StatusByte'], = struct.unpack("!B", content)
elif (data_id & 0x00F0) == 0x20: # Status Word
o['StatusWord'], = struct.unpack("!L", content)
elif (data_id & 0x00F0) == 0x40: # RSSI
o['RSSI'], = struct.unpack("!b", content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def write_ack(self, mid, data=[]):
"""Send a message a read confirmation."""
self.write_msg(mid, data)
for tries in range(100):
mid_ack, data_ack = self.read_msg()
if mid_ack==(mid+1):
break
else:
raise MTException("Ack (0x%X) expected, MID 0x%X received instead"\
" (after 100 tries)."%(mid+1, mid_ack))
return data_ack
def parse_position(data_id, content, ffmt):
o = {}
heightFlag = False
if (data_id & 0x00F0) == 0x40: # LatLon
o['lat'], o['lon'] = struct.unpack('!' + 2 * ffmt, content)
elif (data_id & 0x00F0) == 0x20: # Altitude Ellipsoid
o['ellipsoid'] = struct.unpack('!' + 1 * ffmt, content)
heightFlag = True
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o, heightFlag
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 parse_angular_velocity(data_id, content, ffmt):
o = {}
# FIXME is it really 802y and 803y as in the doc?
if (data_id & 0x00F0) == 0x20: # Rate of Turn
o['gyrX'], o['gyrY'], o['gyrZ'] = \
struct.unpack('!'+3*ffmt, content)
elif (data_id & 0x00F0) == 0x30: # Delta Q
o['Delta q0'], o['Delta q1'], o['Delta q2'], o['Delta q3'] = \
struct.unpack('!'+4*ffmt, content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_SCR(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # ACC+GYR+MAG+Temperature
o['accX'], o['accY'], o['accZ'], o['gyrX'], o['gyrY'], \
o['gyrZ'], o['magX'], o['magY'], o['magZ'], o['Temp']=\
struct.unpack("!9Hh", content)
elif (data_id & 0x00F0) == 0x20: # Gyro Temperature
o['tempGyrX'], o['tempGyrY'], o['tempGyrZ'] = \
struct.unpack("!hhh", content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def find_baudrate(port):
baudrates = (115200, 460800, 921600, 230400, 57600, 38400, 19200, 9600)
for br in baudrates:
try:
mt = MTDevice(port, br)
except serial.SerialException:
raise MTException("unable to open %s" % port)
try:
mt.GoToConfig()
mt.GoToMeasurement()
return br
except MTException:
pass
def read_msg(self):
"""Low-level message receiving function."""
start = time.time()
while (time.time() - start) < self.timeout:
# read first char of preamble
c = self.device.read()
if not c:
raise MTException("timeout waiting for message.")
if ord(c) <> 0xFA:
continue
# second part of preamble
if ord(self.device.read()) <> 0xFF: # we assume no timeout anymore
continue
# read message id and length of message
mid, length = struct.unpack('!BB', self.device.read(2))
if length == 255: # extended length
length, = struct.unpack('!H', self.device.read(2))
# read contents and checksum
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]]))
# print "MT: Got message id 0x%02X with %d data bytes: [%s]"%(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 parse_orientation_data(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # Quaternion
o['Q0'], o['Q1'], o['Q2'], o['Q3'] = struct.unpack(
'!' + 4 * ffmt, content)
elif (data_id & 0x00F0) == 0x20: # Rotation Matrix
o['a'], o['b'], o['c'], o['d'], o['e'], o['f'], o['g'], o['h'],\
o['i'] = struct.unpack('!'+9*ffmt, content)
elif (data_id & 0x00F0) == 0x30: # Euler Angles
o['Roll'], o['Pitch'], o['Yaw'] = struct.unpack(
'!' + 3 * ffmt, content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_position(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # Altitude MSL
o['altMsl'], = struct.unpack('!' + ffmt, content)
elif (data_id & 0x00F0) == 0x20: # Altitude Ellipsoid
o['altEllipsoid'], = struct.unpack('!' + ffmt, content)
elif (data_id & 0x00F0) == 0x30: # Position ECEF
o['ecefX'], o['ecefY'], o['ecefZ'] = \
struct.unpack('!'+3*ffmt, content)
elif (data_id & 0x00F0) == 0x40: # LatLon
o['lat'], o['lon'] = struct.unpack('!' + 2 * ffmt, content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_acceleration(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # Delta V
o['Delta v.x'], o['Delta v.y'], o['Delta v.z'] = \
struct.unpack('!'+3*ffmt, content)
elif (data_id & 0x00F0) == 0x20: # Acceleration
o['accX'], o['accY'], o['accZ'] = \
struct.unpack('!'+3*ffmt, content)
elif (data_id & 0x00F0) == 0x30: # Free Acceleration
o['freeAccX'], o['freeAccY'], o['freeAccZ'] = \
struct.unpack('!'+3*ffmt, content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def write_ack(self, mid, data=b'', n_retries=500):
"""Send a message and read confirmation."""
self.write_msg(mid, data)
for _ in range(n_retries):
mid_ack, data_ack = self.read_msg()
if mid_ack == (mid + 1):
break
elif self.verbose:
print "ack (0x%02X) expected, got 0x%02X instead" % \
(mid+1, mid_ack)
else:
raise MTException("Ack (0x%02X) expected, MID 0x%02X received "
"instead (after %d retries)." %
(mid + 1, mid_ack, n_retries))
return data_ack
def ReqAvailableScenarios(self):
"""Request the available XKF scenarios on the device.
Assume the device is in Config state."""
scenarios_dat = self.write_ack(MID.ReqAvailableScenarios)
scenarios = []
try:
for i in range(len(scenarios_dat) / 22):
scenario_type, version, label =\
struct.unpack('!BB20s', scenarios_dat[22*i:22*(i+1)])
scenarios.append((scenario_type, version, label.strip()))
## available XKF scenarios
self.scenarios = scenarios
except struct.error:
raise MTException("could not parse the available XKF scenarios.")
return scenarios
def main():
parser = argparse.ArgumentParser(description='Process some integers.')
parser.add_argument('-v', '--verbose', action='store_true')
parser.add_argument('-d', '--device', required=True)
parser.add_argument('-b', '--baudrate', default=115200)
args = parser.parse_args()
if args.verbose:
mtdevice.verbose = True
try:
try:
mt = mtdevice.MTDevice(args.device, args.baudrate)
except serial.SerialException:
raise MTException("unable to open serial port %s!" % args.device)
print("Configuring calibration mode and settings")
# configure mode and settings for required data
mode = mtdevice.get_mode('r')
settings = mtdevice.get_settings('t')
mt.GoToConfig()
mt.SetOutputMode(mode)
mt.SetOutputSettings(settings)
mt.SetPeriod(1152)
mt.SetOutputSkipFactor(0)
# read config data to get device ID
conf = mt.ReqConfiguration()
# write data to file
fname = determine_file_name(
'MT_%08X_' % (conf['device ID']) + '%04d.mtb', 1)
print("Writing to file %s. Press CTRL-C to stop logging." % fname)
with open(fname, 'wb') as f:
try:
mt.set_logfile(f)
# increase of timeout may be needed
mt.device.timeout = 0.1
mt.ReqConfiguration()
mt.ReqEmts()
mt.GoToMeasurement()
while True:
mt.read_msg()
except KeyboardInterrupt:
mt.set_logfile(None)
f.close()
pass
except MTException as e:
#traceback.print_tb(sys.exc_info()[2])
print e
def parse_timestamp(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # UTC Time
o['ns'], o['Year'], o['Month'], o['Day'], o['Hour'],\
o['Minute'], o['Second'], o['Flags'] =\
struct.unpack('!LHBBBBBB', content)
elif (data_id & 0x00F0) == 0x20: # Packet Counter
o['PacketCounter'], = struct.unpack('!H', content)
elif (data_id & 0x00F0) == 0x30: # Integer Time of Week
o['TimeOfWeek'], = struct.unpack('!L', content)
elif (data_id & 0x00F0) == 0x40: # GPS Age
o['gpsAge'], = struct.unpack('!B', content)
elif (data_id & 0x00F0) == 0x50: # Pressure Age
o['pressureAge'], = struct.unpack('!B', content)
elif (data_id & 0x00F0) == 0x60: # Sample Time Fine
o['SampleTimeFine'], = struct.unpack('!L', content)
elif (data_id & 0x00F0) == 0x70: # Sample Time Coarse
o['SampleTimeCoarse'], = struct.unpack('!L', content)
elif (data_id & 0x00F0) == 0x80: # Frame Range
o['startFrame'], o['endFrame'] = struct.unpack('!HH', content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_MTData2(self, data):
# Functions to parse each type of packet
def parse_temperature(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # Temperature
o['Temp'], = struct.unpack('!' + ffmt, content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_timestamp(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # UTC Time
o['ns'], o['Year'], o['Month'], o['Day'], o['Hour'],\
o['Minute'], o['Second'], o['Flags'] =\
struct.unpack('!LHBBBBBB', content)
elif (data_id & 0x00F0) == 0x20: # Packet Counter
o['PacketCounter'], = struct.unpack('!H', content)
elif (data_id & 0x00F0) == 0x30: # Integer Time of Week
o['TimeOfWeek'], = struct.unpack('!L', content)
elif (data_id & 0x00F0) == 0x40: # GPS Age
o['gpsAge'], = struct.unpack('!B', content)
elif (data_id & 0x00F0) == 0x50: # Pressure Age
o['pressureAge'], = struct.unpack('!B', content)
elif (data_id & 0x00F0) == 0x60: # Sample Time Fine
o['SampleTimeFine'], = struct.unpack('!L', content)
elif (data_id & 0x00F0) == 0x70: # Sample Time Coarse
o['SampleTimeCoarse'], = struct.unpack('!L', content)
elif (data_id & 0x00F0) == 0x80: # Frame Range
o['startFrame'], o['endFrame'] = struct.unpack('!HH', content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_orientation_data(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # Quaternion
o['Q0'], o['Q1'], o['Q2'], o['Q3'] = struct.unpack(
'!' + 4 * ffmt, content)
elif (data_id & 0x00F0) == 0x20: # Rotation Matrix
o['a'], o['b'], o['c'], o['d'], o['e'], o['f'], o['g'], o['h'],\
o['i'] = struct.unpack('!'+9*ffmt, content)
elif (data_id & 0x00F0) == 0x30: # Euler Angles
o['Roll'], o['Pitch'], o['Yaw'] = struct.unpack(
'!' + 3 * ffmt, content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_pressure(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # Baro pressure
# FIXME is it really U4 as in the doc and not a float/double?
o['Pressure'], = struct.unpack('!L', content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_acceleration(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # Delta V
o['Delta v.x'], o['Delta v.y'], o['Delta v.z'] = \
struct.unpack('!'+3*ffmt, content)
elif (data_id & 0x00F0) == 0x20: # Acceleration
o['accX'], o['accY'], o['accZ'] = \
struct.unpack('!'+3*ffmt, content)
elif (data_id & 0x00F0) == 0x30: # Free Acceleration
o['freeAccX'], o['freeAccY'], o['freeAccZ'] = \
struct.unpack('!'+3*ffmt, content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_position(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # Altitude MSL
o['altMsl'], = struct.unpack('!' + ffmt, content)
elif (data_id & 0x00F0) == 0x20: # Altitude Ellipsoid
o['altEllipsoid'], = struct.unpack('!' + ffmt, content)
elif (data_id & 0x00F0) == 0x30: # Position ECEF
o['ecefX'], o['ecefY'], o['ecefZ'] = \
struct.unpack('!'+3*ffmt, content)
elif (data_id & 0x00F0) == 0x40: # LatLon
o['lat'], o['lon'] = struct.unpack('!' + 2 * ffmt, content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_angular_velocity(data_id, content, ffmt):
o = {}
# FIXME is it really 802y and 803y as in the doc?
if (data_id & 0x00F0) == 0x20: # Rate of Turn
o['gyrX'], o['gyrY'], o['gyrZ'] = \
struct.unpack('!'+3*ffmt, content)
elif (data_id & 0x00F0) == 0x30: # Delta Q
o['Delta q0'], o['Delta q1'], o['Delta q2'], o['Delta q3'] = \
struct.unpack('!'+4*ffmt, content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_GPS(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x30: # DOP
o['iTOW'], g, p, t, v, h, n, e = \
struct.unpack('!LHHHHHHH', content)
o['gDOP'], o['pDOP'], o['tDOP'], o['vDOP'], o['hDOP'], \
o['nDOP'], o['eDOP'] = 0.01*g, 0.01*p, 0.01*t, \
0.01*v, 0.01*h, 0.01*n, 0.01*e
elif (data_id & 0x00F0) == 0x40: # SOL
o['iTOW'], o['fTOW'], o['Week'], o['gpsFix'], o['Flags'], \
o['ecefX'], o['ecefY'], o['ecefZ'], o['pAcc'], \
o['ecefVX'], o['ecefVY'], o['ecefVZ'], o['sAcc'], \
o['pDOP'], o['numSV'] = \
struct.unpack('!LlhBBlllLlllLHxBx', content)
elif (data_id & 0x00F0) == 0x80: # Time UTC
o['iTOW'], o['tAcc'], o['nano'], o['year'], o['month'], \
o['day'], o['hour'], o['min'], o['sec'], o['valid'] = \
struct.unpack('!LLlHBBBBBB', content)
elif (data_id & 0x00F0) == 0xA0: # SV Info
o['iTOW'], o['numCh'] = struct.unpack('!LBxx', content[:8])
channels = []
ch = {}
for i in range(o['numCh']):
ch['chn'], ch['svid'], ch['flags'], ch['quality'], \
ch['cno'], ch['elev'], ch['azim'], ch['prRes'] = \
struct.unpack('!BBBBBbhl', content[8+12*i:20+12*i])
channels.append(ch)
o['channels'] = channels
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_SCR(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # ACC+GYR+MAG+Temperature
o['accX'], o['accY'], o['accZ'], o['gyrX'], o['gyrY'], \
o['gyrZ'], o['magX'], o['magY'], o['magZ'], o['Temp']=\
struct.unpack("!9Hh", content)
elif (data_id & 0x00F0) == 0x20: # Gyro Temperature
o['tempGyrX'], o['tempGyrY'], o['tempGyrZ'] = \
struct.unpack("!hhh", content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_analog_in(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # Analog In 1
o['analogIn1'], = struct.unpack("!H", content)
elif (data_id & 0x00F0) == 0x20: # Analog In 2
o['analogIn2'], = struct.unpack("!H", content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_magnetic(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x20: # Magnetic Field
o['magX'], o['magY'], o['magZ'] = \
struct.unpack("!3"+ffmt, content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_velocity(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # Velocity XYZ
o['velX'], o['velY'], o['velZ'] = \
struct.unpack("!3"+ffmt, content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_status(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # Status Byte
o['StatusByte'], = struct.unpack("!B", content)
elif (data_id & 0x00F0) == 0x20: # Status Word
o['StatusWord'], = struct.unpack("!L", content)
elif (data_id & 0x00F0) == 0x40: # RSSI
o['RSSI'], = struct.unpack("!b", content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
# data object
output = {}
while data:
try:
data_id, size = struct.unpack('!HB', data[:3])
if (data_id & 0x0003) == 0x3:
float_format = 'd'
elif (data_id & 0x0003) == 0x0:
float_format = 'f'
else:
raise MTException("fixed point precision not supported.")
content = data[3:3 + size]
data = data[3 + size]
group = data_id & 0xFF00
ffmt = float_format
if group == XDIGroup.Temperature:
output['Temperature'] = parse_temperature(
data_id, content, ffmt)
elif group == XDIGroup.Timestamp:
output['Timestamp'] = parse_timestamp(
data_id, content, ffmt)
elif group == XDIGroup.OrientationData:
output['Orientation Data'] = parse_orientation_data(
data_id, content, ffmt)
elif group == XDIGroup.Pressure:
output['Pressure'] = parse_pressure(data_id, content, ffmt)
elif group == XDIGroup.Acceleration:
output['Acceleration'] = parse_acceleration(
data_id, content, ffmt)
elif group == XDIGroup.Position:
output['Position'] = parse_position(data_id, content, ffmt)
elif group == XDIGroup.AngularVelocity:
output['Angular Velocity'] = parse_angular_velocity(
data_id, content, ffmt)
elif group == XDIGroup.GPS:
output['GPS'] = parse_GPS(data_id, content, ffmt)
elif group == XDIGroup.SensorComponentReadout:
output['SCR'] = parse_SCR(data_id, content, ffmt)
elif group == XDIGroup.AnalogIn:
output['Analog In'] = parse_analog_in(
data_id, content, ffmt)
elif group == XDIGroup.Magnetic:
output['Magnetic'] = parse_magnetic(data_id, content, ffmt)
elif group == XDIGroup.Velocity:
output['Velocity'] = parse_velocity(data_id, content, ffmt)
elif group == XDIGroup.Status:
output['Status'] = parse_status(data_id, content, ffmt)
else:
raise MTException("unknown XDI group: 0x%04X." % group)
except struct.error, e:
raise MTException("couldn't parse MTData2 message.")
class MTDevice(object):
"""XSens MT device communication object."""
def __init__(self,
port,
baudrate=115200,
timeout=0.1,
autoconf=True,
config_mode=False):
"""Open device."""
## serial interface to the device
self.device = serial.Serial(port,
baudrate,
timeout=timeout,
writeTimeout=timeout)
self.device.flushInput() # flush to make sure the port is ready TODO
self.device.flushOutput() # flush to make sure the port is ready TODO
## timeout for communication
self.timeout = timeout
if autoconf:
self.auto_config()
else:
## mode parameter of the IMU
self.mode = None
## settings parameter of the IMU
self.settings = None
## length of the MTData message
self.length = None
## header of the MTData message
self.header = None
if config_mode:
self.GoToConfig()
############################################################
# Low-level communication
############################################################
## Low-level message sending function.
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)
self.device.write(msg)
# print "MT: Write message id 0x%02X with %d data bytes: [%s]"%(mid,length,
# ' '.join("%02X"% v for v in data))
#self.device.flush() #TODO evaluate
## Low-level MTData receiving function.
# Take advantage of known message length.
def read_data_msg(self, buf=bytearray()):
"""Low-level MTData receiving function.
Take advantage of known message length."""
start = time.time()
if self.length > 254:
totlength = 7 + self.length
else:
totlength = 5 + self.length
while (time.time() - start) < self.timeout:
while len(buf) < totlength:
buf.extend(self.device.read(totlength - len(buf)))
preamble_ind = buf.find(self.header)
if preamble_ind == -1: # not found
# discard unexploitable data
#sys.stderr.write("MT: discarding (no preamble).\n")
del buf[:-3]
continue
elif preamble_ind: # found but not at start
# discard leading bytes
#sys.stderr.write("MT: discarding (before preamble).\n")
del buf[:preamble_ind]
# complete message for checksum
while len(buf) < totlength:
buf.extend(self.device.read(totlength - len(buf)))
if 0xFF & sum(buf[1:]):
#sys.stderr.write("MT: invalid checksum; discarding data and "\
# "waiting for next message.\n")
del buf[:buf.find(self.header) - 2]
continue
data = str(buf[-self.length - 1:-1])
del buf[:]
return data
else:
raise MTException("could not find MTData message.")
## Low-level message receiving function.
def read_msg(self):
"""Low-level message receiving function."""
start = time.time()
while (time.time() - start) < self.timeout:
# read first char of preamble
c = self.device.read()
if not c:
raise MTException("timeout waiting for message.")
if ord(c) <> 0xFA:
continue
# second part of preamble
if ord(self.device.read()) <> 0xFF: # we assume no timeout anymore
continue
# read message id and length of message
mid, length = struct.unpack('!BB', self.device.read(2))
if length == 255: # extended length
length, = struct.unpack('!H', self.device.read(2))
# read contents and checksum
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]]))
# print "MT: Got message id 0x%02X with %d data bytes: [%s]"%(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.")
## Send a message and read confirmation
def write_ack(self, mid, data=[]):
"""Send a message a read confirmation."""
self.write_msg(mid, data)
for tries in range(10):
mid_ack, data_ack = self.read_msg()
if mid_ack == (mid + 1):
break
else:
raise MTException("Ack (0x%X) expected, MID 0x%X received instead"\
" (after 10 tries)."%(mid+1, mid_ack))
return data_ack
############################################################
# High-level functions
############################################################
## Reset MT device.
def Reset(self):
"""Reset MT device."""
self.write_ack(MID.Reset)
## Place MT device in configuration mode.
def GoToConfig(self):
"""Place MT device in configuration mode."""
self.write_ack(MID.GoToConfig)
## Place MT device in measurement mode.
def GoToMeasurement(self):
"""Place MT device in measurement mode."""
self.write_ack(MID.GoToMeasurement)
## Restore MT device configuration to factory defaults (soft version).
def RestoreFactoryDefaults(self):
"""Restore MT device configuration to factory defaults (soft version).
"""
self.GoToConfig()
self.write_ack(MID.RestoreFactoryDef)
## Get current output mode.
# Assume the device is in Config state.
def GetOutputMode(self):
"""Get current output mode.
Assume the device is in Config state."""
data = self.write_ack(MID.SetOutputMode)
self.mode, = struct.unpack('!H', data)
return self.mode
## Select which information to output.
# Assume the device is in Config state.
def SetOutputMode(self, mode):
"""Select which information to output.
Assume the device is in Config state."""
H, L = (mode & 0xFF00) >> 8, mode & 0x00FF
self.write_ack(MID.SetOutputMode, (H, L))
## Get current output mode.
# Assume the device is in Config state.
def GetOutputSettings(self):
"""Get current output mode.
Assume the device is in Config state."""
data = self.write_ack(MID.SetOutputSettings)
self.settings, = struct.unpack('!I', data)
return self.settings
## Select how to output the information.
# Assume the device is in Config state.
def SetOutputSettings(self, settings):
"""Select how to output the information.
Assume the device is in Config state."""
HH, HL = (settings & 0xFF000000) >> 24, (settings & 0x00FF0000) >> 16
LH, LL = (settings & 0x0000FF00) >> 8, settings & 0x000000FF
self.write_ack(MID.SetOutputSettings, (HH, HL, LH, LL))
## Set the period of sampling.
# Assume the device is in Config state.
def SetPeriod(self, period):
"""Set the period of sampling.
Assume the device is in Config state."""
H, L = (period & 0xFF00) >> 8, period & 0x00FF
self.write_ack(MID.SetPeriod, (H, L))
## Set the output skip factor.
# Assume the device is in Config state.
def SetOutputSkipFactor(self, skipfactor):
"""Set the output skip factor.
Assume the device is in Config state."""
H, L = (skipfactor & 0xFF00) >> 8, skipfactor & 0x00FF
self.write_ack(MID.SetOutputSkipFactor, (H, L))
## Get data length.
# Assume the device is in Config state.
def ReqDataLength(self):
"""Get data length.
Assume the device is in Config state."""
data = self.write_ack(MID.ReqDataLength)
self.length, = struct.unpack('!H', data)
self.header = '\xFA\xFF\x32' + chr(self.length)
return self.length
## Ask for the current configuration of the MT device.
# Assume the device is in Config state.
def ReqConfiguration(self):
"""Ask for the current configuration of the MT device.
Assume the device is in Config state."""
config = self.write_ack(MID.ReqConfiguration)
try:
masterID, period, skipfactor, _, _, _, date, time, num, deviceID,\
length, mode, settings =\
struct.unpack('!IHHHHI8s8s32x32xHIHHI8x', config)
except struct.error:
raise MTException("could not parse configuration.")
self.mode = mode
self.settings = settings
self.length = length
self.header = '\xFA\xFF\x32' + chr(length)
conf = {
'output-mode': mode,
'output-settings': settings,
'length': length,
'period': period,
'skipfactor': skipfactor,
'Master device ID': masterID,
'date': date,
'time': time,
'number of devices': num,
'device ID': deviceID
}
return conf
## Set the baudrate of the device using the baudrate id.
# Assume the device is in Config state.
def SetBaudrate(self, brid):
"""Set the baudrate of the device using the baudrate id.
Assume the device is in Config state."""
self.write_ack(MID.SetBaudrate, (brid, ))
## Request the available XKF scenarios on the device.
# Assume the device is in Config state.
def ReqAvailableScenarios(self):
"""Request the available XKF scenarios on the device.
Assume the device is in Config state."""
scenarios_dat = self.write_ack(MID.ReqAvailableScenarios)
scenarios = []
try:
for i in range(len(scenarios_dat) / 22):
scenario_type, version, label =\
struct.unpack('!BB20s', scenarios_dat[22*i:22*(i+1)])
scenarios.append((scenario_type, version, label.strip()))
## available XKF scenarios
self.scenarios = scenarios
except struct.error:
raise MTException("could not parse the available XKF scenarios.")
return scenarios
## Request the ID of the currently used XKF scenario.
# Assume the device is in Config state.
def ReqCurrentScenario(self):
"""Request the ID of the currently used XKF scenario.
Assume the device is in Config state."""
data = self.write_ack(MID.ReqCurrentScenario)
## current XKF id
self.scenario_id, = struct.unpack('!H', data)
try:
scenarios = self.scenarios
except AttributeError:
scenarios = self.ReqAvailableScenarios()
for t, _, label in scenarios:
if t == self.scenario_id:
## current XKF label
self.scenario_label = label
break
else:
self.scenario_label = ""
return self.scenario_id, self.scenario_label
## Sets the XKF scenario to use.
# Assume the device is in Config state.
def SetCurrentScenario(self, scenario_id):
"""Sets the XKF scenario to use.
Assume the device is in Config state."""
self.write_ack(MID.SetCurrentScenario, (0x00, scenario_id & 0xFF))
############################################################
# High-level utility functions
############################################################
## Configure the mode and settings of the MT device.
def configure(self, mode, settings, period=None, skipfactor=None):
"""Configure the mode and settings of the MT device."""
self.GoToConfig()
self.SetOutputMode(mode)
self.SetOutputSettings(settings)
if period is not None:
self.SetPeriod(period)
if skipfactor is not None:
self.SetOutputSkipFactor(skipfactor)
self.GetOutputMode()
self.GetOutputSettings()
self.ReqDataLength()
self.GoToMeasurement()
## Read configuration from device.
def auto_config(self):
"""Read configuration from device."""
self.GoToConfig()
mode = self.GetOutputMode()
settings = self.GetOutputSettings()
length = self.ReqDataLength()
self.GoToMeasurement()
return mode, settings, length
## Read and parse a measurement packet
def read_measurement(self, mode=None, settings=None):
"""Read and parse a measurement packet."""
# getting mode
if mode is None:
mode = self.mode
if settings is None:
settings = self.settings
# getting data
data = self.read_data_msg()
#_, data = self.read_msg()
# data object
output = {}
try:
# raw IMU first
if mode & OutputMode.RAW:
o = {}
o['accX'], o['accY'], o['accZ'], o['gyrX'], o['gyrY'], o['gyrZ'],\
o['magX'], o['magY'], o['magZ'], o['temp'] =\
struct.unpack('!10H', data[:20])
data = data[20:]
output['RAW'] = o
# raw GPS second
if mode & OutputMode.RAWGPS:
o = {}
o['Press'], o['bPrs'], o['ITOW'], o['LAT'], o['LON'], o['ALT'],\
o['VEL_N'], o['VEL_E'], o['VEL_D'], o['Hacc'], o['Vacc'],\
o['Sacc'], o['bGPS'] = struct.unpack('!HBI6i3IB', data[:44])
data = data[44:]
output['RAWGPS'] = o
# temperature
if mode & OutputMode.Temp:
temp, = struct.unpack('!f', data[:4])
data = data[4:]
output['Temp'] = temp
# calibrated data
if mode & OutputMode.Calib:
o = {}
if not (settings & OutputSettings.CalibMode_GyrMag):
o['accX'], o['accY'], o['accZ'] = struct.unpack('!3f',\
data[:12])
data = data[12:]
if not (settings & OutputSettings.CalibMode_AccMag):
o['gyrX'], o['gyrY'], o['gyrZ'] = struct.unpack('!3f',\
data[:12])
data = data[12:]
if not (settings & OutputSettings.CalibMode_AccGyr):
o['magX'], o['magY'], o['magZ'] = struct.unpack('!3f',\
data[:12])
data = data[12:]
output['Calib'] = o
# orientation
if mode & OutputMode.Orient:
o = {}
if settings & OutputSettings.OrientMode_Euler:
o['roll'], o['pitch'], o['yaw'] = struct.unpack(
'!3f', data[:12])
data = data[12:]
elif settings & OutputSettings.OrientMode_Matrix:
a, b, c, d, e, f, g, h, i = struct.unpack('!9f', data[:36])
data = data[36:]
o['matrix'] = ((a, b, c), (d, e, f), (g, h, i))
else: # OutputSettings.OrientMode_Quaternion:
q0, q1, q2, q3 = struct.unpack('!4f', data[:16])
data = data[16:]
o['quaternion'] = (q0, q1, q2, q3)
output['Orient'] = o
# auxiliary
if mode & OutputMode.Auxiliary:
o = {}
if not (settings & OutputSettings.AuxiliaryMode_NoAIN1):
o['Ain_1'], = struct.unpack('!H', data[:2])
data = data[2:]
if not (settings & OutputSettings.AuxiliaryMode_NoAIN2):
o['Ain_2'], = struct.unpack('!H', data[:2])
data = data[2:]
output['Auxiliary'] = o
# position
if mode & OutputMode.Position:
o = {}
o['Lat'], o['Lon'], o['Alt'] = struct.unpack('!3f', data[:12])
data = data[12:]
output['Position'] = o
# velocity
if mode & OutputMode.Velocity:
o = {}
o['Vel_X'], o['Vel_Y'], o['Vel_Z'] = struct.unpack(
'!3f', data[:12])
data = data[12:]
output['Velocity'] = o
# status
if mode & OutputMode.Status:
status, = struct.unpack('!B', data[:1])
data = data[1:]
output['Status'] = status
# sample counter
if settings & OutputSettings.Timestamp_SampleCnt:
TS, = struct.unpack('!H', data[:2])
data = data[2:]
output['Sample'] = TS
except struct.error, e:
raise MTException("could not parse MTData message.")
if data <> '':
raise MTException("could not parse MTData message (too long).")
return output
data = data[12:]
output['Vel'] = o
# status
if mode & OutputMode.Status:
status, = struct.unpack('!B', data[:1])
data = data[1:]
output['Stat'] = status
# sample counter
if settings & OutputSettings.Timestamp_SampleCnt:
TS, = struct.unpack('!H', data[:2])
data = data[2:]
output['Sample'] = TS
except struct.error, e:
raise MTException("could not parse MTData message.")
if data <> '':
raise MTException("could not parse MTData message (too long).")
return output
## Change the baudrate, reset the device and reopen communication.
def ChangeBaudrate(self, baudrate):
"""Change the baudrate, reset the device and reopen communication."""
self.GoToConfig()
brid = Baudrates.get_BRID(baudrate)
self.SetBaudrate(brid)
self.Reset()
#self.device.flush()
self.device.baudrate = baudrate
#self.device.flush()
time.sleep(0.01)
self.read_msg()
self.write_msg(0x3f)
def waitfor(size=1):
while self.device.inWaiting() < size:
if time.time() - new_start >= self.timeout:
raise MTException("timeout waiting for message.")
def parse_MTData(self, data, mode=None, settings=None):
"""Read and parse a measurement packet."""
# getting mode
if mode is None:
mode = self.mode
if settings is None:
settings = self.settings
# data object
output = {}
try:
# raw IMU first
if mode & OutputMode.RAW:
o = {}
o['accX'], o['accY'], o['accZ'], o['gyrX'], o['gyrY'], o['gyrZ'],\
o['magX'], o['magY'], o['magZ'], o['temp'] =\
struct.unpack('!10H', data[:20])
data = data[20:]
output['RAW'] = o
# raw GPS second
if mode & OutputMode.RAWGPS:
o = {}
o['Press'], o['bPrs'], o['ITOW'], o['LAT'], o['LON'], o['ALT'],\
o['VEL_N'], o['VEL_E'], o['VEL_D'], o['Hacc'], o['Vacc'],\
o['Sacc'], o['bGPS'] = struct.unpack('!HBI6i3IB', data[:44])
data = data[44:]
output['RAWGPS'] = o
# temperature
if mode & OutputMode.Temp:
temp, = struct.unpack('!f', data[:4])
data = data[4:]
output['Temp'] = temp
# calibrated data
if mode & OutputMode.Calib:
o = {}
if not (settings & OutputSettings.CalibMode_GyrMag):
o['accX'], o['accY'], o['accZ'] = struct.unpack('!3f',\
data[:12])
data = data[12:]
if not (settings & OutputSettings.CalibMode_AccMag):
o['gyrX'], o['gyrY'], o['gyrZ'] = struct.unpack('!3f',\
data[:12])
data = data[12:]
if not (settings & OutputSettings.CalibMode_AccGyr):
o['magX'], o['magY'], o['magZ'] = struct.unpack('!3f',\
data[:12])
data = data[12:]
output['Calib'] = o
# orientation
if mode & OutputMode.Orient:
o = {}
if settings & OutputSettings.OrientMode_Euler:
o['roll'], o['pitch'], o['yaw'] = struct.unpack(
'!3f', data[:12])
data = data[12:]
elif settings & OutputSettings.OrientMode_Matrix:
a, b, c, d, e, f, g, h, i = struct.unpack('!9f', data[:36])
data = data[36:]
o['matrix'] = ((a, b, c), (d, e, f), (g, h, i))
else: # OutputSettings.OrientMode_Quaternion:
q0, q1, q2, q3 = struct.unpack('!4f', data[:16])
data = data[16:]
o['quaternion'] = (q0, q1, q2, q3)
output['Orient'] = o
# auxiliary
if mode & OutputMode.Auxiliary:
o = {}
if not (settings & OutputSettings.AuxiliaryMode_NoAIN1):
o['Ain_1'], = struct.unpack('!H', data[:2])
data = data[2:]
if not (settings & OutputSettings.AuxiliaryMode_NoAIN2):
o['Ain_2'], = struct.unpack('!H', data[:2])
data = data[2:]
output['Auxiliary'] = o
# position
if mode & OutputMode.Position:
o = {}
o['Lat'], o['Lon'], o['Alt'] = struct.unpack('!3f', data[:12])
data = data[12:]
output['Pos'] = o
# velocity
if mode & OutputMode.Velocity:
o = {}
o['Vel_X'], o['Vel_Y'], o['Vel_Z'] = struct.unpack(
'!3f', data[:12])
data = data[12:]
output['Vel'] = o
# status
if mode & OutputMode.Status:
status, = struct.unpack('!B', data[:1])
data = data[1:]
output['Stat'] = status
# sample counter
if settings & OutputSettings.Timestamp_SampleCnt:
TS, = struct.unpack('!H', data[:2])
data = data[2:]
output['Sample'] = TS
except struct.error, e:
raise MTException("could not parse MTData message.")
def parse_MTData2(self, data):
# Functions to parse each type of packet
def parse_temperature(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # Temperature
o['Temp'], = struct.unpack('!' + ffmt, content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_timestamp(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # UTC Time
o['ns'], o['Year'], o['Month'], o['Day'], o['Hour'],\
o['Minute'], o['Second'], o['Flags'] =\
struct.unpack('!LHBBBBBB', content)
elif (data_id & 0x00F0) == 0x20: # Packet Counter
o['PacketCounter'], = struct.unpack('!H', content)
elif (data_id & 0x00F0) == 0x30: # Integer Time of Week
o['TimeOfWeek'], = struct.unpack('!L', content)
elif (data_id & 0x00F0) == 0x40: # GPS Age
o['gpsAge'], = struct.unpack('!B', content)
elif (data_id & 0x00F0) == 0x50: # Pressure Age
o['pressureAge'], = struct.unpack('!B', content)
elif (data_id & 0x00F0) == 0x60: # Sample Time Fine
o['SampleTimeFine'], = struct.unpack('!L', content)
elif (data_id & 0x00F0) == 0x70: # Sample Time Coarse
o['SampleTimeCoarse'], = struct.unpack('!L', content)
elif (data_id & 0x00F0) == 0x80: # Frame Range
o['startFrame'], o['endFrame'] = struct.unpack('!HH', content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_orientation_data(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # Quaternion
o['Q0'], o['Q1'], o['Q2'], o['Q3'] = struct.unpack(
'!' + 4 * ffmt, content)
elif (data_id & 0x00F0) == 0x20: # Rotation Matrix
o['a'], o['b'], o['c'], o['d'], o['e'], o['f'], o['g'], o['h'],\
o['i'] = struct.unpack('!'+9*ffmt, content)
elif (data_id & 0x00F0) == 0x30: # Euler Angles
o['Roll'], o['Pitch'], o['Yaw'] = struct.unpack(
'!' + 3 * ffmt, content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_pressure(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # Baro pressure
# FIXME is it really U4 as in the doc and not a float/double?
o['Pressure'], = struct.unpack('!L', content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_acceleration(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # Delta V
o['Delta v.x'], o['Delta v.y'], o['Delta v.z'] = \
struct.unpack('!'+3*ffmt, content)
elif (data_id & 0x00F0) == 0x20: # Acceleration
o['accX'], o['accY'], o['accZ'] = \
struct.unpack('!'+3*ffmt, content)
elif (data_id & 0x00F0) == 0x30: # Free Acceleration
o['freeAccX'], o['freeAccY'], o['freeAccZ'] = \
struct.unpack('!'+3*ffmt, content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_position(data_id, content, ffmt):
o = {}
heightFlag = False
if (data_id & 0x00F0) == 0x40: # LatLon
o['lat'], o['lon'] = struct.unpack('!' + 2 * ffmt, content)
elif (data_id & 0x00F0) == 0x20: # Altitude Ellipsoid
o['ellipsoid'] = struct.unpack('!' + 1 * ffmt, content)
heightFlag = True
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o, heightFlag
def parse_angular_velocity(data_id, content, ffmt):
o = {}
# FIXME is it really 802y and 803y as in the doc?
if (data_id & 0x00F0) == 0x20: # Rate of Turn
o['gyrX'], o['gyrY'], o['gyrZ'] = \
struct.unpack('!'+3*ffmt, content)
elif (data_id & 0x00F0) == 0x30: # Delta Q
o['Delta q0'], o['Delta q1'], o['Delta q2'], o['Delta q3'] = \
struct.unpack('!'+4*ffmt, content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_GNSS(data_id, content, ffmt):
o = {}
pvtFlag = False
if (data_id & 0x00F0) == 0x10: # GNSS PVT DATA
o['iTOW'],x1,x2,x3,x4,x5,x6,x7,x8,x9,o['fix'],o['flag'],o['nSat'],x10,lon,lat,h,a,hAcc, \
vAcc,vN,vE,vD,x11,x12,sAcc,headAcc,headVeh,gDop,pDop,tDop,vDop,hDop,nDop,eDop = \
struct.unpack('!LHBBBBBBLiBBBBiiiiLLiiiiiLLIHHHHHHH', content)
o['lat'], o['lon'], o['hEll'], o['hMsl'], o['velN'], o['velE'], o['velD'], \
o['horzAcc'], o['vertAcc'], o['speedAcc'], o['GDOP'], o['PDOP'], o['TDOP'],\
o['VDOP'], o['HDOP'], o['NDOP'], o['EDOP'], o['heading'], o['headingAcc'] = 1e-7*lat, 1e-7*lon, 1e-3*h, \
1e-3*a, 1e-3*vN, 1e-3*vE, 1e-3*vD, 1e-3*hAcc, 1e-3*vAcc, 1e-3*sAcc, 1e-2*gDop, \
1e-2*pDop, 1e-2*tDop, 1e-2*vDop, 1e-2*hDop, 1e-2*nDop, 1e-2*eDop, 1e-5*headVeh, 1e-5*headAcc
pvtFlag = True
elif (data_id & 0x00F0) == 0x20: # GNSS SAT Info
o['iTOW'], o['numCh'] = struct.unpack('!LBxxx', content[:8])
channels = []
ch = {}
for i in range(o['numCh']):
ch['gnssId'], ch['svId'], ch['cno'], ch['flags'] = \
struct.unpack('!BBBB', content[8+4*i:12+4*i])
channels.append(ch)
ch = {} # empty
o['channels'] = channels
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o, pvtFlag
def parse_SCR(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # ACC+GYR+MAG+Temperature
o['accX'], o['accY'], o['accZ'], o['gyrX'], o['gyrY'], \
o['gyrZ'], o['magX'], o['magY'], o['magZ'], o['Temp']=\
struct.unpack("!9Hh", content)
elif (data_id & 0x00F0) == 0x20: # Gyro Temperature
o['tempGyrX'], o['tempGyrY'], o['tempGyrZ'] = \
struct.unpack("!hhh", content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_analog_in(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # Analog In 1
o['analogIn1'], = struct.unpack("!H", content)
elif (data_id & 0x00F0) == 0x20: # Analog In 2
o['analogIn2'], = struct.unpack("!H", content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_magnetic(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x20: # Magnetic Field
o['magX'], o['magY'], o['magZ'] = \
struct.unpack("!3"+ffmt, content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_velocity(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # Velocity XYZ
o['velX'], o['velY'], o['velZ'] = \
struct.unpack("!3"+ffmt, content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
def parse_status(data_id, content, ffmt):
o = {}
if (data_id & 0x00F0) == 0x10: # Status Byte
o['StatusByte'], = struct.unpack("!B", content)
elif (data_id & 0x00F0) == 0x20: # Status Word
o['StatusWord'], = struct.unpack("!L", content)
elif (data_id & 0x00F0) == 0x40: # RSSI
o['RSSI'], = struct.unpack("!b", content)
else:
raise MTException("unknown packet: 0x%04X." % data_id)
return o
# data object
output = {}
while data:
try:
data_id, size = struct.unpack('!HB', data[:3])
if (data_id & 0x0003) == 0x3:
float_format = 'd'
elif (data_id & 0x0003) == 0x0:
float_format = 'f'
else:
raise MTException("fixed point precision not supported.")
content = data[3:3 + size]
data = data[3 + size:]
group = data_id & 0xFF00
ffmt = float_format
if group == XDIGroup.Temperature:
output['Temperature'] = parse_temperature(
data_id, content, ffmt)
elif group == XDIGroup.Timestamp:
output['Timestamp'] = parse_timestamp(
data_id, content, ffmt)
elif group == XDIGroup.OrientationData:
output['Orientation Data'] = parse_orientation_data(
data_id, content, ffmt)
elif group == XDIGroup.Pressure:
output['Pressure'] = parse_pressure(data_id, content, ffmt)
elif group == XDIGroup.Acceleration:
output['Acceleration'] = parse_acceleration(
data_id, content, ffmt)
elif group == XDIGroup.Position:
temp, dataFlagPos = parse_position(data_id, content, ffmt)
if dataFlagPos:
output['Altitude'] = temp
else:
output['Latlon'] = temp
elif group == XDIGroup.AngularVelocity:
output['Angular Velocity'] = parse_angular_velocity(
data_id, content, ffmt)
elif group == XDIGroup.GNSS:
temp, dataFlagGnss = parse_GNSS(data_id, content, ffmt)
if dataFlagGnss:
output['Gnss PVT'] = temp
else:
output['Gnss SATINFO'] = temp
elif group == XDIGroup.SensorComponentReadout:
output['SCR'] = parse_SCR(data_id, content, ffmt)
elif group == XDIGroup.AnalogIn:
output['Analog In'] = parse_analog_in(
data_id, content, ffmt)
elif group == XDIGroup.Magnetic:
output['Magnetic'] = parse_magnetic(data_id, content, ffmt)
elif group == XDIGroup.Velocity:
output['Velocity'] = parse_velocity(data_id, content, ffmt)
elif group == XDIGroup.Status:
output['Status'] = parse_status(data_id, content, ffmt)
else:
raise MTException("unknown XDI group: 0x%04X." % group)
except struct.error, e:
raise MTException("couldn't parse MTData2 message.")
def configureMti(self, mtiSampleRate, mtiMode):
"""Configure the mode and settings of the MTMk4 device."""
self.GoToConfig()
self.timeout = math.pow(
mtiSampleRate,
-1) + MID.additionalTimeOutOffset # additional 5ms leeway
print "Timeout changed to %1.3fs based on current settings." % (
self.timeout)
mid = MID.SetOutputConfiguration
midReqDID = MID.ReqDID
dataReqDID = (0x00, 0x00)
dataDID = self.write_ack(midReqDID, dataReqDID)
try:
masterID = struct.unpack('!L', dataDID)
except struct.error:
raise MTException("could not parse configuration.")
# to have a clear distinction between MTi-G-700 and 100-series devices
deviceIDProductMask = hex(masterID[0] & 0x00f00000)
deviceTypeMask = hex(masterID[0] & 0x0f000000)
# check for user input for the delta q and delta v quantities
new_imu_period = XDIMessage.DeltaQFs
if mtiSampleRate < int(new_imu_period):
new_imu_period = mtiSampleRate
# check for user input for the rate IMU quantities
rate_imu_period = XDIMessage.RateOfTurnFs
if mtiSampleRate < int(rate_imu_period):
rate_imu_period = mtiSampleRate
# check for user input for the mag quantities
new_mag_period = XDIMessage.MagneticFieldFs
if mtiSampleRate < int(new_mag_period):
new_mag_period = mtiSampleRate
# check for user input for the baro samples
new_pressure_period = XDIMessage.PressureFs
if mtiSampleRate < int(new_pressure_period):
new_pressure_period = mtiSampleRate
if (deviceIDProductMask[2] == XDIProductMask.MTi1Series):
new_imu_period = rate_imu_period = new_mag_period = XDIMessage.FsModule
if mtiSampleRate < int(new_imu_period):
new_imu_period = rate_imu_period = new_mag_period = mtiSampleRate
# All messages with corresponding output data rates
"Packet couter, SampleTimeFine"
# mPc = self.getMtiConfigBytes(XDIMessage.PacketCounter, XDIMessage.PaddedFs)
mStf = self.getMtiConfigBytes(XDIMessage.SampleTimeFine,
XDIMessage.PaddedFs)
"Sensor data"
mImuDq = self.getMtiConfigBytes(XDIMessage.DeltaQ, new_imu_period)
mImuDv = self.getMtiConfigBytes(XDIMessage.DeltaV, new_imu_period)
"Sensor data (rate quantities)"
mImuMag = self.getMtiConfigBytes(XDIMessage.MagneticField,
new_mag_period)
mImuGyr = self.getMtiConfigBytes(XDIMessage.RateOfTurn,
rate_imu_period)
mImuAcc = self.getMtiConfigBytes(XDIMessage.Acceleration,
rate_imu_period)
"Baro data"
mImuP = self.getMtiConfigBytes(XDIMessage.Pressure,
new_pressure_period)
"GNSS data"
mGnssPvt = self.getMtiConfigBytes(XDIMessage.GnssPvtData,
XDIMessage.GnssFs)
mGnssSat = self.getMtiConfigBytes(XDIMessage.GnssSatInfo,
XDIMessage.GnssFs)
"Status word"
mSw = self.getMtiConfigBytes(XDIMessage.StatusWord,
XDIMessage.PaddedFs)
# Filter related messages
"Filter estimate"
mOrientationQuat = self.getMtiConfigBytes(XDIMessage.OrientationQuat,
rate_imu_period)
mOrientation = self.getMtiConfigBytes(XDIMessage.Orientation,
rate_imu_period)
mVelocity = self.getMtiConfigBytes(XDIMessage.Velocity,
rate_imu_period)
mPosition = self.getMtiConfigBytes(XDIMessage.PositionLatLon,
rate_imu_period)
mHeight = self.getMtiConfigBytes(XDIMessage.PositionHeight,
rate_imu_period)
# Output configuration set based on the product ID and user specification
if (deviceIDProductMask[2] == XDIProductMask.MTi100Series) & (
deviceTypeMask[2] == XDIProductMask.MTi700Device):
print "MTi-G-700/710 (GNSS/INS) device detected."
if mtiMode == 1:
print "Enabled publishing all sensor data"
data = mStf + mImuDq + mImuDv + mImuMag + mImuP + mGnssPvt + mGnssSat + mSw + mOrientationQuat
elif mtiMode == 2:
print "Enabled publishing all sensor data (rate quantities)"
data = mStf + mImuGyr + mImuAcc + mImuMag + mImuP + mGnssPvt + mGnssSat + mSw + mOrientationQuat
elif mtiMode == 3:
print "Enabled publishing all filter estimates"
data = mStf + mSw + mOrientation + mVelocity + mPosition + mHeight
else:
raise MTException("unknown mtiMode: (%d)." % (mtiMode))
elif deviceIDProductMask[2] == XDIProductMask.MTi100Series:
print "MTi-100/200/300 device detected."
if mtiMode == 1:
print "Enabled publishing all sensor data"
data = mStf + mImuDq + mImuDv + mImuMag + mImuP + mSw + mOrientationQuat
elif mtiMode == 2:
print "Enabled publishing all sensor data (rate quantities)"
data = mStf + mImuGyr + mImuAcc + mImuMag + mImuP + mSw + mOrientationQuat
elif mtiMode == 3:
print "Enabled publishing all filter estimates"
data = mStf + mSw + mOrientation
else:
raise MTException("unknown mtiMode: (%d)." % (mtiMode))
elif deviceIDProductMask[2] == XDIProductMask.MTi10Series:
print "MTi-10/20/30 device detected"
if mtiMode == 1:
print "Enabled publishing all sensor data"
data = mStf + mImuDq + mImuDv + mImuMag + mSw + mOrientationQuat
elif mtiMode == 2:
print "Enabled publishing all sensor data (rate quantities)"
data = mStf + mImuGyr + mImuAcc + mImuMag + mSw + mOrientationQuat
elif mtiMode == 3:
print "Enabled publishing all filter estimates"
data = mStf + mSw + mOrientation
else:
raise MTException("unknown mtiMode: (%d)." % (mtiMode))
elif (deviceIDProductMask[2] == XDIProductMask.MTi1Series) & (
deviceTypeMask[2] == XDIProductMask.MTi7Device):
print "MTi-7 device detected"
if mtiMode == 1:
print "Enabled publishing all sensor data"
data = mStf + mImuDq + mImuDv + mImuMag + mImuP + mGnssPvt + mSw + mOrientationQuat
elif mtiMode == 2:
print "Enabled publishing all sensor data (rate quantities)"
data = mStf + mImuGyr + mImuAcc + mImuMag + mImuP + mGnssPvt + mSw + mOrientationQuat
elif mtiMode == 3:
print "Enabled publishing all filter estimates"
data = mStf + mSw + mOrientation + mVelocity + mPosition + mHeight
else:
raise MTException("unknown mtiMode: (%d)." % (mtiMode))
elif deviceIDProductMask[2] == XDIProductMask.MTi1Series:
print "MTi-1/2/3 device detected"
if mtiMode == 1:
print "Enabled publishing all sensor data"
data = mStf + mImuDq + mImuDv + mImuMag + mSw + mOrientationQuat
elif mtiMode == 2:
print "Enabled publishing all sensor data (rate quantities)"
data = mStf + mImuGyr + mImuAcc + mImuMag + mSw + mOrientationQuat
elif mtiMode == 3:
print "Enabled publishing all filter estimates"
data = mStf + mSw + mOrientation
else:
raise MTException("unknown mtiMode: (%d)." % (mtiMode))
else:
raise MTException("Unknown device")
self.write_msg(mid, data)
# check for the set baudrate
dataAck = self.write_ack(MID.SetBaudrate, ())
bridAck = struct.unpack('!B', dataAck)
brSettings = Baudrates.get_BR(bridAck[0])
print "Device configured at %1.0f bps" % (brSettings)
self.GoToMeasurement()
print "Using %s @ %d" % devs[0]
device, baudrate = devs[0]
else:
print "No suitable device found."
return 1
# find baudrate
if not baudrate:
baudrate = find_baudrate(device)
if not baudrate:
print "No suitable baudrate found."
return 1
# open device
try:
mt = MTDevice(device, baudrate)
except serial.SerialException:
raise MTException("unable to open %s" % device)
# execute actions
if 'inspect' in actions:
mt.GoToConfig()
print "Device: %s at %d bps:" % (device, baudrate)
print "General configuration:", mt.ReqConfiguration()
print "Available scenarios:", mt.ReqAvailableScenarios()
print "Current scenario: %s (id: %d)" % mt.ReqCurrentScenario(
)[::-1]
mt.GoToMeasurement()
if 'change-baudrate' in actions:
print "Changing baudrate from %d to %d bps\n" % (baudrate,
new_baudrate),
sys.stdout.flush()
mt.ChangeBaudrate(new_baudrate)
if 'reset' in actions:
