nodes/mtdevice.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 from __future__ import print_function
3 
4 import datetime
5 import getopt
6 import pprint
7 import re
8 from serial import SerialException
9 import sys
10 
11 from xsens_driver.mtdef import MTException, MTErrorMessage, MTTimeoutException, OutputMode, OutputSettings
12 from xsens_driver.mtdevice import find_devices, find_baudrate, MTDevice
13 
14 
17 def usage():
18  print("""MT device driver.
19 Usage:
20  ./mtdevice.py [commands] [opts]
21 
22 Commands:
23  -h, --help
24  Print this help and quit.
25  -r, --reset
26  Reset device to factory defaults.
27  -a, --change-baudrate=NEW_BAUD
28  Change baudrate from BAUD (see below) to NEW_BAUD.
29  -c, --configure=OUTPUT
30  Configure the device (see OUTPUT description below).
31  -e, --echo
32  Print MTData. It is the default if no other command is supplied.
33  -i, --inspect
34  Print current MT device configuration.
35  -x, --xkf-scenario=ID
36  Change the current XKF scenario.
37  -l, --legacy-configure
38  Configure the device in legacy mode (needs MODE and SETTINGS arguments
39  below).
40  -v, --verbose
41  Verbose output.
42  -y, --synchronization=settings (see below)
43  Configure the synchronization settings of each sync line (see below).
44  -u, --utc-time=time (see below)
45  Set the UTC time buffer of the device.
46  -g, --gnss-platform=platform
47  Change the GNSS navigation filter settings (check the documentation).
48  -o, --option-flags=flags (see below)
49  Set the option flags.
50  -j, --icc-command=command (see below)
51  Send command to the In-run Compass Calibration.
52 
53 Generic options:
54  -d, --device=DEV
55  Serial interface of the device (default: /dev/ttyUSB0). If 'auto', then
56  all serial ports are tested at all baudrates and the first
57  suitable device is used.
58  -b, --baudrate=BAUD
59  Baudrate of serial interface (default: 115200). If 0, then all
60  rates are tried until a suitable one is found.
61  -t, --timeout=TIMEOUT
62  Timeout of serial communication in second (default: 0.002).
63  -w, --initial-wait=WAIT
64  Initial wait to allow device to be ready in second (default: 0.1).
65 
66 Configuration option:
67  OUTPUT
68  The format is a sequence of "<group><type><frequency>?<format>?"
69  separated by commas.
70  The frequency and format are optional.
71  The groups and types can be:
72  t temperature (max frequency: 1 Hz):
73  tt temperature
74  i timestamp (max frequency: 2000 Hz):
75  iu UTC time
76  ip packet counter
77  ii Integer Time of the Week (ITOW)
78  if sample time fine
79  ic sample time coarse
80  ir frame range
81  o orientation data (max frequency: 400 Hz):
82  oq quaternion
83  om rotation matrix
84  oe Euler angles
85  b pressure (max frequency: 50 Hz):
86  bp baro pressure
87  a acceleration (max frequency: 2000 Hz (see documentation)):
88  ad delta v
89  aa acceleration
90  af free acceleration
91  ah acceleration HR (max frequency 1000 Hz)
92  p position (max frequency: 400 Hz):
93  pa altitude ellipsoid
94  pp position ECEF
95  pl latitude longitude
96  n GNSS (max frequency: 4 Hz):
97  np GNSS PVT data
98  ns GNSS satellites info
99  w angular velocity (max frequency: 2000 Hz (see documentation)):
100  wr rate of turn
101  wd delta q
102  wh rate of turn HR (max frequency 1000 Hz)
103  g GPS (max frequency: 4 Hz):
104  gd DOP
105  gs SOL
106  gu time UTC
107  gi SV info
108  r Sensor Component Readout (max frequency: 2000 Hz):
109  rr ACC, GYR, MAG, temperature
110  rt Gyro temperatures
111  m Magnetic (max frequency: 100 Hz):
112  mf magnetic Field
113  v Velocity (max frequency: 400 Hz):
114  vv velocity XYZ
115  s Status (max frequency: 2000 Hz):
116  sb status byte
117  sw status word
118  Frequency is specified in decimal and is assumed to be the maximum
119  frequency if it is omitted.
120  Format is a combination of the precision for real valued numbers and
121  coordinate system:
122  precision:
123  f single precision floating point number (32-bit) (default)
124  d double precision floating point number (64-bit)
125  coordinate system:
126  e East-North-Up (default)
127  n North-East-Down
128  w North-West-Up
129  Examples:
130  The default configuration for the MTi-1/10/100 IMUs can be
131  specified either as:
132  "wd,ad,mf,ip,if,sw"
133  or
134  "wd2000fe,ad2000fe,mf100fe,ip2000,if2000,sw2000"
135  For getting quaternion orientation in float with sample time:
136  "oq400fw,if2000"
137  For longitude, latitude, altitude and orientation (on MTi-G-700):
138  "pl400fe,pa400fe,oq400fe"
139 
140 Synchronization settings:
141  The format follows the xsens protocol documentation. All fields are
142  required and separated by commas.
143  Note: The entire synchronization buffer is wiped every time a new one
144  is set, so it is necessary to specify the settings of multiple
145  lines at once.
146  It also possible to clear the synchronization with the argument "clear"
147 
148  Function (see manual for details):
149  3 Trigger indication
150  4 Interval Transition Measurement
151  8 SendLatest
152  9 ClockBiasEstimation
153  11 StartSampling
154  Line (manual for details):
155  0 ClockIn
156  1 GPSClockIn (only available for 700/710)
157  2 Input Line (SyncIn)
158  4 SyncOut
159  5 ExtTimepulseIn (only available for 700/710)
160  6 Software (only available for SendLatest with ReqData message)
161  Polarity:
162  1 Positive pulse/ Rising edge
163  2 Negative pulse/ Falling edge
164  3 Both/ Toggle
165  Trigger Type:
166  0 multiple times
167  1 once
168  Skip First (unsigned_int):
169  Number of initial events to skip before taking actions
170  Skip Factor (unsigned_int):
171  Number of events to skip before taking action again
172  Ignored with ReqData.
173  Pulse Width (unsigned_int):
174  Ignored for SyncIn.
175  For SyncOut, the width of the generated pulse in 100 microseconds
176  unit. Ignored for Toggle pulses.
177  Delay:
178  Delay after receiving a sync pulse to taking action,
179  100 microseconds units, range [0...600000]
180  Clock Period:
181  Reference clock period in milliseconds for ClockBiasEstimation
182  Offset:
183  Offset from event to pulse generation.
184  100 microseconds unit, range [-30000...+30000]
185 
186  Examples:
187  For changing the sync setting of the SyncIn line to trigger indication
188  with rising edge, one time triggering and no skipping and delay. Enter
189  the settings as:
190  "3,2,1,1,0,0,0,0"
191 
192  Note a number is still in the place for pulse width despite it being
193  ignored.
194 
195  To set multiple lines at once:
196  ./mtdevice.py -y 3,2,1,0,0,0,0,0 -y 9,0,1,0,0,0,10,0
197 
198  To clear the synchronization settings of MTi
199  ./mtdevice.py -y clear
200 
201 UTC time settings:
202  There are two ways to set the UTCtime for the MTi.
203  Option #1: set MTi to the current UTC time based on local system time with
204  the option 'now'
205  Option #2: set MTi to a specified UTC time
206  The time fields are set as follows:
207  year: range [1999,2099]
208  month: range [1,12]
209  day: day of the month, range [1,31]
210  hour: hour of the day, range [0,23]
211  min: minute of the hour, range [0,59]
212  sec: second of the minute, range [0,59]
213  ns: nanosecond of the second, range [0,1000000000]
214  flag:
215  1: Valid Time of Week
216  2: Valid Week Number
217  4: valid UTC
218  Note: the flag is ignored for --utc-time as it is set by the device
219  itself when connected to a GPS
220 
221  Examples:
222  Set UTC time for the device:
223  ./mtdevice.py -u now
224  ./mtdevice.py -u 1999,1,1,0,0,0,0,0
225 
226 GNSS platform settings:
227  Only for MTi-G-700/710 with firmware>=1.7.
228  The following two platform settings are listed in the documentation:
229  0: Portable
230  8: Airbone <4g
231  Check the XSens documentation before changing anything.
232 
233 Option flags:
234  Several flags can be set or cleared.
235  0x00000001 DisableAutoStore: when set, configuration changes are not saved
236  in non-volatile memory (only MTi-1 series)
237  0x00000002 DisableAutoMeasurement: when set, device will stay in Config
238  Mode upon start up (only MTi-1 series)
239  0x00000004 EnableBeidou: when set, enable Beidou and disable GLONASS (only
240  MTi-G-710)
241  0x00000010 EnableAHS: enable Active Heading Stabilization (overrides
242  magnetic reference)
243  0x00000080 EnableInRunCompassCalibration: doc is unclear
244  The flags provided must be a pair of ored values: the first for flags to be
245  set the second for the flags to be cleared.
246  Examples:
247  Only set DisableAutoStore and DisableAutoMeasurement flags:
248  ./mtdevice.py -o 0x03,0x00
249  Disable AHS (clear EnableAHS flag):
250  ./mtdevice.py -o 0x00,0x10
251  Set DisableAutoStore and clear DisableAutoMeasurement:
252  ./mtdevice.py -o 0x02,0x01
253 
254 In-run Compass Calibration commands:
255  The idea of ICC is to record magnetic field data during so-called
256  representative motion in order to better calibrate the magnetometer and
257  improve the fusion.
258  Typical usage would be to issue the start command, then move the device
259  for some time then issue the stop command. If parameters are acceptable,
260  these can be stored using the store command.
261  Commands:
262  00: Start representative motion
263  01: Stop representative motion; return ddt, dimension, and status.
264  02: Store ICC parameters
265  03: Get representative motion state; return 1 if active
266  Check the documentation for more details.
267 
268 Legacy options:
269  -m, --output-mode=MODE
270  Legacy mode of the device to select the information to output.
271  This is required for 'legacy-configure' command.
272  MODE can be either the mode value in hexadecimal, decimal or
273  binary form, or a string composed of the following characters
274  (in any order):
275  t temperature, [0x0001]
276  c calibrated data, [0x0002]
277  o orientation data, [0x0004]
278  a auxiliary data, [0x0008]
279  p position data (requires MTi-G), [0x0010]
280  v velocity data (requires MTi-G), [0x0020]
281  s status data, [0x0800]
282  g raw GPS mode (requires MTi-G), [0x1000]
283  r raw (incompatible with others except raw GPS), [0x4000]
284  For example, use "--output-mode=so" to have status and
285  orientation data.
286  -s, --output-settings=SETTINGS
287  Legacy settings of the device. This is required for 'legacy-configure'
288  command.
289  SETTINGS can be either the settings value in hexadecimal,
290  decimal or binary form, or a string composed of the following
291  characters (in any order):
292  t sample count (excludes 'n')
293  n no sample count (excludes 't')
294  u UTC time
295  q orientation in quaternion (excludes 'e' and 'm')
296  e orientation in Euler angles (excludes 'm' and 'q')
297  m orientation in matrix (excludes 'q' and 'e')
298  A acceleration in calibrated data
299  G rate of turn in calibrated data
300  M magnetic field in calibrated data
301  i only analog input 1 (excludes 'j')
302  j only analog input 2 (excludes 'i')
303  N North-East-Down instead of default: X North Z up
304  For example, use "--output-settings=tqMAG" for all calibrated
305  data, sample counter and orientation in quaternion.
306  -p, --period=PERIOD
307  Sampling period in (1/115200) seconds (default: 1152).
308  Minimum is 225 (1.95 ms, 512 Hz), maximum is 1152
309  (10.0 ms, 100 Hz).
310  Note that for legacy devices it is the period at which sampling occurs,
311  not the period at which messages are sent (see below).
312 
313 Deprecated options:
314  -f, --deprecated-skip-factor=SKIPFACTOR
315  Only for mark III devices.
316  Number of samples to skip before sending MTData message
317  (default: 0).
318  The frequency at which MTData message is send is:
319  115200/(PERIOD * (SKIPFACTOR + 1))
320  If the value is 0xffff, no data is send unless a ReqData request
321  is made.
322 """)
323 
324 
325 
328 def main():
329  # parse command line
330  shopts = 'hra:c:eild:b:m:s:p:f:x:vy:u:g:o:j:t:w:'
331  lopts = ['help', 'reset', 'change-baudrate=', 'configure=', 'echo',
332  'inspect', 'legacy-configure', 'device=', 'baudrate=',
333  'output-mode=', 'output-settings=', 'period=',
334  'deprecated-skip-factor=', 'xkf-scenario=', 'verbose',
335  'synchronization=', 'utc-time=', 'gnss-platform=',
336  'option-flags=', 'icc-command=', 'timeout=', 'initial-wait=']
337  try:
338  opts, args = getopt.gnu_getopt(sys.argv[1:], shopts, lopts)
339  except getopt.GetoptError as e:
340  print(e)
341  usage()
342  return 1
343  # default values
344  device = '/dev/ttyUSB0'
345  baudrate = 115200
346  timeout = 0.002
347  initial_wait = 0.1
348  mode = None
349  settings = None
350  period = None
351  skipfactor = None
352  new_baudrate = None
353  new_xkf = None
354  actions = []
355  verbose = False
356  sync_settings = [] # list of synchronization settings
357 
358  # filling in arguments
359  for o, a in opts:
360  if o in ('-h', '--help'):
361  usage()
362  return
363  elif o in ('-r', '--reset'):
364  actions.append('reset')
365  elif o in ('-a', '--change-baudrate'):
366  try:
367  new_baudrate = int(a)
368  except ValueError:
369  print("change-baudrate argument must be integer.")
370  return 1
371  actions.append('change-baudrate')
372  elif o in ('-c', '--configure'):
373  output_config = get_output_config(a)
374  if output_config is None:
375  return 1
376  actions.append('configure')
377  elif o in ('-e', '--echo'):
378  actions.append('echo')
379  elif o in ('-i', '--inspect'):
380  actions.append('inspect')
381  elif o in ('-l', '--legacy-configure'):
382  actions.append('legacy-configure')
383  elif o in ('-x', '--xkf-scenario'):
384  try:
385  new_xkf = int(a)
386  except ValueError:
387  print("xkf-scenario argument must be integer.")
388  return 1
389  actions.append('xkf-scenario')
390  elif o in ('-y', '--synchronization'):
391  new_sync_settings = get_synchronization_settings(a)
392  if new_sync_settings is None:
393  return 1
394  sync_settings.append(new_sync_settings)
395  actions.append('synchronization')
396  elif o in ('-u', '--setUTCtime'):
397  UTCtime_settings = get_UTCtime(a)
398  if UTCtime_settings is None:
399  return 1
400  actions.append('setUTCtime')
401  elif o in ('-d', '--device'):
402  device = a
403  elif o in ('-b', '--baudrate'):
404  try:
405  baudrate = int(a)
406  except ValueError:
407  print("baudrate argument must be integer.")
408  return 1
409  elif o in ('-m', '--output-mode'):
410  mode = get_mode(a)
411  if mode is None:
412  return 1
413  elif o in ('-s', '--output-settings'):
414  settings = get_settings(a)
415  if settings is None:
416  return 1
417  elif o in ('-p', '--period'):
418  try:
419  period = int(a)
420  except ValueError:
421  print("period argument must be integer.")
422  return 1
423  elif o in ('-f', '--deprecated-skip-factor'):
424  try:
425  skipfactor = int(a)
426  except ValueError:
427  print("skip-factor argument must be integer.")
428  return 1
429  elif o in ('-v', '--verbose'):
430  verbose = True
431  elif o in ('-g', '--gnss-platform'):
432  platform = get_gnss_platform(a)
433  if platform is None:
434  return 1
435  actions.append('gnss-platform')
436  elif o in ('-o', '--option-flags'):
437  flag_tuple = get_option_flags(a)
438  if flag_tuple is None:
439  return 1
440  actions.append('option-flags')
441  elif o in ('-j', '--icc-command'):
442  icc_command = get_icc_command(a)
443  if icc_command is None:
444  return 1
445  actions.append('icc-command')
446  elif o in ('-t', '--timeout'):
447  try:
448  timeout = float(a)
449  except ValueError:
450  print("timeout argument must be a floating number.")
451  return 1
452  elif o in ('-w', '--initial-wait'):
453  try:
454  initial_wait = float(a)
455  except ValueError:
456  print("initial-wait argument must be a floating number.")
457  return 1
458 
459  # if nothing else: echo
460  if len(actions) == 0:
461  actions.append('echo')
462  try:
463  if device == 'auto':
464  devs = find_devices(timeout=timeout, verbose=verbose,
465  initial_wait=initial_wait)
466  if devs:
467  print("Detected devices:", "".join('\n\t%s @ %d' % (d, p)
468  for d, p in devs))
469  print("Using %s @ %d" % devs[0])
470  device, baudrate = devs[0]
471  else:
472  print("No suitable device found.")
473  return 1
474  # find baudrate
475  if not baudrate:
476  baudrate = find_baudrate(device, timeout=timeout, verbose=verbose,
477  initial_wait=initial_wait)
478  if not baudrate:
479  print("No suitable baudrate found.")
480  return 1
481  # open device
482  try:
483  mt = MTDevice(device, baudrate, timeout=timeout, verbose=verbose,
484  initial_wait=initial_wait)
485  except SerialException:
486  raise MTException("unable to open %s" % device)
487  # execute actions
488  if 'inspect' in actions:
489  inspect(mt, device, baudrate)
490  if 'change-baudrate' in actions:
491  print("Changing baudrate from %d to %d:" % (baudrate,
492  new_baudrate), end=' ')
493  sys.stdout.flush()
494  mt.ChangeBaudrate(new_baudrate)
495  print(" Ok") # should we test that it was actually ok?
496  if 'reset' in actions:
497  print("Restoring factory defaults", end=' ')
498  sys.stdout.flush()
499  mt.RestoreFactoryDefaults()
500  print(" Ok") # should we test that it was actually ok?
501  if 'configure' in actions:
502  print("Changing output configuration", end=' ')
503  sys.stdout.flush()
504  mt.SetOutputConfiguration(output_config)
505  print(" Ok") # should we test that it was actually ok?
506  if 'synchronization' in actions:
507  print("Changing synchronization settings", end=' ')
508  sys.stdout.flush()
509  mt.SetSyncSettings(sync_settings)
510  print(" Ok") # should we test that it was actually ok?
511  if 'setUTCtime' in actions:
512  print("Setting UTC time in the device", end=' ')
513  sys.stdout.flush()
514  mt.SetUTCTime(UTCtime_settings[6],
515  UTCtime_settings[0],
516  UTCtime_settings[1],
517  UTCtime_settings[2],
518  UTCtime_settings[3],
519  UTCtime_settings[4],
520  UTCtime_settings[5],
521  UTCtime_settings[7])
522  print(" Ok") # should we test that it was actually ok?
523  if 'gnss-platform' in actions:
524  print("Setting GNSS platform", end=' ')
525  sys.stdout.flush()
526  mt.SetGnssPlatform(platform)
527  print(" Ok") # should we test that it was actually ok?
528  if 'option-flags' in actions:
529  print("Setting option flags", end=' ')
530  sys.stdout.flush()
531  mt.SetOptionFlags(*flag_tuple)
532  print(" Ok") # should we test that it was actually ok?
533  if 'icc-command' in actions:
534  icc_command_names = {
535  0: 'start representative motion',
536  1: 'stop representative motion',
537  2: 'store ICC results',
538  3: 'representative motion state'}
539  print("Sending ICC command 0x%02X (%s):" % (
540  icc_command, icc_command_names[icc_command]), end=' ')
541  sys.stdout.flush()
542  res = mt.IccCommand(icc_command)
543  if icc_command == 0x00:
544  print(" Ok") # should we test that it was actually ok?
545  elif icc_command == 0x01:
546  print(res)
547  elif icc_command == 0x02:
548  print(" Ok") # should we test that it was actually ok?
549  elif icc_command == 0x03:
550  res_string = {0: 'representative motion inactive',
551  1: 'representation motion active'}
552  print("0x02X (%s)" % (res, res_string.get(res, 'unknown')))
553  if 'legacy-configure' in actions:
554  if mode is None:
555  print("output-mode is require to configure the device in "
556  "legacy mode.")
557  return 1
558  if settings is None:
559  print("output-settings is required to configure the device "
560  "in legacy mode.")
561  return 1
562  print("Configuring in legacy mode", end=' ')
563  sys.stdout.flush()
564  mt.configure_legacy(mode, settings, period, skipfactor)
565  print(" Ok") # should we test it was actually ok?
566  if 'xkf-scenario' in actions:
567  print("Changing XKF scenario", end=' ')
568  sys.stdout.flush()
569  mt.SetCurrentScenario(new_xkf)
570  print("Ok")
571  if 'echo' in actions:
572  # if (mode is None) or (settings is None):
573  # mode, settings, length = mt.auto_config()
574  # print mode, settings, length
575  try:
576  while True:
577  print(mt.read_measurement(mode, settings))
578  except KeyboardInterrupt:
579  pass
580  except MTErrorMessage as e:
581  print("MTErrorMessage:", e)
582  except MTException as e:
583  print("MTException:", e)
584 
585 
586 def inspect(mt, device, baudrate):
587  """Inspection."""
588  def config_fmt(config):
589  """Hexadecimal configuration."""
590  return '[%s]' % ', '.join('(0x%04X, %d)' % (mode, freq)
591  for (mode, freq) in config)
592 
593  def hex_fmt(size=4):
594  """Factory for hexadecimal representation formatter."""
595  fmt = '0x%%0%dX' % (2*size)
596 
597  def f(value):
598  """Hexadecimal representation."""
599  # length of string is twice the size of the value (in bytes)
600  return fmt % value
601  return f
602 
603  def sync_fmt(settings):
604  """Synchronization settings: N*12 bytes"""
605  return '[%s]' % ', '.join('(0x%02X, 0x%02X, 0x%02X, 0x%02X,'
606  ' 0x%04X, 0x%04X, 0x%04X, 0x%04X)' % s
607  for s in settings)
608 
609  def try_message(m, f, formater=None, *args, **kwargs):
610  print(' %s ' % m, end=' ')
611  try:
612  if formater is not None:
613  print(formater(f(*args, **kwargs)))
614  else:
615  pprint.pprint(f(*args, **kwargs), indent=4)
616  except MTTimeoutException as e:
617  print('timeout: might be unsupported by your device.')
618  except MTErrorMessage as e:
619  if e.code == 0x04:
620  print('message unsupported by your device.')
621  else:
622  raise e
623  print("Device: %s at %d Bd:" % (device, baudrate))
624  try_message("device ID:", mt.GetDeviceID, hex_fmt(4))
625  try_message("product code:", mt.GetProductCode)
626  try_message("hardware version:", mt.GetHardwareVersion)
627  try_message("firmware revision:", mt.GetFirmwareRev)
628  try_message("baudrate:", mt.GetBaudrate)
629  try_message("error mode:", mt.GetErrorMode, hex_fmt(2))
630  try_message("option flags:", mt.GetOptionFlags, hex_fmt(4))
631  try_message("location ID:", mt.GetLocationID, hex_fmt(2))
632  try_message("transmit delay:", mt.GetTransmitDelay)
633  try_message("synchronization settings:", mt.GetSyncSettings, sync_fmt)
634  try_message("general configuration:", mt.GetConfiguration)
635  try_message("output configuration (mark IV devices):",
636  mt.GetOutputConfiguration, config_fmt)
637  try_message("string output type:", mt.GetStringOutputType)
638  try_message("period:", mt.GetPeriod)
639  try_message("alignment rotation sensor:", mt.GetAlignmentRotation,
640  parameter=0)
641  try_message("alignment rotation local:", mt.GetAlignmentRotation,
642  parameter=1)
643  try_message("output mode:", mt.GetOutputMode, hex_fmt(2))
644  try_message("extended output mode:", mt.GetExtOutputMode, hex_fmt(2))
645  try_message("output settings:", mt.GetOutputSettings, hex_fmt(4))
646  try_message("GPS coordinates (lat, lon, alt):", mt.GetLatLonAlt)
647  try_message("GNSS platform:", mt.GetGnssPlatform)
648  try_message("available scenarios:", mt.GetAvailableScenarios)
649  try_message("current scenario ID:", mt.GetCurrentScenario)
650  try_message("UTC time:", mt.GetUTCTime)
651 
652 
653 def get_output_config(config_arg):
654  """Parse the mark IV output configuration argument."""
655  # code and max frequency
656  code_dict = {
657  'tt': (0x0810, 1),
658  'iu': (0x1010, 2000),
659  'ip': (0x1020, 2000),
660  'ii': (0x1030, 2000),
661  'if': (0x1060, 2000),
662  'ic': (0x1070, 2000),
663  'ir': (0x1080, 2000),
664  'oq': (0x2010, 400),
665  'om': (0x2020, 400),
666  'oe': (0x2030, 400),
667  'bp': (0x3010, 50),
668  'ad': (0x4010, 2000),
669  'aa': (0x4020, 2000),
670  'af': (0x4030, 2000),
671  'ah': (0x4040, 1000),
672  'pa': (0x5020, 400),
673  'pp': (0x5030, 400),
674  'pl': (0x5040, 400),
675  'np': (0x7010, 4),
676  'ns': (0x7020, 4),
677  'wr': (0x8020, 2000),
678  'wd': (0x8030, 2000),
679  'wh': (0x8040, 1000),
680  'gd': (0x8830, 4),
681  'gs': (0x8840, 4),
682  'gu': (0x8880, 4),
683  'gi': (0x88A0, 4),
684  'rr': (0xA010, 2000),
685  'rt': (0xA020, 2000),
686  'mf': (0xC020, 100),
687  'vv': (0xD010, 400),
688  'sb': (0xE010, 2000),
689  'sw': (0xE020, 2000)
690  }
691  # format flags
692  format_dict = {'f': 0x00, 'd': 0x03, 'e': 0x00, 'n': 0x04, 'w': 0x08}
693  config_re = re.compile('([a-z]{2})(\d+)?([fdenw])?([fdnew])?')
694  output_configuration = []
695  try:
696  for item in config_arg.split(','):
697  group, frequency, fmt1, fmt2 = config_re.findall(item.lower())[0]
698  code, max_freq = code_dict[group]
699  if fmt1 in format_dict:
700  code |= format_dict[fmt1]
701  if fmt2 in format_dict:
702  code |= format_dict[fmt2]
703  if frequency:
704  frequency = min(max_freq, int(frequency))
705  else:
706  frequency = max_freq
707  output_configuration.append((code, frequency))
708  return output_configuration
709  except (IndexError, KeyError):
710  print('could not parse output specification "%s"' % item)
711  return
712 
713 
714 def get_mode(arg):
715  """Parse command line output-mode argument."""
716  try: # decimal
717  mode = int(arg)
718  return mode
719  except ValueError:
720  pass
721  if arg[0] == '0':
722  try: # binary
723  mode = int(arg, 2)
724  return mode
725  except ValueError:
726  pass
727  try: # hexadecimal
728  mode = int(arg, 16)
729  return mode
730  except ValueError:
731  pass
732  # string mode specification
733  mode = 0
734  for c in arg:
735  if c == 't':
736  mode |= OutputMode.Temp
737  elif c == 'c':
738  mode |= OutputMode.Calib
739  elif c == 'o':
740  mode |= OutputMode.Orient
741  elif c == 'a':
742  mode |= OutputMode.Auxiliary
743  elif c == 'p':
744  mode |= OutputMode.Position
745  elif c == 'v':
746  mode |= OutputMode.Velocity
747  elif c == 's':
748  mode |= OutputMode.Status
749  elif c == 'g':
750  mode |= OutputMode.RAWGPS
751  elif c == 'r':
752  mode |= OutputMode.RAW
753  else:
754  print("Unknown output-mode specifier: '%s'" % c)
755  return
756  return mode
757 
758 
759 def get_settings(arg):
760  """Parse command line output-settings argument."""
761  try: # decimal
762  settings = int(arg)
763  return settings
764  except ValueError:
765  pass
766  if arg[0] == '0':
767  try: # binary
768  settings = int(arg, 2)
769  return settings
770  except ValueError:
771  pass
772  try: # hexadecimal
773  settings = int(arg, 16)
774  return settings
775  except ValueError:
776  pass
777  # strings settings specification
778  timestamp = 0
779  orient_mode = 0
780  calib_mode = OutputSettings.CalibMode_Mask
781  NED = 0
782  for c in arg:
783  if c == 't':
784  timestamp = OutputSettings.Timestamp_SampleCnt
785  elif c == 'n':
786  timestamp = OutputSettings.Timestamp_None
787  elif c == 'u':
788  timestamp |= OutputSettings.Timestamp_UTCTime
789  elif c == 'q':
790  orient_mode = OutputSettings.OrientMode_Quaternion
791  elif c == 'e':
792  orient_mode = OutputSettings.OrientMode_Euler
793  elif c == 'm':
794  orient_mode = OutputSettings.OrientMode_Matrix
795  elif c == 'A':
796  calib_mode &= OutputSettings.CalibMode_Acc
797  elif c == 'G':
798  calib_mode &= OutputSettings.CalibMode_Gyr
799  elif c == 'M':
800  calib_mode &= OutputSettings.CalibMode_Mag
801  elif c == 'i':
802  calib_mode &= OutputSettings.AuxiliaryMode_NoAIN2
803  elif c == 'j':
804  calib_mode &= OutputSettings.AuxiliaryMode_NoAIN1
805  elif c == 'N':
806  NED = OutputSettings.Coordinates_NED
807  else:
808  print("Unknown output-settings specifier: '%s'" % c)
809  return
810  settings = timestamp | orient_mode | calib_mode | NED
811  return settings
812 
813 
814 def get_synchronization_settings(arg):
815  """Parse command line synchronization-settings argument."""
816  if arg == "clear":
817  sync_settings = [0, 0, 0, 0, 0, 0, 0, 0]
818  return sync_settings
819  else:
820  # Parse each field from the argument
821  sync_settings = arg.split(',')
822  try:
823  # convert string to int
824  sync_settings = tuple([int(i) for i in sync_settings])
825  except ValueError:
826  print("Synchronization sync_settings must be integers.")
827  return
828  # check synchronization sync_settings
829  if sync_settings[0] in (3, 4, 8, 9, 11) and \
830  sync_settings[1] in (0, 1, 2, 4, 5, 6) and \
831  sync_settings[2] in (1, 2, 3) and \
832  sync_settings[3] in (0, 1):
833  return sync_settings
834  else:
835  print("Invalid synchronization settings.")
836  return
837 
838 
839 def get_UTCtime(arg):
840  """Parse command line UTC time specification."""
841  # If argument is now, fill the time settings with the current time
842  # else fill the time settings with the specified time
843  if arg == "now":
844  timestamp = datetime.datetime.utcnow() # use datetime to get microsec
845  time_settings = []
846  time_settings.append(timestamp.year)
847  time_settings.append(timestamp.month)
848  time_settings.append(timestamp.day)
849  time_settings.append(timestamp.hour)
850  time_settings.append(timestamp.minute)
851  time_settings.append(timestamp.second)
852  time_settings.append(timestamp.microsecond*1000) # *1000 to get ns
853  time_settings.append(0) # default flag to 0
854  return time_settings
855  else:
856  # Parse each field from the argument
857  time_settings = arg.split(',')
858  try:
859  time_settings = [int(i) for i in time_settings]
860  except ValueError:
861  print("UTCtime settings must be integers.")
862  return
863 
864  # check UTCtime settings
865  if 1999 <= time_settings[0] <= 2099 and \
866  1 <= time_settings[1] <= 12 and \
867  1 <= time_settings[2] <= 31 and \
868  0 <= time_settings[3] <= 23 and \
869  0 <= time_settings[4] <= 59 and \
870  0 <= time_settings[5] <= 59 and \
871  0 <= time_settings[6] <= 1000000000:
872  return time_settings
873  else:
874  print("Invalid UTCtime settings.")
875  return
876 
877 
878 def get_gnss_platform(arg):
879  """Parse and check command line GNSS platform argument."""
880  try:
881  platform = int(arg)
882  except ValueError:
883  print("GNSS platform must be an integer.")
884  return
885  if platform in (0, 8):
886  return platform
887  else:
888  print("Invalid GNSS platform argument (excepted 0 or 8).")
889  return
890 
891 
892 def get_option_flags(arg):
893  """Parse and check command line option flags argument."""
894  try:
895  set_flag, clear_flag = map(lambda s: int(s.strip(), base=0),
896  arg.split(','))
897  return (set_flag, clear_flag)
898  except ValueError:
899  print('incorrect option flags specification (expected a pair of '
900  'values)')
901  return
902 
903 
904 def get_icc_command(arg):
905  """Parse and check ICC command argument."""
906  try:
907  icc_command = int(arg, base=0)
908  if icc_command not in range(4):
909  raise ValueError
910  return icc_command
911  except ValueError:
912  print('invalid ICC command "%s"; expected 0, 1, 2, or 3.' % arg)
913  return
914 
915 
916 if __name__ == '__main__':
917  main()
mtdevice.get_icc_command
def get_icc_command(arg)
Definition: nodes/mtdevice.py:904
mtdevice.get_gnss_platform
def get_gnss_platform(arg)
Definition: nodes/mtdevice.py:878
mtdevice.get_mode
def get_mode(arg)
Definition: nodes/mtdevice.py:714
xsens_driver.mtdevice
Definition: src/xsens_driver/mtdevice.py:1
mtdevice.get_settings
def get_settings(arg)
Definition: nodes/mtdevice.py:759
xsens_driver.mtdef
Definition: mtdef.py:1
xsens_driver.mtdevice.MTDevice
MTDevice class.
Definition: src/xsens_driver/mtdevice.py:27
mtdevice.get_output_config
def get_output_config(config_arg)
Definition: nodes/mtdevice.py:653
mtdevice.main
def main()
Main function.
Definition: nodes/mtdevice.py:328
xsens_driver.mtdevice.find_devices
def find_devices(timeout=0.002, verbose=False, initial_wait=0.1)
Definition: src/xsens_driver/mtdevice.py:1183
xsens_driver.mtdef.MTException
Definition: mtdef.py:284
xsens_driver.mtdevice.find_baudrate
def find_baudrate(port, timeout=0.002, verbose=False, initial_wait=0.1)
Auto detect baudrate.
Definition: src/xsens_driver/mtdevice.py:1200
mtdevice.get_UTCtime
def get_UTCtime(arg)
Definition: nodes/mtdevice.py:839
mtdevice.get_option_flags
def get_option_flags(arg)
Definition: nodes/mtdevice.py:892
mtdevice.inspect
def inspect(mt, device, baudrate)
Definition: nodes/mtdevice.py:586
mtdevice.usage
def usage()
Documentation for stand alone usage.
Definition: nodes/mtdevice.py:17
mtdevice.get_synchronization_settings
def get_synchronization_settings(arg)
Definition: nodes/mtdevice.py:814

xsens_driver
Author(s):
autogenerated on Tue May 2 2023 02:05:55