mtdevice.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import serial
3 import struct
4 import sys
5 import getopt
6 import time
7 import datetime
8 import glob
9 import re
10 import pprint
11 
12 from mtdef import MID, OutputMode, OutputSettings, MTException, Baudrates, \
13  XDIGroup, getMIDName, DeviceState, DeprecatedMID, MTErrorMessage, \
14  MTTimeoutException
15 
16 
17 
20 class MTDevice(object):
21  """XSens MT device communication object."""
22 
23  def __init__(self, port, baudrate=115200, timeout=0.002, autoconf=True,
24  config_mode=False, verbose=False, initial_wait=0.1):
25  """Open device."""
26  self.verbose = verbose
27  # serial interface to the device
28  try:
29  self.device = serial.Serial(port, baudrate, timeout=timeout,
30  writeTimeout=timeout)
31  except IOError:
32  # FIXME with pyserial3 we might need some specific flags
33  self.device = serial.Serial(port, baudrate, timeout=timeout,
34  writeTimeout=timeout, rtscts=True,
35  dsrdtr=True)
36  time.sleep(initial_wait) # open returns before device is ready
37  self.device.flushInput()
38  self.device.flushOutput()
39  # timeout for communication
40  self.timeout = 100*timeout
41  # state of the device
42  self.state = None
43  if autoconf:
44  self.auto_config_legacy()
45  else:
46  # mode parameter of the IMU
47  self.mode = None
48  # settings parameter of the IMU
49  self.settings = None
50  # length of the MTData message
51  self.length = None
52  # header of the MTData message
53  self.header = None
54  if config_mode:
55  self.GoToConfig()
56 
57 
60  def write_msg(self, mid, data=b''):
61  """Low-level message sending function."""
62  length = len(data)
63  if length > 254:
64  lendat = b'\xFF' + struct.pack('!H', length)
65  else:
66  lendat = struct.pack('!B', length)
67  packet = b'\xFA\xFF' + struct.pack('!B', mid) + lendat + data
68  packet += struct.pack('!B', 0xFF & (-(sum(map(ord, packet[1:])))))
69  msg = packet
70  start = time.time()
71  while ((time.time()-start) < self.timeout) and self.device.read():
72  pass
73  try:
74  self.device.write(msg)
75  except serial.serialutil.SerialTimeoutException:
76  raise MTTimeoutException("writing message")
77  if self.verbose:
78  print "MT: Write message id 0x%02X (%s) with %d data bytes: "\
79  "[%s]" % (mid, getMIDName(mid), length,
80  ' '.join("%02X" % ord(v) for v in data))
81 
82  def waitfor(self, size=1):
83  """Get a given amount of data."""
84  buf = bytearray()
85  for _ in range(100):
86  buf.extend(self.device.read(size-len(buf)))
87  if len(buf) == size:
88  return buf
89  if self.verbose:
90  print "waiting for %d bytes, got %d so far: [%s]" % \
91  (size, len(buf), ' '.join('%02X' % v for v in buf))
92  raise MTTimeoutException("waiting for message")
93 
94  def read_data_msg(self, buf=bytearray()):
95  """Low-level MTData receiving function.
96  Take advantage of known message length.
97  """
98  start = time.time()
99  if self.length <= 254:
100  totlength = 5 + self.length
101  else:
102  totlength = 7 + self.length
103  while (time.time()-start) < self.timeout:
104  buf.extend(self.waitfor(totlength - len(buf)))
105  preamble_ind = buf.find(self.header)
106  if preamble_ind == -1: # not found
107  # discard unexploitable data
108  if self.verbose:
109  sys.stderr.write("MT: discarding (no preamble).\n")
110  del buf[:-3]
111  continue
112  elif preamble_ind: # found but not at start
113  # discard leading bytes
114  if self.verbose:
115  sys.stderr.write("MT: discarding (before preamble).\n")
116  del buf[:preamble_ind]
117  # complete message for checksum
118  buf.extend(self.waitfor(totlength-len(buf)))
119  if 0xFF & sum(buf[1:]):
120  if self.verbose:
121  sys.stderr.write("MT: invalid checksum; discarding data "
122  "and waiting for next message.\n")
123  del buf[:buf.find(self.header)-2]
124  continue
125  data = str(buf[-self.length-1:-1])
126  del buf[:]
127  return data
128  else:
129  raise MTException("could not find MTData message.")
130 
131  def read_msg(self):
132  """Low-level message receiving function."""
133  start = time.time()
134  while (time.time()-start) < self.timeout:
135  # first part of preamble
136  if ord(self.waitfor()) != 0xFA:
137  continue
138  # second part of preamble
139  if ord(self.waitfor()) != 0xFF: # we assume no timeout anymore
140  continue
141  # read message id and length of message
142  mid, length = struct.unpack('!BB', self.waitfor(2))
143  if length == 255: # extended length
144  length, = struct.unpack('!H', self.waitfor(2))
145  # read contents and checksum
146  buf = self.waitfor(length+1)
147  checksum = buf[-1]
148  data = struct.unpack('!%dB' % length, buf[:-1])
149  # check message integrity
150  if 0xFF & sum(data, 0xFF+mid+length+checksum):
151  if self.verbose:
152  sys.stderr.write("invalid checksum; discarding data and "
153  "waiting for next message.\n")
154  continue
155  if self.verbose:
156  print "MT: Got message id 0x%02X (%s) with %d data bytes: "\
157  "[%s]" % (mid, getMIDName(mid), length,
158  ' '.join("%02X" % v for v in data))
159  if mid == MID.Error:
160  raise MTErrorMessage(data[0])
161  return (mid, buf[:-1])
162  else:
163  raise MTException("could not find message.")
164 
165  def write_ack(self, mid, data=b'', n_resend=30, n_read=25):
166  """Send a message and read confirmation."""
167  for _ in range(n_resend):
168  self.write_msg(mid, data)
169  for _ in range(n_read):
170  mid_ack, data_ack = self.read_msg()
171  if mid_ack == (mid+1):
172  break
173  elif self.verbose:
174  print "ack (0x%02X) expected, got 0x%02X instead" % \
175  (mid+1, mid_ack)
176  else: # inner look not broken
177  continue # retry (send+wait)
178  break # still no luck
179  else:
180  n_retries = n_resend*n_read
181  raise MTException("Ack (0x%02X) expected, MID 0x%02X received "
182  "instead (after %d retries)." % (mid+1, mid_ack,
183  n_retries))
184  return data_ack
185 
187  """Switch device to config state if necessary."""
188  if self.state != DeviceState.Config:
189  self.GoToConfig()
190 
192  """Switch device to measurement state if necessary."""
193  if self.state != DeviceState.Measurement:
194  self.GoToMeasurement()
195 
196 
199  def Reset(self, go_to_config=False):
200  """Reset MT device.
201 
202  If go_to_config then send WakeUpAck in order to leave the device in
203  config mode.
204  """
205  self.write_ack(MID.Reset)
206  if go_to_config:
207  time.sleep(0.01)
208  mid, _ = self.read_msg()
209  if mid == MID.WakeUp:
210  self.write_msg(MID.WakeUpAck)
211  self.state = DeviceState.Config
212  else:
213  self.state = DeviceState.Measurement
214 
215  def GoToConfig(self):
216  """Place MT device in configuration mode."""
217  self.write_ack(MID.GoToConfig)
218  self.state = DeviceState.Config
219 
220  def GoToMeasurement(self):
221  """Place MT device in measurement mode."""
222  self._ensure_config_state()
223  self.write_ack(MID.GoToMeasurement)
224  self.state = DeviceState.Measurement
225 
226  def GetDeviceID(self):
227  """Get the device identifier."""
228  self._ensure_config_state()
229  data = self.write_ack(MID.ReqDID)
230  deviceID, = struct.unpack('!I', data)
231  return deviceID
232 
233  def GetProductCode(self):
234  """Get the product code."""
235  self._ensure_config_state()
236  data = self.write_ack(MID.ReqProductCode)
237  return str(data).strip()
238 
240  """Get the hardware version."""
241  self._ensure_config_state()
242  data = self.write_ack(MID.ReqHardwareVersion)
243  major, minor = struct.unpack('!BB', data)
244  return (major, minor)
245 
246  def GetFirmwareRev(self):
247  """Get the firmware version."""
248  self._ensure_config_state()
249  data = self.write_ack(MID.ReqFWRev)
250  if len(data) == 3:
251  major, minor, revision = struct.unpack('!BBB', data)
252  return (major, minor, revision)
253  else:
254  # TODO check buildnr/svnrev not sure unsigned
255  major, minor, rev, buildnr, svnrev = struct.unpack('!BBBII', data)
256  return (major, minor, rev, buildnr, svnrev)
257 
258  def RunSelfTest(self):
259  """Run the built-in self test."""
260  self._ensure_config_state()
261  data = self.write_ack(MID.RunSelfTest)
262  bit_names = ['accX', 'accY', 'accZ', 'gyrX', 'gyrY', 'gyrZ',
263  'magX', 'magY', 'magZ']
264  self_test_results = []
265  for i, name in enumerate(bit_names):
266  self_test_results.append((name, (data >> i) & 1))
267  return self_test_results
268 
269  def GetBaudrate(self):
270  """Get the current baudrate id of the device."""
271  self._ensure_config_state()
272  data = self.write_ack(MID.SetBaudrate)
273  return data[0]
274 
275  def SetBaudrate(self, brid):
276  """Set the baudrate of the device using the baudrate id."""
277  self._ensure_config_state()
278  self.write_ack(MID.SetBaudrate, (brid,))
279 
280  def GetErrorMode(self):
281  """Get the current error mode of the device."""
282  self._ensure_config_state()
283  data = self.write_ack(MID.SetErrorMode)
284  error_mode, = struct.unpack('!H', data)
285  return error_mode
286 
287  def SetErrorMode(self, error_mode):
288  """Set the error mode of the device.
289 
290  The error mode defines the way the device deals with errors (expect
291  message errors):
292  0x0000: ignore any errors except message handling errors,
293  0x0001: in case of missing sampling instance: increase sample
294  counter and do not send error message,
295  0x0002: in case of missing sampling instance: increase sample
296  counter and send error message,
297  0x0003: in case of non-message handling error, an error message is
298  sent and the device will go into Config State.
299  """
300  self._ensure_config_state()
301  data = struct.pack('!H', error_mode)
302  self.write_ack(MID.SetErrorMode, data)
303 
304  def GetOptionFlags(self):
305  """Get the option flags (MTi-1 series)."""
306  self._ensure_config_state()
307  data = self.write_ack(MID.SetOptionFlags)
308  flags, = struct.unpack('!I', data)
309  return flags
310 
311  def SetOptionFlags(self, set_flags, clear_flags):
312  """Set the option flags (MTi-1 series)."""
313  self._ensure_config_state()
314  data = struct.pack('!II', set_flags, clear_flags)
315  self.write_ack(MID.SetOptionFlags, data)
316 
317  def GetLocationID(self):
318  """Get the location ID of the device."""
319  self._ensure_config_state()
320  data = self.write_ack(MID.SetLocationID)
321  location_id, = struct.unpack('!H', data)
322  return location_id
323 
324  def SetLocationID(self, location_id):
325  """Set the location ID of the device (arbitrary)."""
326  self._ensure_config_state()
327  data = struct.pack('!H', location_id)
328  self.write_ack(MID.SetLocationID, data)
329 
331  """Restore MT device configuration to factory defaults (soft version).
332  """
333  self._ensure_config_state()
334  self.write_ack(MID.RestoreFactoryDef)
335 
336  def GetTransmitDelay(self):
337  """Get the transmission delay (only RS485)."""
338  self._ensure_config_state()
339  data = self.write_ack(MID.SetTransmitDelay)
340  transmit_delay, = struct.unpack('!H', data)
341  return transmit_delay
342 
343  def SetTransmitDelay(self, transmit_delay):
344  """Set the transmission delay (only RS485)."""
345  self._ensure_config_state()
346  data = struct.pack('!H', transmit_delay)
347  self.write_ack(MID.SetTransmitDelay, data)
348 
349  def GetSyncSettings(self):
350  """Get the synchronisation settings."""
351  self._ensure_config_state()
352  data = self.write_ack(MID.SetSyncSettings)
353  sync_settings = [struct.unpack('!BBBBHHHH', data[o:o+12])
354  for o in range(0, len(data), 12)]
355  return sync_settings
356 
357  def SetSyncSettings(self, sync_settings):
358  """Set the synchronisation settings (mark IV)"""
359  self._ensure_config_state()
360  data = b''.join(struct.pack('!BBBBHHHH', *sync_setting)
361  for sync_setting in sync_settings)
362  self.write_ack(MID.SetSyncSettings, data)
363 
364  def GetConfiguration(self):
365  """Ask for the current configuration of the MT device."""
366  self._ensure_config_state()
367  config = self.write_ack(MID.ReqConfiguration)
368  try:
369  masterID, period, skipfactor, _, _, _, date, time, num, deviceID,\
370  length, mode, settings =\
371  struct.unpack('!IHHHHI8s8s32x32xHIHHI8x', config)
372  except struct.error:
373  raise MTException("could not parse configuration.")
374  self.mode = mode
375  self.settings = settings
376  self.length = length
377  if self.length <= 254:
378  self.header = b'\xFA\xFF\x32' + struct.pack('!B', self.length)
379  else:
380  self.header = b'\xFA\xFF\x32\xFF' + struct.pack('!H', self.length)
381  conf = {'output-mode': mode,
382  'output-settings': settings,
383  'length': length,
384  'period': period,
385  'skipfactor': skipfactor,
386  'Master device ID': masterID,
387  'date': date,
388  'time': time,
389  'number of devices': num,
390  'device ID': deviceID}
391  return conf
392 
394  """Get the output configuration of the device (mark IV)."""
395  self._ensure_config_state()
396  data = self.write_ack(MID.SetOutputConfiguration)
397  output_configuration = [struct.unpack('!HH', data[o:o+4])
398  for o in range(0, len(data), 4)]
399  return output_configuration
400 
401  def SetOutputConfiguration(self, output_configuration):
402  """Set the output configuration of the device (mark IV)."""
403  self._ensure_config_state()
404  data = b''.join(struct.pack('!HH', *output)
405  for output in output_configuration)
406  self.write_ack(MID.SetOutputConfiguration, data)
407 
409  """Get the NMEA data output configuration."""
410  self._ensure_config_state()
411  data = self.write_ack(MID.SetStringOutputType)
412  string_output_type, = struct.unpack('!H', data)
413  return string_output_type
414 
415  def SetStringOutputType(self, string_output_type):
416  """Set the configuration of the NMEA data output."""
417  self._ensure_config_state()
418  data = struct.pack('!H', string_output_type)
419  self.write_ack(MID.SetStringOutputType, data)
420 
421  def GetPeriod(self):
422  """Get the sampling period."""
423  self._ensure_config_state()
424  data = self.write_ack(MID.SetPeriod)
425  period, = struct.unpack('!H', data)
426  return period
427 
428  def SetPeriod(self, period):
429  """Set the sampling period."""
430  self._ensure_config_state()
431  data = struct.pack('!H', period)
432  self.write_ack(MID.SetPeriod, data)
433 
434  def GetAlignmentRotation(self, parameter):
435  """Get the object alignment.
436 
437  parameter indicates the desired alignment quaternion:
438  0 for sensor alignment (RotSensor),
439  1 for local alignment (RotLocal).
440  """
441  self._ensure_config_state()
442  data = struct.pack('!B', parameter)
443  data = self.write_ack(MID.SetAlignmentRotation, data)
444  if len(data) == 16: # fix for older firmwares
445  q0, q1, q2, q3 = struct.unpack('!ffff', data)
446  return parameter, (q0, q1, q2, q3)
447  elif len(data) == 17:
448  param, q0, q1, q2, q3 = struct.unpack('!Bffff', data)
449  return param, (q0, q1, q2, q3)
450  else:
451  raise MTException('Could not parse ReqAlignmentRotationAck message:'
452  ' wrong size of message (%d instead of either 16 '
453  'or 17).' % len(data))
454 
455  def SetAlignmentRotation(self, parameter, quaternion):
456  """Set the object alignment.
457 
458  parameter indicates the desired alignment quaternion:
459  0 for sensor alignment (RotSensor),
460  1 for local alignment (RotLocal).
461  """
462  self._ensure_config_state()
463  data = struct.pack('!Bffff', parameter, *quaternion)
464  self.write_ack(MID.SetAlignmentRotation, data)
465 
466  def GetOutputMode(self):
467  """Get current output mode."""
468  self._ensure_config_state()
469  data = self.write_ack(MID.SetOutputMode)
470  self.mode, = struct.unpack('!H', data)
471  return self.mode
472 
473  def SetOutputMode(self, mode):
474  """Select which information to output."""
475  self._ensure_config_state()
476  data = struct.pack('!H', mode)
477  self.write_ack(MID.SetOutputMode, data)
478  self.mode = mode
479 
480  def GetExtOutputMode(self):
481  """Get current extended output mode (for alternative UART)."""
482  self._ensure_config_state()
483  data = self.write_ack(MID.SetExtOutputMode)
484  ext_mode, = struct.unpack('!H', data)
485  return ext_mode
486 
487  def SetExtOutputMode(self, ext_mode):
488  """Set extended output mode (for alternative UART)."""
489  self._ensure_config_state()
490  data = struct.pack('!H', ext_mode)
491  self.write_ack(MID.SetExtOutputMode, data)
492 
493  def GetOutputSettings(self):
494  """Get current output mode."""
495  self._ensure_config_state()
496  data = self.write_ack(MID.SetOutputSettings)
497  self.settings, = struct.unpack('!I', data)
498  return self.settings
499 
500  def SetOutputSettings(self, settings):
501  """Select how to output the information."""
502  self._ensure_config_state()
503  data = struct.pack('!I', settings)
504  self.write_ack(MID.SetOutputSettings, data)
505  self.settings = settings
506 
507  def SetOutputSkipFactor(self, skipfactor): # deprecated
508  """Set the output skip factor."""
509  self._ensure_config_state()
510  data = struct.pack('!H', skipfactor)
511  self.write_ack(DeprecatedMID.SetOutputSkipFactor, data)
512 
513  def ReqDataLength(self): # deprecated
514  """Get data length for mark III devices."""
515  self._ensure_config_state()
516  try:
517  data = self.write_ack(DeprecatedMID.ReqDataLength)
518  except MTErrorMessage as e:
519  if e.code == 0x04:
520  sys.stderr.write("ReqDataLength message is deprecated and not "
521  "recognised by your device.")
522  return
523  raise e
524  self.length, = struct.unpack('!H', data)
525  if self.length <= 254:
526  self.header = b'\xFA\xFF\x32' + struct.pack('!B', self.length)
527  else:
528  self.header = b'\xFA\xFF\x32\xFF' + struct.pack('!H', self.length)
529  return self.length
530 
531  def GetLatLonAlt(self):
532  """Get the stored position of the device.
533  It is used internally for local magnetic declination and local gravity.
534  """
535  self._ensure_config_state()
536  data = self.write_ack(MID.SetLatLonAlt)
537  if len(data) == 24:
538  lat, lon, alt = struct.unpack('!ddd', data)
539  elif len(data) == 12:
540  lat, lon, alt = struct.unpack('!fff', data)
541  else:
542  raise MTException('Could not parse ReqLatLonAltAck message: wrong'
543  'size of message.')
544  return (lat, lon, alt)
545 
546  def SetLatLonAlt(self, lat, lon, alt):
547  """Set the position of the device.
548  It is used internally for local magnetic declination and local gravity.
549  """
550  self._ensure_config_state()
551  data = struct.pack('!ddd', lat, lon, alt)
552  self.write_ack(MID.SetLatLonAlt, data)
553 
555  """Get the available XKF scenarios on the device."""
556  self._ensure_config_state()
557  data = self.write_ack(MID.ReqAvailableScenarios)
558  scenarios = []
559  try:
560  for i in range(len(data)/22):
561  scenario_type, version, label =\
562  struct.unpack('!BB20s', data[22*i:22*(i+1)])
563  scenarios.append((scenario_type, version, label.strip()))
564  # available XKF scenarios
565  self.scenarios = scenarios
566  except struct.error:
567  raise MTException("could not parse the available XKF scenarios.")
568  return scenarios
569 
571  """Get the ID of the currently used XKF scenario."""
572  self._ensure_config_state()
573  data = self.write_ack(MID.SetCurrentScenario)
574  _, self.scenario_id = struct.unpack('!BB', data) # version, id
575  return self.scenario_id
576 
577  def SetCurrentScenario(self, scenario_id):
578  """Set the XKF scenario to use."""
579  self._ensure_config_state()
580  data = struct.pack('!BB', 0, scenario_id) # version, id
581  self.write_ack(MID.SetCurrentScenario, data)
582 
583  # New names in mk5
584  GetAvailableFilterProfiles = GetAvailableScenarios
585  GetFilterProfile = GetCurrentScenario
586  SetFilterProfile = SetCurrentScenario
587 
588  def GetGnssPlatform(self):
589  """Get the current GNSS navigation filter settings."""
590  self._ensure_config_state()
591  data = self.write_ack(MID.SetGnssPlatform)
592  platform, = struct.unpack('!H', data)
593  return platform
594 
595  def SetGnssPlatform(self, platform):
596  """Set the GNSS navigation filter settings."""
597  self._ensure_config_state()
598  data = struct.pack('!H', platform)
599  self.write_ack(MID.SetGnssPlatform, data)
600 
601  def ResetOrientation(self, code):
602  """Reset the orientation.
603 
604  Code can take several values:
605  0x0000: store current settings (only in config mode),
606  0x0001: heading reset (NOT supported by MTi-G),
607  0x0003: object reset.
608  """
609  data = struct.pack('!H', code)
610  self.write_ack(MID.ResetOrientation, data)
611 
612  def SetNoRotation(self, duration):
613  """Initiate the "no rotation" procedure to estimate gyro biases."""
615  data = struct.pack('!H', duration)
616  self.write_ack(MID.SetNoRotation, data)
617 
618  def GetUTCTime(self):
619  """Get UTC time from device."""
620  self._ensure_config_state()
621  data = self.write_ack(MID.SetUTCTime)
622  ns, year, month, day, hour, minute, second, flag = \
623  struct.unpack('!IHBBBBBB', data)
624  return (ns, year, month, day, hour, minute, second, flag)
625 
626  def SetUTCTime(self, ns, year, month, day, hour, minute, second, flag):
627  """Set UTC time on the device."""
628  self._ensure_config_state()
629  data = struct.pack('!IHBBBBBB', ns, year, month, day, hour, minute,
630  second, flag) # no clue what setting flag can mean
631  self.write_ack(MID.SetUTCTime, data)
632 
633  def AdjustUTCTime(self, ticks):
634  """Adjust UTC Time of device using correction ticks (0.1 ms)."""
635  self._ensure_config_state()
636  data = struct.pack('!i', ticks)
637  self.write(MID.AdjustUTCTime, data) # no ack mentioned in the doc
638 
639  def IccCommand(self, command):
640  """Command of In-run Compass Calibration (ICC)."""
641  if command not in (0, 1, 2, 3):
642  raise MTException("unknown ICC command 0x%02X" % command)
643  cmd_data = struct.pack('!B', command)
644  res_data = self.write_ack(MID.IccCommand, cmd_data)
645  cmd_ack = struct.unpack('!B', res_data[:1])
646  payload = res_data[1:]
647  if cmd_ack != command:
648  raise MTException("expected ack of command 0x%02X; got 0x%02X "
649  "instead" % (command, cmd_ack))
650  if cmd_ack == 0:
651  return
652  elif cmd_ack == 1:
653  ddt_value, dimension, status = struct.unpack('!fBB', payload)
654  return ddt_value, dimension, status
655  elif cmd_ack == 2:
656  return
657  elif cmd_ack == 3:
658  state = struct.unpack('!B', payload)
659  return state
660 
661 
664  def configure_legacy(self, mode, settings, period=None, skipfactor=None):
665  """Configure the mode and settings of the MT device in legacy mode."""
666  try:
667  # switch mark IV devices to legacy mode
668  self.SetOutputConfiguration([(0x0000, 0)])
669  except MTErrorMessage as e:
670  if e.code == 0x04:
671  # mark III device
672  pass
673  else:
674  raise
675  except MTException as e:
676  if self.verbose:
677  print "no ack received while switching from MTData2 to MTData."
678  pass # no ack???
679  self.SetOutputMode(mode)
680  self.SetOutputSettings(settings)
681  if period is not None:
682  self.SetPeriod(period)
683  if skipfactor is not None:
684  self.SetOutputSkipFactor(skipfactor)
685  self.GetConfiguration()
686 
688  """Read configuration from device in legacy mode."""
689  self.GetConfiguration()
690  return self.mode, self.settings, self.length
691 
692  def read_measurement(self, mode=None, settings=None):
694  # getting data
695  # data = self.read_data_msg()
696  mid, data = self.read_msg()
697  if mid == MID.MTData:
698  return self.parse_MTData(data, mode, settings)
699  elif mid == MID.MTData2:
700  return self.parse_MTData2(data)
701  else:
702  raise MTException("unknown data message: mid=0x%02X (%s)." %
703  (mid, getMIDName(mid)))
704 
705  def parse_MTData2(self, data):
706  # Functions to parse each type of packet
707  def parse_temperature(data_id, content, ffmt):
708  o = {}
709  if (data_id & 0x00F0) == 0x10: # Temperature
710  o['Temp'], = struct.unpack('!'+ffmt, content)
711  else:
712  raise MTException("unknown packet: 0x%04X." % data_id)
713  return o
714 
715  def parse_timestamp(data_id, content, ffmt):
716  o = {}
717  if (data_id & 0x00F0) == 0x10: # UTC Time
718  o['ns'], o['Year'], o['Month'], o['Day'], o['Hour'],\
719  o['Minute'], o['Second'], o['Flags'] =\
720  struct.unpack('!LHBBBBBB', content)
721  elif (data_id & 0x00F0) == 0x20: # Packet Counter
722  o['PacketCounter'], = struct.unpack('!H', content)
723  elif (data_id & 0x00F0) == 0x30: # Integer Time of Week
724  o['TimeOfWeek'], = struct.unpack('!L', content)
725  elif (data_id & 0x00F0) == 0x40: # GPS Age # deprecated
726  o['gpsAge'], = struct.unpack('!B', content)
727  elif (data_id & 0x00F0) == 0x50: # Pressure Age # deprecated
728  o['pressureAge'], = struct.unpack('!B', content)
729  elif (data_id & 0x00F0) == 0x60: # Sample Time Fine
730  o['SampleTimeFine'], = struct.unpack('!L', content)
731  elif (data_id & 0x00F0) == 0x70: # Sample Time Coarse
732  o['SampleTimeCoarse'], = struct.unpack('!L', content)
733  elif (data_id & 0x00F0) == 0x80: # Frame Range
734  o['startFrame'], o['endFrame'] = struct.unpack('!HH', content)
735  else:
736  raise MTException("unknown packet: 0x%04X." % data_id)
737  return o
738 
739  def parse_orientation_data(data_id, content, ffmt):
740  o = {}
741  if (data_id & 0x000C) == 0x00: # ENU
742  o['frame'] = 'ENU'
743  elif (data_id & 0x000C) == 0x04: # NED
744  o['frame'] = 'NED'
745  elif (data_id & 0x000C) == 0x08: # NWU
746  o['frame'] = 'NWU'
747  if (data_id & 0x00F0) == 0x10: # Quaternion
748  o['Q0'], o['Q1'], o['Q2'], o['Q3'] = struct.unpack('!'+4*ffmt,
749  content)
750  elif (data_id & 0x00F0) == 0x20: # Rotation Matrix
751  o['a'], o['b'], o['c'], o['d'], o['e'], o['f'], o['g'],\
752  o['h'], o['i'] = struct.unpack('!'+9*ffmt, content)
753  elif (data_id & 0x00F0) == 0x30: # Euler Angles
754  o['Roll'], o['Pitch'], o['Yaw'] = struct.unpack('!'+3*ffmt,
755  content)
756  else:
757  raise MTException("unknown packet: 0x%04X." % data_id)
758  return o
759 
760  def parse_pressure(data_id, content, ffmt):
761  o = {}
762  if (data_id & 0x00F0) == 0x10: # Baro pressure
763  o['Pressure'], = struct.unpack('!L', content)
764  else:
765  raise MTException("unknown packet: 0x%04X." % data_id)
766  return o
767 
768  def parse_acceleration(data_id, content, ffmt):
769  o = {}
770  if (data_id & 0x000C) == 0x00: # ENU
771  o['frame'] = 'ENU'
772  elif (data_id & 0x000C) == 0x04: # NED
773  o['frame'] = 'NED'
774  elif (data_id & 0x000C) == 0x08: # NWU
775  o['frame'] = 'NWU'
776  if (data_id & 0x00F0) == 0x10: # Delta V
777  o['Delta v.x'], o['Delta v.y'], o['Delta v.z'] = \
778  struct.unpack('!'+3*ffmt, content)
779  elif (data_id & 0x00F0) == 0x20: # Acceleration
780  o['accX'], o['accY'], o['accZ'] = \
781  struct.unpack('!'+3*ffmt, content)
782  elif (data_id & 0x00F0) == 0x30: # Free Acceleration
783  o['freeAccX'], o['freeAccY'], o['freeAccZ'] = \
784  struct.unpack('!'+3*ffmt, content)
785  elif (data_id & 0x00F0) == 0x40: # AccelerationHR
786  o['accX'], o['accY'], o['accZ'] = \
787  struct.unpack('!'+3*ffmt, content)
788  else:
789  raise MTException("unknown packet: 0x%04X." % data_id)
790  return o
791 
792  def parse_position(data_id, content, ffmt):
793  o = {}
794  if (data_id & 0x000C) == 0x00: # ENU
795  o['frame'] = 'ENU'
796  elif (data_id & 0x000C) == 0x04: # NED
797  o['frame'] = 'NED'
798  elif (data_id & 0x000C) == 0x08: # NWU
799  o['frame'] = 'NWU'
800  if (data_id & 0x00F0) == 0x10: # Altitude MSL # deprecated
801  o['altMsl'], = struct.unpack('!'+ffmt, content)
802  elif (data_id & 0x00F0) == 0x20: # Altitude Ellipsoid
803  o['altEllipsoid'], = struct.unpack('!'+ffmt, content)
804  elif (data_id & 0x00F0) == 0x30: # Position ECEF
805  o['ecefX'], o['ecefY'], o['ecefZ'] = \
806  struct.unpack('!'+3*ffmt, content)
807  elif (data_id & 0x00F0) == 0x40: # LatLon
808  o['lat'], o['lon'] = struct.unpack('!'+2*ffmt, content)
809  else:
810  raise MTException("unknown packet: 0x%04X." % data_id)
811  return o
812 
813  def parse_GNSS(data_id, content, ffmt):
814  o = {}
815  if (data_id & 0x00F0) == 0x10: # GNSS PVT data
816  o['itow'], o['year'], o['month'], o['day'], o['hour'],\
817  o['min'], o['sec'], o['valid'], o['tAcc'], o['nano'],\
818  o['fixtype'], o['flags'], o['numSV'], o['lon'], o['lat'],\
819  o['height'], o['hMSL'], o['hAcc'], o['vAcc'], o['velN'],\
820  o['velE'], o['velD'], o['gSpeed'], o['headMot'],\
821  o['sAcc'], o['headAcc'], o['headVeh'], o['gdop'],\
822  o['pdop'], o['tdop'], o['vdop'], o['hdop'], o['ndop'],\
823  o['edop'] =\
824  struct.unpack('!IHBBBBBBIiBBBxiiiiIIiiiiiIIiHHHHHHH',
825  content)
826  # scaling correction
827  o['lon'] *= 1e-7
828  o['lat'] *= 1e-7
829  o['headMot'] *= 1e-5
830  o['headVeh'] *= 1e-5
831  o['gdop'] *= 0.01
832  o['pdop'] *= 0.01
833  o['tdop'] *= 0.01
834  o['vdop'] *= 0.01
835  o['hdop'] *= 0.01
836  o['ndop'] *= 0.01
837  o['edop'] *= 0.01
838  elif (data_id & 0x00F0) == 0x20: # GNSS satellites info
839  o['iTOW'], o['numSvs'] = struct.unpack('!LBxxx', content[:8])
840  svs = []
841  ch = {}
842  for i in range(o['numSvs']):
843  ch['gnssId'], ch['svId'], ch['cno'], ch['flags'] = \
844  struct.unpack('!BBBB', content[8+4*i:12+4*i])
845  svs.append(ch)
846  o['svs'] = svs
847  else:
848  raise MTException("unknown packet: 0x%04X." % data_id)
849  return o
850 
851  def parse_angular_velocity(data_id, content, ffmt):
852  o = {}
853  if (data_id & 0x000C) == 0x00: # ENU
854  o['frame'] = 'ENU'
855  elif (data_id & 0x000C) == 0x04: # NED
856  o['frame'] = 'NED'
857  elif (data_id & 0x000C) == 0x08: # NWU
858  o['frame'] = 'NWU'
859  if (data_id & 0x00F0) == 0x20: # Rate of Turn
860  o['gyrX'], o['gyrY'], o['gyrZ'] = \
861  struct.unpack('!'+3*ffmt, content)
862  elif (data_id & 0x00F0) == 0x30: # Delta Q
863  o['Delta q0'], o['Delta q1'], o['Delta q2'], o['Delta q3'] = \
864  struct.unpack('!'+4*ffmt, content)
865  elif (data_id & 0x00F0) == 0x40: # RateOfTurnHR
866  o['gyrX'], o['gyrY'], o['gyrZ'] = \
867  struct.unpack('!'+3*ffmt, content)
868  else:
869  raise MTException("unknown packet: 0x%04X." % data_id)
870  return o
871 
872  def parse_GPS(data_id, content, ffmt):
873  o = {}
874  if (data_id & 0x00F0) == 0x30: # DOP
875  o['iTOW'], g, p, t, v, h, n, e = \
876  struct.unpack('!LHHHHHHH', content)
877  o['gDOP'], o['pDOP'], o['tDOP'], o['vDOP'], o['hDOP'], \
878  o['nDOP'], o['eDOP'] = 0.01*g, 0.01*p, 0.01*t, \
879  0.01*v, 0.01*h, 0.01*n, 0.01*e
880  elif (data_id & 0x00F0) == 0x40: # SOL
881  o['iTOW'], o['fTOW'], o['Week'], o['gpsFix'], o['Flags'], \
882  o['ecefX'], o['ecefY'], o['ecefZ'], o['pAcc'], \
883  o['ecefVX'], o['ecefVY'], o['ecefVZ'], o['sAcc'], \
884  o['pDOP'], o['numSV'] = \
885  struct.unpack('!LlhBBlllLlllLHxBx', content)
886  # scaling correction
887  o['pDOP'] *= 0.01
888  elif (data_id & 0x00F0) == 0x80: # Time UTC
889  o['iTOW'], o['tAcc'], o['nano'], o['year'], o['month'], \
890  o['day'], o['hour'], o['min'], o['sec'], o['valid'] = \
891  struct.unpack('!LLlHBBBBBB', content)
892  elif (data_id & 0x00F0) == 0xA0: # SV Info
893  o['iTOW'], o['numCh'] = struct.unpack('!LBxxx', content[:8])
894  channels = []
895  ch = {}
896  for i in range(o['numCh']):
897  ch['chn'], ch['svid'], ch['flags'], ch['quality'], \
898  ch['cno'], ch['elev'], ch['azim'], ch['prRes'] = \
899  struct.unpack('!BBBBBbhl', content[8+12*i:20+12*i])
900  channels.append(ch)
901  o['channels'] = channels
902  else:
903  raise MTException("unknown packet: 0x%04X." % data_id)
904  return o
905 
906  def parse_SCR(data_id, content, ffmt):
907  o = {}
908  if (data_id & 0x00F0) == 0x10: # ACC+GYR+MAG+Temperature
909  o['accX'], o['accY'], o['accZ'], o['gyrX'], o['gyrY'], \
910  o['gyrZ'], o['magX'], o['magY'], o['magZ'], o['Temp'] = \
911  struct.unpack("!9Hh", content)
912  elif (data_id & 0x00F0) == 0x20: # Gyro Temperature
913  o['tempGyrX'], o['tempGyrY'], o['tempGyrZ'] = \
914  struct.unpack("!hhh", content)
915  else:
916  raise MTException("unknown packet: 0x%04X." % data_id)
917  return o
918 
919  def parse_analog_in(data_id, content, ffmt): # deprecated
920  o = {}
921  if (data_id & 0x00F0) == 0x10: # Analog In 1
922  o['analogIn1'], = struct.unpack("!H", content)
923  elif (data_id & 0x00F0) == 0x20: # Analog In 2
924  o['analogIn2'], = struct.unpack("!H", content)
925  else:
926  raise MTException("unknown packet: 0x%04X." % data_id)
927  return o
928 
929  def parse_magnetic(data_id, content, ffmt):
930  o = {}
931  if (data_id & 0x000C) == 0x00: # ENU
932  o['frame'] = 'ENU'
933  elif (data_id & 0x000C) == 0x04: # NED
934  o['frame'] = 'NED'
935  elif (data_id & 0x000C) == 0x08: # NWU
936  o['frame'] = 'NWU'
937  if (data_id & 0x00F0) == 0x20: # Magnetic Field
938  o['magX'], o['magY'], o['magZ'] = \
939  struct.unpack("!3"+ffmt, content)
940  else:
941  raise MTException("unknown packet: 0x%04X." % data_id)
942  return o
943 
944  def parse_velocity(data_id, content, ffmt):
945  o = {}
946  if (data_id & 0x000C) == 0x00: # ENU
947  o['frame'] = 'ENU'
948  elif (data_id & 0x000C) == 0x04: # NED
949  o['frame'] = 'NED'
950  elif (data_id & 0x000C) == 0x08: # NWU
951  o['frame'] = 'NWU'
952  if (data_id & 0x00F0) == 0x10: # Velocity XYZ
953  o['velX'], o['velY'], o['velZ'] = \
954  struct.unpack("!3"+ffmt, content)
955  else:
956  raise MTException("unknown packet: 0x%04X." % data_id)
957  return o
958 
959  def parse_status(data_id, content, ffmt):
960  o = {}
961  if (data_id & 0x00F0) == 0x10: # Status Byte
962  o['StatusByte'], = struct.unpack("!B", content)
963  elif (data_id & 0x00F0) == 0x20: # Status Word
964  o['StatusWord'], = struct.unpack("!L", content)
965  elif (data_id & 0x00F0) == 0x40: # RSSI # deprecated
966  o['RSSI'], = struct.unpack("!b", content)
967  else:
968  raise MTException("unknown packet: 0x%04X." % data_id)
969  return o
970 
971  # data object
972  output = {}
973  while data:
974  try:
975  data_id, size = struct.unpack('!HB', data[:3])
976  if (data_id & 0x0003) == 0x3:
977  float_format = 'd'
978  elif (data_id & 0x0003) == 0x0:
979  float_format = 'f'
980  else:
981  raise MTException("fixed point precision not supported.")
982  content = data[3:3+size]
983  data = data[3+size:]
984  group = data_id & 0xF800
985  ffmt = float_format
986  if group == XDIGroup.Temperature:
987  output.setdefault('Temperature', {}).update(
988  parse_temperature(data_id, content, ffmt))
989  elif group == XDIGroup.Timestamp:
990  output.setdefault('Timestamp', {}).update(
991  parse_timestamp(data_id, content, ffmt))
992  elif group == XDIGroup.OrientationData:
993  output.setdefault('Orientation Data', {}).update(
994  parse_orientation_data(data_id, content, ffmt))
995  elif group == XDIGroup.Pressure:
996  output.setdefault('Pressure', {}).update(
997  parse_pressure(data_id, content, ffmt))
998  elif group == XDIGroup.Acceleration:
999  output.setdefault('Acceleration', {}).update(
1000  parse_acceleration(data_id, content, ffmt))
1001  elif group == XDIGroup.Position:
1002  output.setdefault('Position', {}).update(
1003  parse_position(data_id, content, ffmt))
1004  elif group == XDIGroup.GNSS:
1005  output.setdefault('GNSS', {}).update(
1006  parse_GNSS(data_id, content, ffmt))
1007  elif group == XDIGroup.AngularVelocity:
1008  output.setdefault('Angular Velocity', {}).update(
1009  parse_angular_velocity(data_id, content, ffmt))
1010  elif group == XDIGroup.GPS:
1011  output.setdefault('GPS', {}).update(
1012  parse_GPS(data_id, content, ffmt))
1013  elif group == XDIGroup.SensorComponentReadout:
1014  output.setdefault('SCR', {}).update(
1015  parse_SCR(data_id, content, ffmt))
1016  elif group == XDIGroup.AnalogIn: # deprecated
1017  output.setdefault('Analog In', {}).update(
1018  parse_analog_in(data_id, content, ffmt))
1019  elif group == XDIGroup.Magnetic:
1020  output.setdefault('Magnetic', {}).update(
1021  parse_magnetic(data_id, content, ffmt))
1022  elif group == XDIGroup.Velocity:
1023  output.setdefault('Velocity', {}).update(
1024  parse_velocity(data_id, content, ffmt))
1025  elif group == XDIGroup.Status:
1026  output.setdefault('Status', {}).update(
1027  parse_status(data_id, content, ffmt))
1028  else:
1029  raise MTException("unknown XDI group: 0x%04X." % group)
1030  except struct.error:
1031  raise MTException("couldn't parse MTData2 message (data_id: "
1032  "0x%04X, size: %d)." % (data_id, size))
1033  return output
1034 
1035  def parse_MTData(self, data, mode=None, settings=None):
1036  """Read and parse a legacy measurement packet."""
1037  # getting mode
1038  if mode is None:
1039  mode = self.mode
1040  if settings is None:
1041  settings = self.settings
1042  # data object
1043  output = {}
1044  try:
1045  # raw IMU first
1046  if mode & OutputMode.RAW:
1047  o = {}
1048  o['accX'], o['accY'], o['accZ'], o['gyrX'], o['gyrY'], \
1049  o['gyrZ'], o['magX'], o['magY'], o['magZ'], o['temp'] =\
1050  struct.unpack('!10H', data[:20])
1051  data = data[20:]
1052  output['RAW'] = o
1053  # raw GPS second
1054  if mode & OutputMode.RAWGPS:
1055  o = {}
1056  o['Press'], o['bPrs'], o['ITOW'], o['LAT'], o['LON'],\
1057  o['ALT'], o['VEL_N'], o['VEL_E'], o['VEL_D'], o['Hacc'],\
1058  o['Vacc'], o['Sacc'], o['bGPS'] =\
1059  struct.unpack('!HBI6i3IB', data[:44])
1060  data = data[44:]
1061  output['RAWGPS'] = o
1062  # temperature
1063  if mode & OutputMode.Temp:
1064  temp, = struct.unpack('!f', data[:4])
1065  data = data[4:]
1066  output['Temp'] = temp
1067  # calibrated data
1068  if mode & OutputMode.Calib:
1069  o = {}
1070  if (settings & OutputSettings.Coordinates_NED):
1071  o['frame'] = 'NED'
1072  else:
1073  o['frame'] = 'ENU'
1074  if not (settings & OutputSettings.CalibMode_GyrMag):
1075  o['accX'], o['accY'], o['accZ'] = struct.unpack('!3f',
1076  data[:12])
1077  data = data[12:]
1078  if not (settings & OutputSettings.CalibMode_AccMag):
1079  o['gyrX'], o['gyrY'], o['gyrZ'] = struct.unpack('!3f',
1080  data[:12])
1081  data = data[12:]
1082  if not (settings & OutputSettings.CalibMode_AccGyr):
1083  o['magX'], o['magY'], o['magZ'] = struct.unpack('!3f',
1084  data[:12])
1085  data = data[12:]
1086  output['Calib'] = o
1087  # orientation
1088  if mode & OutputMode.Orient:
1089  o = {}
1090  if (settings & OutputSettings.Coordinates_NED):
1091  o['frame'] = 'NED'
1092  else:
1093  o['frame'] = 'ENU'
1094  if settings & OutputSettings.OrientMode_Euler:
1095  o['roll'], o['pitch'], o['yaw'] = struct.unpack('!3f',
1096  data[:12])
1097  data = data[12:]
1098  elif settings & OutputSettings.OrientMode_Matrix:
1099  a, b, c, d, e, f, g, h, i = struct.unpack('!9f',
1100  data[:36])
1101  data = data[36:]
1102  o['matrix'] = ((a, b, c), (d, e, f), (g, h, i))
1103  else: # OutputSettings.OrientMode_Quaternion:
1104  q0, q1, q2, q3 = struct.unpack('!4f', data[:16])
1105  data = data[16:]
1106  o['quaternion'] = (q0, q1, q2, q3)
1107  output['Orient'] = o
1108  # auxiliary
1109  if mode & OutputMode.Auxiliary:
1110  o = {}
1111  if not (settings & OutputSettings.AuxiliaryMode_NoAIN1):
1112  o['Ain_1'], = struct.unpack('!H', data[:2])
1113  data = data[2:]
1114  if not (settings & OutputSettings.AuxiliaryMode_NoAIN2):
1115  o['Ain_2'], = struct.unpack('!H', data[:2])
1116  data = data[2:]
1117  output['Auxiliary'] = o
1118  # position
1119  if mode & OutputMode.Position:
1120  o = {}
1121  o['Lat'], o['Lon'], o['Alt'] = struct.unpack('!3f', data[:12])
1122  data = data[12:]
1123  output['Pos'] = o
1124  # velocity
1125  if mode & OutputMode.Velocity:
1126  o = {}
1127  if (settings & OutputSettings.Coordinates_NED):
1128  o['frame'] = 'NED'
1129  else:
1130  o['frame'] = 'ENU'
1131  o['Vel_X'], o['Vel_Y'], o['Vel_Z'] = struct.unpack('!3f',
1132  data[:12])
1133  data = data[12:]
1134  output['Vel'] = o
1135  # status
1136  if mode & OutputMode.Status:
1137  status, = struct.unpack('!B', data[:1])
1138  data = data[1:]
1139  output['Stat'] = status
1140  # sample counter
1141  if settings & OutputSettings.Timestamp_SampleCnt:
1142  TS, = struct.unpack('!H', data[:2])
1143  data = data[2:]
1144  output['Sample'] = TS
1145  # UTC time
1146  if settings & OutputSettings.Timestamp_UTCTime:
1147  o = {}
1148  o['ns'], o['Year'], o['Month'], o['Day'], o['Hour'],\
1149  o['Minute'], o['Second'], o['Flags'] = struct.unpack(
1150  '!ihbbbbb', data[:12])
1151  data = data[12:]
1152  output['Timestamp'] = o
1153  # TODO at that point data should be empty
1154  except struct.error, e:
1155  raise MTException("could not parse MTData message.")
1156  if data != '':
1157  raise MTException("could not parse MTData message (too long).")
1158  return output
1159 
1160  def ChangeBaudrate(self, baudrate):
1161  """Change the baudrate, reset the device and reopen communication."""
1162  brid = Baudrates.get_BRID(baudrate)
1163  self.SetBaudrate(brid)
1164  self.Reset()
1165  # self.device.flush()
1166  self.device.baudrate = baudrate
1167  # self.device.flush()
1168  time.sleep(0.01)
1169  self.read_msg()
1170  self.write_msg(MID.WakeUpAck)
1171 
1172 
1173 
1176 def find_devices(timeout=0.002, verbose=False, initial_wait=0.1):
1177  mtdev_list = []
1178  for port in glob.glob("/dev/tty*S*"):
1179  if verbose:
1180  print "Trying '%s'" % port
1181  try:
1182  br = find_baudrate(port, timeout, verbose, initial_wait)
1183  if br:
1184  mtdev_list.append((port, br))
1185  except MTException:
1186  pass
1187  return mtdev_list
1188 
1189 
1190 
1193 def find_baudrate(port, timeout=0.002, verbose=False, initial_wait=0.1):
1194  baudrates = (115200, 460800, 921600, 230400, 57600, 38400, 19200, 9600)
1195  for br in baudrates:
1196  if verbose:
1197  print "Trying %d bd:" % br,
1198  sys.stdout.flush()
1199  try:
1200  mt = MTDevice(port, br, timeout=timeout, verbose=verbose,
1201  initial_wait=initial_wait)
1202  except serial.SerialException:
1203  if verbose:
1204  print "fail: unable to open device."
1205  raise MTException("unable to open %s" % port)
1206  try:
1207  mt.GoToConfig()
1208  mt.GoToMeasurement()
1209  if verbose:
1210  print "ok."
1211  return br
1212  except MTException:
1213  if verbose:
1214  print "fail."
1215 
1216 
1217 
1220 def usage():
1221  print """MT device driver.
1222 Usage:
1223  ./mtdevice.py [commands] [opts]
1224 
1225 Commands:
1226  -h, --help
1227  Print this help and quit.
1228  -r, --reset
1229  Reset device to factory defaults.
1230  -a, --change-baudrate=NEW_BAUD
1231  Change baudrate from BAUD (see below) to NEW_BAUD.
1232  -c, --configure=OUTPUT
1233  Configure the device (see OUTPUT description below).
1234  -e, --echo
1235  Print MTData. It is the default if no other command is supplied.
1236  -i, --inspect
1237  Print current MT device configuration.
1238  -x, --xkf-scenario=ID
1239  Change the current XKF scenario.
1240  -l, --legacy-configure
1241  Configure the device in legacy mode (needs MODE and SETTINGS arguments
1242  below).
1243  -v, --verbose
1244  Verbose output.
1245  -y, --synchronization=settings (see below)
1246  Configure the synchronization settings of each sync line (see below).
1247  -u, --utc-time=time (see below)
1248  Set the UTC time buffer of the device.
1249  -g, --gnss-platform=platform
1250  Change the GNSS navigation filter settings (check the documentation).
1251  -o, --option-flags=flags (see below)
1252  Set the option flags.
1253  -j, --icc-command=command (see below)
1254  Send command to the In-run Compass Calibration.
1255 
1256 Generic options:
1257  -d, --device=DEV
1258  Serial interface of the device (default: /dev/ttyUSB0). If 'auto', then
1259  all serial ports are tested at all baudrates and the first
1260  suitable device is used.
1261  -b, --baudrate=BAUD
1262  Baudrate of serial interface (default: 115200). If 0, then all
1263  rates are tried until a suitable one is found.
1264  -t, --timeout=TIMEOUT
1265  Timeout of serial communication in second (default: 0.002).
1266  -w, --initial-wait=WAIT
1267  Initial wait to allow device to be ready in second (default: 0.1).
1268 
1269 Configuration option:
1270  OUTPUT
1271  The format is a sequence of "<group><type><frequency>?<format>?"
1272  separated by commas.
1273  The frequency and format are optional.
1274  The groups and types can be:
1275  t temperature (max frequency: 1 Hz):
1276  tt temperature
1277  i timestamp (max frequency: 2000 Hz):
1278  iu UTC time
1279  ip packet counter
1280  ii Integer Time of the Week (ITOW)
1281  if sample time fine
1282  ic sample time coarse
1283  ir frame range
1284  o orientation data (max frequency: 400 Hz):
1285  oq quaternion
1286  om rotation matrix
1287  oe Euler angles
1288  b pressure (max frequency: 50 Hz):
1289  bp baro pressure
1290  a acceleration (max frequency: 2000 Hz (see documentation)):
1291  ad delta v
1292  aa acceleration
1293  af free acceleration
1294  ah acceleration HR (max frequency 1000 Hz)
1295  p position (max frequency: 400 Hz):
1296  pa altitude ellipsoid
1297  pp position ECEF
1298  pl latitude longitude
1299  n GNSS (max frequency: 4 Hz):
1300  np GNSS PVT data
1301  ns GNSS satellites info
1302  w angular velocity (max frequency: 2000 Hz (see documentation)):
1303  wr rate of turn
1304  wd delta q
1305  wh rate of turn HR (max frequency 1000 Hz)
1306  g GPS (max frequency: 4 Hz):
1307  gd DOP
1308  gs SOL
1309  gu time UTC
1310  gi SV info
1311  r Sensor Component Readout (max frequency: 2000 Hz):
1312  rr ACC, GYR, MAG, temperature
1313  rt Gyro temperatures
1314  m Magnetic (max frequency: 100 Hz):
1315  mf magnetic Field
1316  v Velocity (max frequency: 400 Hz):
1317  vv velocity XYZ
1318  s Status (max frequency: 2000 Hz):
1319  sb status byte
1320  sw status word
1321  Frequency is specified in decimal and is assumed to be the maximum
1322  frequency if it is omitted.
1323  Format is a combination of the precision for real valued numbers and
1324  coordinate system:
1325  precision:
1326  f single precision floating point number (32-bit) (default)
1327  d double precision floating point number (64-bit)
1328  coordinate system:
1329  e East-North-Up (default)
1330  n North-East-Down
1331  w North-West-Up
1332  Examples:
1333  The default configuration for the MTi-1/10/100 IMUs can be
1334  specified either as:
1335  "wd,ad,mf,ip,if,sw"
1336  or
1337  "wd2000fe,ad2000fe,mf100fe,ip2000,if2000,sw2000"
1338  For getting quaternion orientation in float with sample time:
1339  "oq400fw,if2000"
1340  For longitude, latitude, altitude and orientation (on MTi-G-700):
1341  "pl400fe,pa400fe,oq400fe"
1342 
1343 Synchronization settings:
1344  The format follows the xsens protocol documentation. All fields are
1345  required and separated by commas.
1346  Note: The entire synchronization buffer is wiped every time a new one
1347  is set, so it is necessary to specify the settings of multiple
1348  lines at once.
1349  It also possible to clear the synchronization with the argument "clear"
1350 
1351  Function (see manual for details):
1352  3 Trigger indication
1353  4 Interval Transition Measurement
1354  8 SendLatest
1355  9 ClockBiasEstimation
1356  11 StartSampling
1357  Line (manual for details):
1358  0 ClockIn
1359  1 GPSClockIn (only available for 700/710)
1360  2 Input Line (SyncIn)
1361  4 SyncOut
1362  5 ExtTimepulseIn (only available for 700/710)
1363  6 Software (only available for SendLatest with ReqData message)
1364  Polarity:
1365  1 Positive pulse/ Rising edge
1366  2 Negative pulse/ Falling edge
1367  3 Both/ Toggle
1368  Trigger Type:
1369  0 multiple times
1370  1 once
1371  Skip First (unsigned_int):
1372  Number of initial events to skip before taking actions
1373  Skip Factor (unsigned_int):
1374  Number of events to skip before taking action again
1375  Ignored with ReqData.
1376  Pulse Width (unsigned_int):
1377  Ignored for SyncIn.
1378  For SyncOut, the width of the generated pulse in 100 microseconds
1379  unit. Ignored for Toggle pulses.
1380  Delay:
1381  Delay after receiving a sync pulse to taking action,
1382  100 microseconds units, range [0...600000]
1383  Clock Period:
1384  Reference clock period in milliseconds for ClockBiasEstimation
1385  Offset:
1386  Offset from event to pulse generation.
1387  100 microseconds unit, range [-30000...+30000]
1388 
1389  Examples:
1390  For changing the sync setting of the SyncIn line to trigger indication
1391  with rising edge, one time triggering and no skipping and delay. Enter
1392  the settings as:
1393  "3,2,1,1,0,0,0,0"
1394 
1395  Note a number is still in the place for pulse width despite it being
1396  ignored.
1397 
1398  To set multiple lines at once:
1399  ./mtdevice.py -y 3,2,1,0,0,0,0,0 -y 9,0,1,0,0,0,10,0
1400 
1401  To clear the synchronization settings of MTi
1402  ./mtdevice.py -y clear
1403 
1404 UTC time settings:
1405  There are two ways to set the UTCtime for the MTi.
1406  Option #1: set MTi to the current UTC time based on local system time with
1407  the option 'now'
1408  Option #2: set MTi to a specified UTC time
1409  The time fields are set as follows:
1410  year: range [1999,2099]
1411  month: range [1,12]
1412  day: day of the month, range [1,31]
1413  hour: hour of the day, range [0,23]
1414  min: minute of the hour, range [0,59]
1415  sec: second of the minute, range [0,59]
1416  ns: nanosecond of the second, range [0,1000000000]
1417  flag:
1418  1: Valid Time of Week
1419  2: Valid Week Number
1420  4: valid UTC
1421  Note: the flag is ignored for --utc-time as it is set by the device
1422  itself when connected to a GPS
1423 
1424  Examples:
1425  Set UTC time for the device:
1426  ./mtdevice.py -u now
1427  ./mtdevice.py -u 1999,1,1,0,0,0,0,0
1428 
1429 GNSS platform settings:
1430  Only for MTi-G-700/710 with firmware>=1.7.
1431  The following two platform settings are listed in the documentation:
1432  0: Portable
1433  8: Airbone <4g
1434  Check the XSens documentation before changing anything.
1435 
1436 Option flags:
1437  Several flags can be set or cleared.
1438  0x00000001 DisableAutoStore: when set, configuration changes are not saved
1439  in non-volatile memory (only MTi-1 series)
1440  0x00000002 DisableAutoMeasurement: when set, device will stay in Config
1441  Mode upon start up (only MTi-1 series)
1442  0x00000004 EnableBeidou: when set, enable Beidou and disable GLONASS (only
1443  MTi-G-710)
1444  0x00000010 EnableAHS: enable Active Heading Stabilization (overrides
1445  magnetic reference)
1446  0x00000080 EnableInRunCompassCalibration: doc is unclear
1447  The flags provided must be a pair of ored values: the first for flags to be
1448  set the second for the flags to be cleared.
1449  Examples:
1450  Only set DisableAutoStore and DisableAutoMeasurement flags:
1451  ./mtdevice.py -o 0x03,0x00
1452  Disable AHS (clear EnableAHS flag):
1453  ./mtdevice.py -o 0x00,0x10
1454  Set DisableAutoStore and clear DisableAutoMeasurement:
1455  ./mtdevice.py -o 0x02,0x01
1456 
1457 In-run Compass Calibration commands:
1458  The idea of ICC is to record magnetic field data during so-called
1459  representative motion in order to better calibrate the magnetometer and
1460  improve the fusion.
1461  Typical usage would be to issue the start command, then move the device
1462  for some time then issue the stop command. If parameters are acceptable,
1463  these can be stored using the store command.
1464  Commands:
1465  00: Start representative motion
1466  01: Stop representative motion; return ddt, dimension, and status.
1467  02: Store ICC parameters
1468  03: Get representative motion state; return 1 if active
1469  Check the documentation for more details.
1470 
1471 Legacy options:
1472  -m, --output-mode=MODE
1473  Legacy mode of the device to select the information to output.
1474  This is required for 'legacy-configure' command.
1475  MODE can be either the mode value in hexadecimal, decimal or
1476  binary form, or a string composed of the following characters
1477  (in any order):
1478  t temperature, [0x0001]
1479  c calibrated data, [0x0002]
1480  o orientation data, [0x0004]
1481  a auxiliary data, [0x0008]
1482  p position data (requires MTi-G), [0x0010]
1483  v velocity data (requires MTi-G), [0x0020]
1484  s status data, [0x0800]
1485  g raw GPS mode (requires MTi-G), [0x1000]
1486  r raw (incompatible with others except raw GPS), [0x4000]
1487  For example, use "--output-mode=so" to have status and
1488  orientation data.
1489  -s, --output-settings=SETTINGS
1490  Legacy settings of the device. This is required for 'legacy-configure'
1491  command.
1492  SETTINGS can be either the settings value in hexadecimal,
1493  decimal or binary form, or a string composed of the following
1494  characters (in any order):
1495  t sample count (excludes 'n')
1496  n no sample count (excludes 't')
1497  u UTC time
1498  q orientation in quaternion (excludes 'e' and 'm')
1499  e orientation in Euler angles (excludes 'm' and 'q')
1500  m orientation in matrix (excludes 'q' and 'e')
1501  A acceleration in calibrated data
1502  G rate of turn in calibrated data
1503  M magnetic field in calibrated data
1504  i only analog input 1 (excludes 'j')
1505  j only analog input 2 (excludes 'i')
1506  N North-East-Down instead of default: X North Z up
1507  For example, use "--output-settings=tqMAG" for all calibrated
1508  data, sample counter and orientation in quaternion.
1509  -p, --period=PERIOD
1510  Sampling period in (1/115200) seconds (default: 1152).
1511  Minimum is 225 (1.95 ms, 512 Hz), maximum is 1152
1512  (10.0 ms, 100 Hz).
1513  Note that for legacy devices it is the period at which sampling occurs,
1514  not the period at which messages are sent (see below).
1515 
1516 Deprecated options:
1517  -f, --deprecated-skip-factor=SKIPFACTOR
1518  Only for mark III devices.
1519  Number of samples to skip before sending MTData message
1520  (default: 0).
1521  The frequency at which MTData message is send is:
1522  115200/(PERIOD * (SKIPFACTOR + 1))
1523  If the value is 0xffff, no data is send unless a ReqData request
1524  is made.
1525 """
1526 
1527 
1528 
1531 def main():
1532  # parse command line
1533  shopts = 'hra:c:eild:b:m:s:p:f:x:vy:u:g:o:j:t:w:'
1534  lopts = ['help', 'reset', 'change-baudrate=', 'configure=', 'echo',
1535  'inspect', 'legacy-configure', 'device=', 'baudrate=',
1536  'output-mode=', 'output-settings=', 'period=',
1537  'deprecated-skip-factor=', 'xkf-scenario=', 'verbose',
1538  'synchronization=', 'utc-time=', 'gnss-platform=',
1539  'option-flags=', 'icc-command=', 'timeout=', 'initial-wait=']
1540  try:
1541  opts, args = getopt.gnu_getopt(sys.argv[1:], shopts, lopts)
1542  except getopt.GetoptError, e:
1543  print e
1544  usage()
1545  return 1
1546  # default values
1547  device = '/dev/ttyUSB0'
1548  baudrate = 115200
1549  timeout = 0.002
1550  initial_wait = 0.1
1551  mode = None
1552  settings = None
1553  period = None
1554  skipfactor = None
1555  new_baudrate = None
1556  new_xkf = None
1557  actions = []
1558  verbose = False
1559  sync_settings = [] # list of synchronization settings
1560 
1561  # filling in arguments
1562  for o, a in opts:
1563  if o in ('-h', '--help'):
1564  usage()
1565  return
1566  elif o in ('-r', '--reset'):
1567  actions.append('reset')
1568  elif o in ('-a', '--change-baudrate'):
1569  try:
1570  new_baudrate = int(a)
1571  except ValueError:
1572  print "change-baudrate argument must be integer."
1573  return 1
1574  actions.append('change-baudrate')
1575  elif o in ('-c', '--configure'):
1576  output_config = get_output_config(a)
1577  if output_config is None:
1578  return 1
1579  actions.append('configure')
1580  elif o in ('-e', '--echo'):
1581  actions.append('echo')
1582  elif o in ('-i', '--inspect'):
1583  actions.append('inspect')
1584  elif o in ('-l', '--legacy-configure'):
1585  actions.append('legacy-configure')
1586  elif o in ('-x', '--xkf-scenario'):
1587  try:
1588  new_xkf = int(a)
1589  except ValueError:
1590  print "xkf-scenario argument must be integer."
1591  return 1
1592  actions.append('xkf-scenario')
1593  elif o in ('-y', '--synchronization'):
1594  new_sync_settings = get_synchronization_settings(a)
1595  if new_sync_settings is None:
1596  return 1
1597  sync_settings.append(new_sync_settings)
1598  actions.append('synchronization')
1599  elif o in ('-u', '--setUTCtime'):
1600  UTCtime_settings = get_UTCtime(a)
1601  if UTCtime_settings is None:
1602  return 1
1603  actions.append('setUTCtime')
1604  elif o in ('-d', '--device'):
1605  device = a
1606  elif o in ('-b', '--baudrate'):
1607  try:
1608  baudrate = int(a)
1609  except ValueError:
1610  print "baudrate argument must be integer."
1611  return 1
1612  elif o in ('-m', '--output-mode'):
1613  mode = get_mode(a)
1614  if mode is None:
1615  return 1
1616  elif o in ('-s', '--output-settings'):
1617  settings = get_settings(a)
1618  if settings is None:
1619  return 1
1620  elif o in ('-p', '--period'):
1621  try:
1622  period = int(a)
1623  except ValueError:
1624  print "period argument must be integer."
1625  return 1
1626  elif o in ('-f', '--deprecated-skip-factor'):
1627  try:
1628  skipfactor = int(a)
1629  except ValueError:
1630  print "skip-factor argument must be integer."
1631  return 1
1632  elif o in ('-v', '--verbose'):
1633  verbose = True
1634  elif o in ('-g', '--gnss-platform'):
1635  platform = get_gnss_platform(a)
1636  if platform is None:
1637  return 1
1638  actions.append('gnss-platform')
1639  elif o in ('-o', '--option-flags'):
1640  flag_tuple = get_option_flags(a)
1641  if flag_tuple is None:
1642  return 1
1643  actions.append('option-flags')
1644  elif o in ('-j', '--icc-command'):
1645  icc_command = get_icc_command(a)
1646  if icc_command is None:
1647  return 1
1648  actions.append('icc-command')
1649  elif o in ('-t', '--timeout'):
1650  try:
1651  timeout = float(a)
1652  except ValueError:
1653  print "timeout argument must be a floating number."
1654  return 1
1655  elif o in ('-w', '--initial-wait'):
1656  try:
1657  initial_wait = float(a)
1658  except ValueError:
1659  print "initial-wait argument must be a floating number."
1660  return 1
1661 
1662  # if nothing else: echo
1663  if len(actions) == 0:
1664  actions.append('echo')
1665  try:
1666  if device == 'auto':
1667  devs = find_devices(timeout=timeout, verbose=verbose,
1668  initial_wait=initial_wait)
1669  if devs:
1670  print "Detected devices:", "".join('\n\t%s @ %d' % (d, p)
1671  for d, p in devs)
1672  print "Using %s @ %d" % devs[0]
1673  device, baudrate = devs[0]
1674  else:
1675  print "No suitable device found."
1676  return 1
1677  # find baudrate
1678  if not baudrate:
1679  baudrate = find_baudrate(device, timeout=timeout, verbose=verbose,
1680  initial_wait=initial_wait)
1681  if not baudrate:
1682  print "No suitable baudrate found."
1683  return 1
1684  # open device
1685  try:
1686  mt = MTDevice(device, baudrate, timeout=timeout, verbose=verbose,
1687  initial_wait=initial_wait)
1688  except serial.SerialException:
1689  raise MTException("unable to open %s" % device)
1690  # execute actions
1691  if 'inspect' in actions:
1692  inspect(mt, device, baudrate)
1693  if 'change-baudrate' in actions:
1694  print "Changing baudrate from %d to %d:" % (baudrate,
1695  new_baudrate),
1696  sys.stdout.flush()
1697  mt.ChangeBaudrate(new_baudrate)
1698  print " Ok" # should we test that it was actually ok?
1699  if 'reset' in actions:
1700  print "Restoring factory defaults",
1701  sys.stdout.flush()
1702  mt.RestoreFactoryDefaults()
1703  print " Ok" # should we test that it was actually ok?
1704  if 'configure' in actions:
1705  print "Changing output configuration",
1706  sys.stdout.flush()
1707  mt.SetOutputConfiguration(output_config)
1708  print " Ok" # should we test that it was actually ok?
1709  if 'synchronization' in actions:
1710  print "Changing synchronization settings",
1711  sys.stdout.flush()
1712  mt.SetSyncSettings(sync_settings)
1713  print " Ok" # should we test that it was actually ok?
1714  if 'setUTCtime' in actions:
1715  print "Setting UTC time in the device",
1716  sys.stdout.flush()
1717  mt.SetUTCTime(UTCtime_settings[6],
1718  UTCtime_settings[0],
1719  UTCtime_settings[1],
1720  UTCtime_settings[2],
1721  UTCtime_settings[3],
1722  UTCtime_settings[4],
1723  UTCtime_settings[5],
1724  UTCtime_settings[7])
1725  print " Ok" # should we test that it was actually ok?
1726  if 'gnss-platform' in actions:
1727  print "Setting GNSS platform",
1728  sys.stdout.flush()
1729  mt.SetGnssPlatform(platform)
1730  print " Ok" # should we test that it was actually ok?
1731  if 'option-flags' in actions:
1732  print "Setting option flags",
1733  sys.stdout.flush()
1734  mt.SetOptionFlags(*flag_tuple)
1735  print " Ok" # should we test that it was actually ok?
1736  if 'icc-command' in actions:
1737  icc_command_names = {
1738  0: 'start representative motion',
1739  1: 'stop representative motion',
1740  2: 'store ICC results',
1741  3: 'representative motion state'}
1742  print "Sending ICC command 0x%02X (%s):" % (
1743  icc_command, icc_command_names[icc_command]),
1744  sys.stdout.flush()
1745  res = mt.IccCommand(icc_command)
1746  if icc_command == 0x00:
1747  print " Ok" # should we test that it was actually ok?
1748  elif icc_command == 0x01:
1749  print res
1750  elif icc_command == 0x02:
1751  print " Ok" # should we test that it was actually ok?
1752  elif icc_command == 0x03:
1753  res_string = {0: 'representative motion inactive',
1754  1: 'representation motion active'}
1755  print "0x02X (%s)" % (res, res_string.get(res, 'unknown'))
1756  if 'legacy-configure' in actions:
1757  if mode is None:
1758  print "output-mode is require to configure the device in "\
1759  "legacy mode."
1760  return 1
1761  if settings is None:
1762  print "output-settings is required to configure the device "\
1763  "in legacy mode."
1764  return 1
1765  print "Configuring in legacy mode",
1766  sys.stdout.flush()
1767  mt.configure_legacy(mode, settings, period, skipfactor)
1768  print " Ok" # should we test it was actually ok?
1769  if 'xkf-scenario' in actions:
1770  print "Changing XKF scenario",
1771  sys.stdout.flush()
1772  mt.SetCurrentScenario(new_xkf)
1773  print "Ok"
1774  if 'echo' in actions:
1775  # if (mode is None) or (settings is None):
1776  # mode, settings, length = mt.auto_config()
1777  # print mode, settings, length
1778  try:
1779  while True:
1780  print mt.read_measurement(mode, settings)
1781  except KeyboardInterrupt:
1782  pass
1783  except MTErrorMessage as e:
1784  print "MTErrorMessage:", e
1785  except MTException as e:
1786  print "MTException:", e
1787 
1788 
1789 def inspect(mt, device, baudrate):
1790  """Inspection."""
1791  def config_fmt(config):
1792  """Hexadecimal configuration."""
1793  return '[%s]' % ', '.join('(0x%04X, %d)' % (mode, freq)
1794  for (mode, freq) in config)
1795 
1796  def hex_fmt(size=4):
1797  """Factory for hexadecimal representation formatter."""
1798  fmt = '0x%%0%dX' % (2*size)
1799 
1800  def f(value):
1801  """Hexadecimal representation."""
1802  # length of string is twice the size of the value (in bytes)
1803  return fmt % value
1804  return f
1805 
1806  def sync_fmt(settings):
1807  """Synchronization settings: N*12 bytes"""
1808  return '[%s]' % ', '.join('(0x%02X, 0x%02X, 0x%02X, 0x%02X,'
1809  ' 0x%04X, 0x%04X, 0x%04X, 0x%04X)' % s
1810  for s in settings)
1811 
1812  def try_message(m, f, formater=None, *args, **kwargs):
1813  print ' %s ' % m,
1814  try:
1815  if formater is not None:
1816  print formater(f(*args, **kwargs))
1817  else:
1818  pprint.pprint(f(*args, **kwargs), indent=4)
1819  except MTTimeoutException as e:
1820  print 'timeout: might be unsupported by your device.'
1821  except MTErrorMessage as e:
1822  if e.code == 0x04:
1823  print 'message unsupported by your device.'
1824  else:
1825  raise e
1826  print "Device: %s at %d Bd:" % (device, baudrate)
1827  try_message("device ID:", mt.GetDeviceID, hex_fmt(4))
1828  try_message("product code:", mt.GetProductCode)
1829  try_message("hardware version:", mt.GetHardwareVersion)
1830  try_message("firmware revision:", mt.GetFirmwareRev)
1831  try_message("baudrate:", mt.GetBaudrate)
1832  try_message("error mode:", mt.GetErrorMode, hex_fmt(2))
1833  try_message("option flags:", mt.GetOptionFlags, hex_fmt(4))
1834  try_message("location ID:", mt.GetLocationID, hex_fmt(2))
1835  try_message("transmit delay:", mt.GetTransmitDelay)
1836  try_message("synchronization settings:", mt.GetSyncSettings, sync_fmt)
1837  try_message("general configuration:", mt.GetConfiguration)
1838  try_message("output configuration (mark IV devices):",
1839  mt.GetOutputConfiguration, config_fmt)
1840  try_message("string output type:", mt.GetStringOutputType)
1841  try_message("period:", mt.GetPeriod)
1842  try_message("alignment rotation sensor:", mt.GetAlignmentRotation,
1843  parameter=0)
1844  try_message("alignment rotation local:", mt.GetAlignmentRotation,
1845  parameter=1)
1846  try_message("output mode:", mt.GetOutputMode, hex_fmt(2))
1847  try_message("extended output mode:", mt.GetExtOutputMode, hex_fmt(2))
1848  try_message("output settings:", mt.GetOutputSettings, hex_fmt(4))
1849  try_message("GPS coordinates (lat, lon, alt):", mt.GetLatLonAlt)
1850  try_message("GNSS platform:", mt.GetGnssPlatform)
1851  try_message("available scenarios:", mt.GetAvailableScenarios)
1852  try_message("current scenario ID:", mt.GetCurrentScenario)
1853  try_message("UTC time:", mt.GetUTCTime)
1854 
1855 
1856 def get_output_config(config_arg):
1857  """Parse the mark IV output configuration argument."""
1858  # code and max frequency
1859  code_dict = {
1860  'tt': (0x0810, 1),
1861  'iu': (0x1010, 2000),
1862  'ip': (0x1020, 2000),
1863  'ii': (0x1030, 2000),
1864  'if': (0x1060, 2000),
1865  'ic': (0x1070, 2000),
1866  'ir': (0x1080, 2000),
1867  'oq': (0x2010, 400),
1868  'om': (0x2020, 400),
1869  'oe': (0x2030, 400),
1870  'bp': (0x3010, 50),
1871  'ad': (0x4010, 2000),
1872  'aa': (0x4020, 2000),
1873  'af': (0x4030, 2000),
1874  'ah': (0x4040, 1000),
1875  'pa': (0x5020, 400),
1876  'pp': (0x5030, 400),
1877  'pl': (0x5040, 400),
1878  'np': (0x7010, 4),
1879  'ns': (0x7020, 4),
1880  'wr': (0x8020, 2000),
1881  'wd': (0x8030, 2000),
1882  'wh': (0x8040, 1000),
1883  'gd': (0x8830, 4),
1884  'gs': (0x8840, 4),
1885  'gu': (0x8880, 4),
1886  'gi': (0x88A0, 4),
1887  'rr': (0xA010, 2000),
1888  'rt': (0xA020, 2000),
1889  'mf': (0xC020, 100),
1890  'vv': (0xD010, 400),
1891  'sb': (0xE010, 2000),
1892  'sw': (0xE020, 2000)
1893  }
1894  # format flags
1895  format_dict = {'f': 0x00, 'd': 0x03, 'e': 0x00, 'n': 0x04, 'w': 0x08}
1896  config_re = re.compile('([a-z]{2})(\d+)?([fdenw])?([fdnew])?')
1897  output_configuration = []
1898  try:
1899  for item in config_arg.split(','):
1900  group, frequency, fmt1, fmt2 = config_re.findall(item.lower())[0]
1901  code, max_freq = code_dict[group]
1902  if fmt1 in format_dict:
1903  code |= format_dict[fmt1]
1904  if fmt2 in format_dict:
1905  code |= format_dict[fmt2]
1906  if frequency:
1907  frequency = min(max_freq, int(frequency))
1908  else:
1909  frequency = max_freq
1910  output_configuration.append((code, frequency))
1911  return output_configuration
1912  except (IndexError, KeyError):
1913  print 'could not parse output specification "%s"' % item
1914  return
1915 
1916 
1917 def get_mode(arg):
1918  """Parse command line output-mode argument."""
1919  try: # decimal
1920  mode = int(arg)
1921  return mode
1922  except ValueError:
1923  pass
1924  if arg[0] == '0':
1925  try: # binary
1926  mode = int(arg, 2)
1927  return mode
1928  except ValueError:
1929  pass
1930  try: # hexadecimal
1931  mode = int(arg, 16)
1932  return mode
1933  except ValueError:
1934  pass
1935  # string mode specification
1936  mode = 0
1937  for c in arg:
1938  if c == 't':
1939  mode |= OutputMode.Temp
1940  elif c == 'c':
1941  mode |= OutputMode.Calib
1942  elif c == 'o':
1943  mode |= OutputMode.Orient
1944  elif c == 'a':
1945  mode |= OutputMode.Auxiliary
1946  elif c == 'p':
1947  mode |= OutputMode.Position
1948  elif c == 'v':
1949  mode |= OutputMode.Velocity
1950  elif c == 's':
1951  mode |= OutputMode.Status
1952  elif c == 'g':
1953  mode |= OutputMode.RAWGPS
1954  elif c == 'r':
1955  mode |= OutputMode.RAW
1956  else:
1957  print "Unknown output-mode specifier: '%s'" % c
1958  return
1959  return mode
1960 
1961 
1962 def get_settings(arg):
1963  """Parse command line output-settings argument."""
1964  try: # decimal
1965  settings = int(arg)
1966  return settings
1967  except ValueError:
1968  pass
1969  if arg[0] == '0':
1970  try: # binary
1971  settings = int(arg, 2)
1972  return settings
1973  except ValueError:
1974  pass
1975  try: # hexadecimal
1976  settings = int(arg, 16)
1977  return settings
1978  except ValueError:
1979  pass
1980  # strings settings specification
1981  timestamp = 0
1982  orient_mode = 0
1983  calib_mode = OutputSettings.CalibMode_Mask
1984  NED = 0
1985  for c in arg:
1986  if c == 't':
1987  timestamp = OutputSettings.Timestamp_SampleCnt
1988  elif c == 'n':
1989  timestamp = OutputSettings.Timestamp_None
1990  elif c == 'u':
1991  timestamp |= OutputSettings.Timestamp_UTCTime
1992  elif c == 'q':
1993  orient_mode = OutputSettings.OrientMode_Quaternion
1994  elif c == 'e':
1995  orient_mode = OutputSettings.OrientMode_Euler
1996  elif c == 'm':
1997  orient_mode = OutputSettings.OrientMode_Matrix
1998  elif c == 'A':
1999  calib_mode &= OutputSettings.CalibMode_Acc
2000  elif c == 'G':
2001  calib_mode &= OutputSettings.CalibMode_Gyr
2002  elif c == 'M':
2003  calib_mode &= OutputSettings.CalibMode_Mag
2004  elif c == 'i':
2005  calib_mode &= OutputSettings.AuxiliaryMode_NoAIN2
2006  elif c == 'j':
2007  calib_mode &= OutputSettings.AuxiliaryMode_NoAIN1
2008  elif c == 'N':
2009  NED = OutputSettings.Coordinates_NED
2010  else:
2011  print "Unknown output-settings specifier: '%s'" % c
2012  return
2013  settings = timestamp | orient_mode | calib_mode | NED
2014  return settings
2015 
2016 
2018  """Parse command line synchronization-settings argument."""
2019  if arg == "clear":
2020  sync_settings = [0, 0, 0, 0, 0, 0, 0, 0]
2021  return sync_settings
2022  else:
2023  # Parse each field from the argument
2024  sync_settings = arg.split(',')
2025  try:
2026  # convert string to int
2027  sync_settings = tuple([int(i) for i in sync_settings])
2028  except ValueError:
2029  print "Synchronization sync_settings must be integers."
2030  return
2031  # check synchronization sync_settings
2032  if sync_settings[0] in (3, 4, 8, 9, 11) and \
2033  sync_settings[1] in (0, 1, 2, 4, 5, 6) and \
2034  sync_settings[2] in (1, 2, 3) and \
2035  sync_settings[3] in (0, 1):
2036  return sync_settings
2037  else:
2038  print "Invalid synchronization settings."
2039  return
2040 
2041 
2042 def get_UTCtime(arg):
2043  """Parse command line UTC time specification."""
2044  # If argument is now, fill the time settings with the current time
2045  # else fill the time settings with the specified time
2046  if arg == "now":
2047  timestamp = datetime.datetime.utcnow() # use datetime to get microsec
2048  time_settings = []
2049  time_settings.append(timestamp.year)
2050  time_settings.append(timestamp.month)
2051  time_settings.append(timestamp.day)
2052  time_settings.append(timestamp.hour)
2053  time_settings.append(timestamp.minute)
2054  time_settings.append(timestamp.second)
2055  time_settings.append(timestamp.microsecond*1000) # *1000 to get ns
2056  time_settings.append(0) # default flag to 0
2057  return time_settings
2058  else:
2059  # Parse each field from the argument
2060  time_settings = arg.split(',')
2061  try:
2062  time_settings = [int(i) for i in time_settings]
2063  except ValueError:
2064  print "UTCtime settings must be integers."
2065  return
2066 
2067  # check UTCtime settings
2068  if 1999 <= time_settings[0] <= 2099 and\
2069  1 <= time_settings[1] <= 12 and\
2070  1 <= time_settings[2] <= 31 and\
2071  0 <= time_settings[3] <= 23 and\
2072  0 <= time_settings[4] <= 59 and\
2073  0 <= time_settings[5] <= 59 and\
2074  0 <= time_settings[6] <= 1000000000:
2075  return time_settings
2076  else:
2077  print "Invalid UTCtime settings."
2078  return
2079 
2080 
2082  """Parse and check command line GNSS platform argument."""
2083  try:
2084  platform = int(arg)
2085  except ValueError:
2086  print "GNSS platform must be an integer."
2087  return
2088  if platform in (0, 8):
2089  return platform
2090  else:
2091  print "Invalid GNSS platform argument (excepted 0 or 8)."
2092  return
2093 
2094 
2096  """Parse and check command line option flags argument."""
2097  try:
2098  set_flag, clear_flag = map(lambda s: int(s.strip(), base=0),
2099  arg.split(','))
2100  return (set_flag, clear_flag)
2101  except ValueError:
2102  print 'incorrect option flags specification (expected a pair of '\
2103  'values)'
2104  return
2105 
2106 
2108  """Parse and check ICC command argument."""
2109  try:
2110  icc_command = int(arg, base=0)
2111  if icc_command not in range(4):
2112  raise ValueError
2113  return icc_command
2114  except ValueError:
2115  print 'invalid ICC command "%s"; expected 0, 1, 2, or 3.' % arg
2116  return
2117 
2118 
2119 if __name__ == '__main__':
2120  main()
def SetPeriod(self, period)
Definition: mtdevice.py:428
def get_icc_command(arg)
Definition: mtdevice.py:2107
def GetOptionFlags(self)
Definition: mtdevice.py:304
def waitfor(self, size=1)
Definition: mtdevice.py:82
def get_gnss_platform(arg)
Definition: mtdevice.py:2081
def GetOutputConfiguration(self)
Definition: mtdevice.py:393
def GetFirmwareRev(self)
Definition: mtdevice.py:246
def GetUTCTime(self)
Definition: mtdevice.py:618
def AdjustUTCTime(self, ticks)
Definition: mtdevice.py:633
def getMIDName(mid)
Definition: mtdef.py:175
def parse_MTData(self, data, mode=None, settings=None)
Definition: mtdevice.py:1035
def GetOutputSettings(self)
Definition: mtdevice.py:493
def GetErrorMode(self)
Definition: mtdevice.py:280
def GoToConfig(self)
Definition: mtdevice.py:215
def get_mode(arg)
Definition: mtdevice.py:1917
def read_data_msg(self, buf=bytearray())
Definition: mtdevice.py:94
def ResetOrientation(self, code)
Definition: mtdevice.py:601
def _ensure_config_state(self)
Definition: mtdevice.py:186
def SetGnssPlatform(self, platform)
Definition: mtdevice.py:595
def GetConfiguration(self)
Definition: mtdevice.py:364
def SetBaudrate(self, brid)
Definition: mtdevice.py:275
def SetUTCTime(self, ns, year, month, day, hour, minute, second, flag)
Definition: mtdevice.py:626
def GetSyncSettings(self)
Definition: mtdevice.py:349
def SetSyncSettings(self, sync_settings)
Definition: mtdevice.py:357
def get_settings(arg)
Definition: mtdevice.py:1962
def SetNoRotation(self, duration)
Definition: mtdevice.py:612
def auto_config_legacy(self)
Definition: mtdevice.py:687
def SetOutputSkipFactor(self, skipfactor)
Definition: mtdevice.py:507
def SetOutputConfiguration(self, output_configuration)
Definition: mtdevice.py:401
def ReqDataLength(self)
Definition: mtdevice.py:513
def read_measurement(self, mode=None, settings=None)
Definition: mtdevice.py:692
def get_output_config(config_arg)
Definition: mtdevice.py:1856
def find_baudrate(port, timeout=0.002, verbose=False, initial_wait=0.1)
Auto detect baudrate.
Definition: mtdevice.py:1193
def RunSelfTest(self)
Definition: mtdevice.py:258
def SetOutputMode(self, mode)
Definition: mtdevice.py:473
def SetExtOutputMode(self, ext_mode)
Definition: mtdevice.py:487
def RestoreFactoryDefaults(self)
Definition: mtdevice.py:330
def GetStringOutputType(self)
Definition: mtdevice.py:408
def GetProductCode(self)
Definition: mtdevice.py:233
def GetHardwareVersion(self)
Definition: mtdevice.py:239
def SetOptionFlags(self, set_flags, clear_flags)
Definition: mtdevice.py:311
def SetTransmitDelay(self, transmit_delay)
Definition: mtdevice.py:343
def write_ack(self, mid, data=b'', n_resend=30, n_read=25)
Definition: mtdevice.py:165
def GetCurrentScenario(self)
Definition: mtdevice.py:570
def main()
Main function.
Definition: mtdevice.py:1531
def read_msg(self)
Definition: mtdevice.py:131
def GetLatLonAlt(self)
Definition: mtdevice.py:531
def SetAlignmentRotation(self, parameter, quaternion)
Definition: mtdevice.py:455
def GetDeviceID(self)
Definition: mtdevice.py:226
def ChangeBaudrate(self, baudrate)
Definition: mtdevice.py:1160
def GetOutputMode(self)
Definition: mtdevice.py:466
def Reset(self, go_to_config=False)
High-level functions.
Definition: mtdevice.py:199
MTDevice class.
Definition: mtdevice.py:20
def __init__(self, port, baudrate=115200, timeout=0.002, autoconf=True, config_mode=False, verbose=False, initial_wait=0.1)
Definition: mtdevice.py:24
def write_msg(self, mid, data=b'')
Low-level communication.
Definition: mtdevice.py:60
def SetErrorMode(self, error_mode)
Definition: mtdevice.py:287
def GetTransmitDelay(self)
Definition: mtdevice.py:336
def find_devices(timeout=0.002, verbose=False, initial_wait=0.1)
Definition: mtdevice.py:1176
def GetPeriod(self)
Definition: mtdevice.py:421
def GetExtOutputMode(self)
Definition: mtdevice.py:480
def SetLocationID(self, location_id)
Definition: mtdevice.py:324
def SetLatLonAlt(self, lat, lon, alt)
Definition: mtdevice.py:546
def IccCommand(self, command)
Definition: mtdevice.py:639
def GoToMeasurement(self)
Definition: mtdevice.py:220
def get_UTCtime(arg)
Definition: mtdevice.py:2042
def GetGnssPlatform(self)
Definition: mtdevice.py:588
def get_option_flags(arg)
Definition: mtdevice.py:2095
def _ensure_measurement_state(self)
Definition: mtdevice.py:191
def configure_legacy(self, mode, settings, period=None, skipfactor=None)
High-level utility functions.
Definition: mtdevice.py:664
def GetAlignmentRotation(self, parameter)
Definition: mtdevice.py:434
def inspect(mt, device, baudrate)
Definition: mtdevice.py:1789
def GetBaudrate(self)
Definition: mtdevice.py:269
def usage()
Documentation for stand alone usage.
Definition: mtdevice.py:1220
def get_synchronization_settings(arg)
Definition: mtdevice.py:2017
def SetStringOutputType(self, string_output_type)
Definition: mtdevice.py:415
def GetAvailableScenarios(self)
Definition: mtdevice.py:554
def parse_MTData2(self, data)
Definition: mtdevice.py:705
def SetCurrentScenario(self, scenario_id)
Definition: mtdevice.py:577
def SetOutputSettings(self, settings)
Definition: mtdevice.py:500
def GetLocationID(self)
Definition: mtdevice.py:317


xsens_driver
Author(s):
autogenerated on Sun Sep 8 2019 03:15:59