ISToolsData.py
Go to the documentation of this file.
1 '''
2 Created on Feb 22, 2014
3 
4 @author: waltj
5 '''
6 from numbers import Number
7 import numpy as np
8 import os
9 import glob
10 import sys
11 import simplekml
12 import ctypes as ct
13 import pylib.pose as pose
14 import pylib.filterTools as ft
15 
16 # Profiling code
17 import time as systime
18 
19 
20 # Set Reference LLA (deg, deg, m) used for NED - Salem, UT
21 refLla = np.r_[ 40.0557114, -111.6585476, 1426.77 ]
22 gpsWeek = 0
23 
24 # Set Reference latitude, longitude, height above ellipsoid (deg, deg, m) used for NED calculations
25 def setRefLla(lla):
26  global refLla
27  refLla = lla
28 
29 def setGpsWeek(week):
30  global gpsWeek
31  # Search for a valid GPS week
32  size = np.shape(week)
33  if size and size[0] > 1:
34 # if week[0]:
35 # week = week[0]
36 # else:
37 # week = week[-1]
38  week = np.max(week)
39 
40  if week > gpsWeek:
41  gpsWeek = week
42 
43 # import time
44 
45 # Default run behavior
46 # execfile("..\INS_logger\IsParseLoggerDat.py")
47 
48 
49 class sChuckHdr(ct.Structure):
50  _pack_ = 1
51  _fields_ = [('marker', ct.c_uint32),
52  ('version', ct.c_uint16),
53  ('classification', ct.c_uint16),
54  ('name', ct.c_char * 4),
55  ('invName', ct.c_char * 4),
56  ('dataSize', ct.c_uint32),
57  ('invDataSize', ct.c_uint32),
58  ('grpNum', ct.c_uint32),
59  ('serialNum', ct.c_uint32),
60  ('pHandle', ct.c_uint32),
61  ('reserved', ct.c_uint32)]
62 
63 class sDataHdr(ct.Structure):
64  _pack_ = 1
65  _fields_ = [('id', ct.c_uint32),
66  ('size', ct.c_uint32),
67  ('offset', ct.c_uint32)]
68 
69 class sDevInfo(ct.Structure):
70  _pack_ = 1
71  _fields_ = [('reserved', ct.c_uint32),
72  ('serialNumber', ct.c_uint32),
73  ('hardwareVer', ct.c_uint8 * 4),
74  ('firmwareVer', ct.c_uint8 * 4),
75  ('build', ct.c_uint32),
76  ('commVer', ct.c_uint8 * 4),
77  ('repoRevision', ct.c_uint32),
78  ('manufacturer', ct.c_uint8 * 24),
79  ]
80 
81 class sImu(ct.Structure):
82  _pack_ = 1
83  _fields_ = [('time', ct.c_double),
84  ('pqr', ct.c_float * 3),
85  ('acc', ct.c_float * 3),
86  ('mag', ct.c_float * 3),
87  ('mslBar', ct.c_float)]
88 
89 class sIns1(ct.Structure):
90  _pack_ = 1
91  _fields_ = [('week', ct.c_uint32),
92  ('time', ct.c_double),
93  ('iStatus', ct.c_uint32),
94  ('hStatus', ct.c_uint32),
95  ('euler', ct.c_float * 3),
96  ('uvw', ct.c_float * 3),
97  ('lla', ct.c_double * 3),
98  ('ned', ct.c_float * 3),
99  ]
100 
101 class sIns2(ct.Structure):
102  _pack_ = 1
103  _fields_ = [('week', ct.c_uint32),
104  ('time', ct.c_double),
105  ('iStatus', ct.c_uint32),
106  ('hStatus', ct.c_uint32),
107  ('q', ct.c_float * 4),
108  ('uvw', ct.c_float * 3),
109  ('lla', ct.c_double * 3),
110  ]
111 
112 class sInsRes(ct.Structure):
113  _pack_ = 1
114  _fields_ = [('timeMs', ct.c_uint32),
115  ('x_dot.lla', ct.c_double * 3),
116  ('x_dot.uvw', ct.c_float * 3),
117  ('x_dot.q', ct.c_float * 4),
118  ('ned_dot', ct.c_float * 3),
119  ('accCoriolis', ct.c_float * 3),
120  ('geoMagNed', ct.c_float * 3),
121  ('magRefNed', ct.c_float * 3),
122  ('magYawOffset', ct.c_float),
123  ]
124 
125 class sInsDev1(ct.Structure):
126  _pack_ = 1
127  _fields_ = [('week', ct.c_uint32),
128  ('time', ct.c_double),
129  ('iStatus', ct.c_uint32),
130  ('hStatus', ct.c_uint32),
131  ('euler', ct.c_float * 3),
132  ('uvw', ct.c_float * 3),
133  ('lla', ct.c_double * 3),
134  ('ned', ct.c_float * 3),
135  ('eulerErr', ct.c_float * 3),
136  ('uvwErr', ct.c_float * 3),
137  ('nedErr', ct.c_float * 3),
138  ]
139 
140 class sGpsPos(ct.Structure):
141  _pack_ = 1
142  _fields_ = [('week', ct.c_uint32),
143  ('timeMs', ct.c_uint32),
144  ('satsUsed', ct.c_uint32),
145  ('cno', ct.c_uint32),
146  ('lla', ct.c_double * 3),
147  ('mMSL', ct.c_float),
148  ('hAcc', ct.c_float),
149  ('vAcc', ct.c_float),
150  ('dop', ct.c_float),
151  ]
152 
153 class sGpsVel(ct.Structure):
154  _pack_ = 1
155  _fields_ = [('timeMs', ct.c_uint32),
156  ('ned', ct.c_float * 3),
157  ('s2D', ct.c_float),
158  ('s3D', ct.c_float),
159  ('sAcc', ct.c_float),
160  ('course', ct.c_float),
161  ('cAcc', ct.c_float),
162  ]
163 
164 class sGps(ct.Structure):
165  _pack_ = 1
166  _fields_ = [('pos', sGpsPos),
167  ('vel', sGpsVel),
168  ('rxps', ct.c_uint32),
169  ('timeOffset', ct.c_double),
170  ]
171 
172 class sStateVars(ct.Structure):
173  _pack_ = 1
174  _fields_ = [('lla', ct.c_double * 3),
175  ('uvw', ct.c_float * 3),
176  ('q', ct.c_float * 4),
177  ]
178 
179 class sInsMisc(ct.Structure):
180  _pack_ = 1
181  _fields_ = [('time', ct.c_double),
182  ('timeMs', ct.c_uint32),
183  ('x', sStateVars),
184  ('theta', ct.c_float * 3),
185  ('ned', ct.c_float * 3),
186  ('dcm', ct.c_float * 9),
187  ('pqr', ct.c_float * 3),
188  ('acc', ct.c_float * 3),
189  ('mag', ct.c_float * 3),
190  ('mslBar', ct.c_float),
191  ]
192 
193 class sSysParams(ct.Structure):
194  _pack_ = 1
195  _fields_ = [('timeMs', ct.c_uint32),
196  ('iStatus', ct.c_uint32),
197  ('hStatus', ct.c_uint32),
198  ('alignAttDetect', ct.c_float),
199  ('alignAttError', ct.c_float),
200  ('alignVelError', ct.c_float),
201  ('alignPosError', ct.c_float),
202  ('insDtMs', ct.c_uint32),
203  ('ftf0', ct.c_float),
204  ('magInclination', ct.c_float),
205  ('magDeclination', ct.c_float),
206  ('genFaultCode', ct.c_uint32),
207  ]
208 
209 class sInsParams(ct.Structure):
210  _pack_ = 1
211  _fields_ = [('timeMs', ct.c_uint32),
212  ('debugI4', ct.c_int32 * 4),
213  ('debugF3', ct.c_float * 3),
214  ('debugV3', ct.c_float * 3),
215  ('debugV3g', ct.c_float * 3),
216  ('sensorState', ct.c_uint32),
217  ('obsK.psi', ct.c_float),
218  ('obsK.vel', ct.c_float),
219  ('obsK.pos', ct.c_float),
220  ('theta', ct.c_float * 3),
221  ('magTimeMs', ct.c_uint32),
222  ('vMagTiltCmp', ct.c_float * 3),
223  ('magHeading', ct.c_float),
224  ('yawErrMag', ct.c_float),
225  ('pqrEffIF', ct.c_float * 3),
226  ('pqrEff', ct.c_float * 3),
227  ('uvwEff', ct.c_float * 3),
228  ]
229 
230 class sObsParams(ct.Structure):
231  _pack_ = 1
232  _fields_ = [('timeMs', ct.c_uint32),
233 
234  ('uvw.timeMs', ct.c_uint32),
235  ('uvw.ref', ct.c_float * 3),
236  ('uvw.refValid', ct.c_uint32),
237  ('uvw.ins', ct.c_float * 3),
238 
239  ('accNed.timeMs', ct.c_uint32),
240  ('accNed.ref', ct.c_float * 3),
241  ('accNed.refValid', ct.c_uint32),
242  ('accNed.ins', ct.c_float * 3),
243 
244  ('velNed.timeMs', ct.c_uint32),
245  ('velNed.ref', ct.c_double * 3),
246  ('velNed.refValid', ct.c_uint32),
247  ('velNed.ins', ct.c_double * 3),
248 
249  ('lla.timeMs', ct.c_uint32),
250  ('lla.ref', ct.c_float * 3),
251  ('lla.refValid', ct.c_uint32),
252  ('lla.ins', ct.c_float * 3),
253 
254  ('mslBar', ct.c_float),
255  ('mslBarDot', ct.c_float),
256  ('uvwErr', ct.c_float * 3),
257  ('nedErr', ct.c_float * 3),
258  ]
259 
260 class sDebugArray(ct.Structure):
261  _pack_ = 1
262  _fields_ = [('i', ct.c_uint32 * 9),
263  ('f', ct.c_float * 9),
264  ('lf', ct.c_double * 3),
265  ]
266 
267 class sSensorBias(ct.Structure):
268  _pack_ = 1
269  _fields_ = [('timeMs', ct.c_uint32),
270  ('pqr', ct.c_float * 3),
271  ('acc', ct.c_float * 3),
272  ('mslBar', ct.c_float),
273  ]
274 
275 class sIo(ct.Structure):
276  _pack_ = 1
277  _fields_ = [('timeMs', ct.c_uint32),
278  ('gpioStatus', ct.c_uint32),
279  ]
280 
281 class sIoServos(ct.Structure):
282  _pack_ = 1
283  _fields_ = [('ch', ct.c_uint32 * 8),
284  ]
285 
286 # def getdict(self):
287 # dict((f, getattr(self, f)) for f, _ in self._fields_)
288 
289 def getdict(struct):
290  return dict((field, getattr(struct, field)) for field, _ in struct._fields_)
291 
293  def devInfo(self):
294  return ('devInfo', sDevInfo)
295  def imu(self):
296  return ('imu', sImu)
297  def ins1(self):
298  return ('ins1', sIns1)
299  def ins2(self):
300  return ('ins2', sIns2)
301  def insMisc(self):
302  return ('insMisc', sInsMisc)
303  def sysParams(self):
304  return ('sysParams', sSysParams)
305  def gpsPos(self):
306  return ('gpsPos', sGpsPos)
307  def gpsVel(self):
308  return ('gpsVel', sGpsVel)
309  def gps(self):
310  return ('gps', sGps)
311  def insResources(self):
312  return ('insResources', sInsRes)
313  def sensorBias(self):
314  return ('sensorBias', sSensorBias)
315  def insParams(self):
316  return ('insParams', sInsParams)
317  def obsParams(self):
318  return ('obsParams', sObsParams)
319  def debugArray(self):
320  return ('debugArray', sDebugArray)
321  def insDev1(self):
322  return ('insDev1', sInsDev1)
323 # def io(self):
324 # return ('io', sIo)
325 
326  def __init__(self):
327  # map the inputs to the function blocks
328  self.structList = {
329  1 : self.devInfo,
330  2 : self.imu, # INS Input
331  3 : self.ins1,
332  4 : self.ins2,
333  5 : self.gps,
334 # 6 : self.config,
335 # 7 : self.asciiBCastPeriod,
336  8 : self.insMisc,
337  9 : self.sysParams,
338 # 10 : self.sysSensors,
339 # 11 : self.flashConfig,
340 # 12 : self.gpsRssi,
341  13 : self.gpsPos,
342  14 : self.gpsVel,
343 # 15 : self.io,
344 # 16 : self.ioServosPwm,
345 # 17 : self.ioServosPpm,
346 # 18 : self.magnetometerCal,
347  19 : self.insResources,
348  27 : self.sensorBias,
349  30 : self.insParams,
350  31 : self.obsParams,
351  39 : self.debugArray,
352  47 : self.insDev1,
353  }
354 
355  def get(self, dHdr):
356  try:
357  func = self.structList.get(dHdr.id)
358  return func()
359  except:
360  return (None, None)
361 
362 
363 # Empty class/dictionary
364 class cObj:
365  def __init__(self):
366 # self.res = []
367  return
368 
369 
370 def vector3( _v, name ):
371  return np.c_[ _v[name+'[0]'].T, _v[name+'[1]'].T, _v[name+'[2]'].T ]
372 
373 def vector4( _v, name ):
374  return np.c_[ _v[name+'[0]'].T, _v[name+'[1]'].T, _v[name+'[2]'].T, _v[name+'[3]'].T ]
375 
376 
377 class cDevice:
378  def __init__(self, index, directory, serialNumber, refIns=None ):
379  global refLla
380 
381  # Profiling
382  timeStart = systime.time()
383  self.loadTime = 0
384  self.unknownId = {}
386  self.directory = directory
387  self.serialNumber = serialNumber
388  self.rdat = {} # Raw data in python list format
389  self.data = {} # data in numpy format
390  self.index = index # index in all serial numbers
391  self.refLla = refLla
392 # self.version = []
393 # self.units = []
394 
395  if refIns!=None:
396  print "#%2d Opening: Ref INS %s" % (index,directory)
397 
398  fileMask = "LOG_REF_INS*.dat"
399  # Use first file in directory if not defined
400  else:
401  print "#%2d Opening: %s %s" %(index,serialNumber,directory)
402 
403  fileMask = "LOG_"+serialNumber+"*.dat"
404 
405  if not os.path.isdir(directory):
406  print "Directory doesn't exist!"
407  sys.exit()
408  os.chdir(directory)
409  self.fileNames = glob.glob(fileMask)
410 
411  if not self.fileNames:
412 # print " ***** Files not found! Check directory name and serial number. ***** "
413  raise Exception('Load Error: .dat files not found.')
414 
415  self.parse()
416 
417  # Profiling
418  self.loadTime = systime.time() - timeStart
419  print "Load time: %.2fs" % (self.loadTime)
420 
421 
422  def parse(self):
423  self.curTime = np.r_[0]
424 
425  # Iterate over files to concatenate data
426  for fileName in self.fileNames:
427  self.__parseFile(fileName)
428 
429  # Convert lists (rdat) to numpy arrays (data)
430  self.data = self.__toNumpy(self.rdat)
431 
432 
433  def appendRDat(self, rdat, name, obj):
434  dct = getdict( obj )
435 
436 # print "Adding: ", name, " ", dct.keys()
437 # print "Adding: ", name
438 
439 # if 'pos' in dct.keys():
440 # if 'gps' == name:
441 # j = 1
442 
443  if not name in rdat.keys():
444 
445  # Create dictionary
446  rd = rdat[name] = {}
447  # print ' ' + name + ' (Created)'
448 
449  # Is a number or list of numbers NOT a dictionary (structure)
450  rd['isNum'] = {}
451 
452  for key in dct.keys():
453  rd['isNum'][key] = isinstance(dct[key], Number) or isinstance(dct[key], ct.Array)
454 
455  # Add empty list for numbers or lists or numbers
456 # if rd[dictKey].notDict:
457  if rd['isNum'][key]:
458  rd[key] = []
459 # print ' ' + ' ' + key + ' (Created)'
460 
461 # print name
462 
463  # Append new data
464  rd = rdat[name]
465  for key in dct.keys():
466 
467  # Append number or arrays
468  if rd['isNum'][key]:
469 # print ' ' + ' ' + key
470  try:
471  size = len(dct[key])
472  except:
473  size = 0
474 
475 
476  if size == 0:
477  rd[key].append(dct[key])
478  else:
479  fList = []
480  for i in range(size):
481  fList.append(dct[key][i])
482  rd[key].append(fList)
483  # Append dictionary (nested struct)
484  else:
485  self.appendRDat(rd, key, dct[key])
486 
487 
488  def __parseFile(self, filename):
489 
490 # print 'parsing: '+filename
491  with open(filename, 'rb') as f:
492 
493  cHdr = sChuckHdr()
494 
495  # Read Chunk Header
496  dHdrSize = ct.sizeof(sDataHdr)
497  while f.readinto(cHdr) and cHdr.marker == 0xFC05EA32:
498 
499  # Read Chunk Data
500  cDat = bytearray(f.read(cHdr.dataSize))
501 
502  # Parse Chunk Data
503  n = 0
504  while n < cHdr.dataSize:
505  # Data Header
506  dHdr = (sDataHdr).from_buffer(cDat,n)
507  n += dHdrSize
508 
509 # print "Found id,size: ", dHdr.id, " ", dHdr.size
510 
511 # if dHdr.id == 1:
512 # j = 1
513 
514  (sName, sType) = self.structs.get(dHdr)
515 
516  # Error check data ID
517  if sType == None:
518  if sType not in self.unknownId.keys():
519  self.unknownId[sType] = 1
520  print "Unknown data id: ", dHdr.id, " size: ", dHdr.size
521  (ct.c_ubyte*dHdr.size).from_buffer(cDat,n)
522  n += dHdr.size
523  continue
524 
525  # Error check data size
526  if ct.sizeof(sType) != dHdr.size:
527  print "Size mismatch, data id: ", dHdr.id, " size: ", dHdr.size
528  (ct.c_ubyte*dHdr.size).from_buffer(cDat,n)
529  n += dHdr.size
530  continue
531 
532  # Add ctypes structures to data dictionary
533  self.appendRDat(self.rdat, sName, (sType).from_buffer(cDat,n) )
534  n += dHdr.size
535 
536 
537  # Recursive function that converts dictionaries and lists to numpy arrays
538  def __toNumpy(self, rdat, depth=-1):
539  depth += 1
540 
541  # Dictionary
542  if type(rdat) is dict:
543  data = {}
544  for key in rdat.keys():
545 # print depth, " ", key
546 # if key == 'pos':
547 # depth += 1
548  data[key] = self.__toNumpy(rdat[key], depth)
549 
550  # List
551  elif type(rdat) is list:
552  data = []
553  # List of dictionaries (array or structs)
554  if type(rdat[0]) is dict:
555  for obj in rdat:
556  data.append( self.__toNumpy(obj, depth) )
557  # 1D or 2D List of numbers
558  elif type(rdat[0]) is list or isinstance(rdat[0], Number):
559  # Convert list to numpy arrays
560  data = np.array(rdat)
561  # List of Objects (nested structs)
562  else:
563 # if rdat[0] ==
564 
565  for obj in rdat:
566  data.append( self.__toNumpy(obj.__dict__, depth) )
567 
568  else:
569  # Add specific value
570  data = np.array(rdat)
571 
572  return data
573 
574 
575 class cDevices:
576  def __init__(self):
577  self.devices = []
578  self.loadTime = 0 # Profiling
579 
580  # Load data for to be viewed. If the "selection.txt" file is found, the line by line contents
581  # of selection.txt specify an additional subdirectory and list of serial numbers to be loaded.
582  # If serial numbers are not specified, either in selection.txt or in the loadData() parameter,
583  # then all serial numbers and files are read.
584  # directory Directory data is loaded from. If not specified, the current directory is used.
585  # serialNumbers Device serial numbers to load. If not specified, all serial numbers and files found are loaded.
586  # startDev First index of found devices (serial numbers) to load.
587  # devCount Number of devices (serial numbers) to load.
588  def loadData(self, directory=None, serialNumbers=None, postProcessed=None, refIns=None, startDev=0, devCount=-1):
589 
590  # Profiling
591  self.loadTime = 0
592  timeLoadStart = systime.time()
593 
594  # We don't support reference INS right now
595  if refIns != None:
596  raise Exception('refIns not supported right now.')
597 
598  if directory!=None:
599  # Convert backslash to forward slash (Windows to Linux)
600  directory = directory.replace('\\','/')
601 
602  # Automatically open logs specified in "selection.txt"
603  os.chdir(directory)
604 
605  # Use selection file if it exists
606  selectionFileName = 'selection.txt'
607  if os.path.exists(selectionFileName):
608  with open(selectionFileName) as f:
609  lines = f.read().splitlines()
610 
611  # Convert backslash to forward slash (Windows to Linux)
612  directory += lines[0].replace('\\','/')
613 
614  # Read serial numbers from selection.txt
615  serialNumbers=lines[1:]
616 
617  if postProcessed==1 :
618  directory += '/post_processed'
619 
620  # Add all devices in directory
621  if serialNumbers == None or serialNumbers == []:
622  # Find list of serial numbers from files in directory
623  files = os.listdir(directory)
624  serNums = []
625  for str in files:
626  if str.find('.dat') != -1:
627  str = str.replace('.dat', '')
628  if str.find('LOG_SN') != -1:
629  str = str[4:11]
630  if not str in serNums:
631  serNums.append(str)
632  elif str.find('LOG_P') != -1:
633  str = str.replace('LOG_', '')
634  str = str[:str.find('_')]
635  if not str in serNums:
636  serNums.append(str)
637  serialNumbers = serNums
638 
639  count = len(serialNumbers)
640 
641  # Validate serial numbers
642  if count <= 0:
643  print "No files found..."
644  return
645 
646  # Find size and last index
647  if devCount > 0 and devCount < count:
648  count = devCount
649  endIndex = min(startDev+count, len(serialNumbers))
650 
651 # print "Start Index: ", startDev, " End Index: ", endIndex
652 
653  # Add devices
654  for i in range(startDev, endIndex):
655  device = cDevice(i, directory, serialNumbers[i], refIns)
656  self.devices.append(device)
657 
658  # Profiling
659  self.loadTime = systime.time() - timeLoadStart
660  print "Total load time: %.2fs" % (self.loadTime)
661 
662 
663 def gpsTimeToUTC(gpsWeek, gpsSOW, leapSecs = 14):
664  # Search for a valid GPS week
665  size = np.shape(gpsWeek)
666  if size and size[0] > 1:
667 # if gpsWeek[0] == 0:
668 # gpsWeek = gpsWeek[-1]
669  # Use the largest value for the week
670  gpsWeek = np.max(gpsWeek)
671 
672  secsInWeek = 604800
673 # secsInDay = 86400
674  gpsEpoch = (1980, 1, 6, 0, 0, 0) # (year, month, day, hh, mm, ss)
675 # secFract = gpsSOW % 1
676 
677  epochTuple = gpsEpoch + (-1, -1, 0)
678  t0 = systime.mktime(epochTuple) - systime.timezone #mktime is localtime, correct for UTC
679  tdiff = (gpsWeek * secsInWeek) + gpsSOW - leapSecs
680  t = t0 + tdiff
681  return t
682 
683 
684 class cSIMPLE:
685  def __init__(self, _v):
686  self.v = _v
687 
688 
689 class cIMU:
690  def __init__(self, _v):
691  global gpsWeek
692  self.v = _v
693  self.__flt = cObj()
694  self.__flt.pqr = None
695  self.__flt.acc = None
696  self.__flt.pqrNoBias = None
697  self.__flt.accNoBias = None
698  self.__flt.barNoBias = None
699  self.cornerFreqHz = 60
700 # self.cornerFreqHz = 30
701 # self.cornerFreqHz = 15
702 
703  self.time = gpsTimeToUTC(gpsWeek, self.v['time'])
704 
705  def fltAcc(self):
706  if self.__flt.acc == None:
707  self.__flt.acc = ft.lpfNoDelay(self.v['acc'], self.cornerFreqHz, time=self.v['time'])
708  return self.__flt.acc
709 
710  def fltPqr(self):
711  if self.__flt.pqr == None:
712  self.__flt.pqr = ft.lpfNoDelay(self.v['pqr'], self.cornerFreqHz, time=self.v['time'])
713  return self.__flt.pqr
714 
715  def fltPqrNoBias(self):
716  if 'pqrNoBias' in self.v.dtype.names and self.__flt.pqrNoBias==None:
717  self.__flt.pqrNoBias = ft.lpfNoDelay(self.v['pqrNoBias'], self.cornerFreqHz, time=self.v['time'])
718  return self.__flt.pqrNoBias
719 
720  def fltAccNoBias(self):
721  if 'accNoBias' in self.v.dtype.names and self.__flt.accNoBias==None:
722  self.__flt.accNoBias = ft.lpfNoDelay(self.v['accNoBias'], self.cornerFreqHz, time=self.v['time'])
723  return self.__flt.accNoBias
724 
725  def fltBarNoBias(self):
726  if 'mslBarNoBias' in self.v.dtype.names and self.__flt.barNoBias==None:
727  self.__flt.mslBarNoBias = ft.lpfNoDelay(self.v['mslBarNoBias'], self.cornerFreqHz, time=self.v['time'])
728  return self.__flt.mslBarNoBias
729 
730 # self.mslBar = ft.smooth(self.v['mslBar']+72, delta=200)
731 # self.mslBarDot = ft.derivative(self.v['time'], self.mslBar, delta=10)
732 # self.mslBarDotLpf = ft.lpfNoDelay(self.mslBarDot, cornerFreqHz=0.5, time = self.v['time'])
733 
734 class cINS:
735  def __init__(self, _v):
736  self.v = _v
737  self.__velNED = None
738  self.__course = None
739  self.__ned = None
740  self.__size = np.shape(self.v['time'])[0]
741  self.time = gpsTimeToUTC(self.v['week'], self.v['time'])
742 
743  if not 'euler' in self.v.dtype.names and 'q' in self.v.dtype.names:
744  self.v['euler'] = pose.quat2eulerArray(self.v['q'])
745 
746  if 'euler' in self.v.dtype.names and not 'q' in self.v.dtype.names:
747  self.v['q'] = pose.euler2quatArray(self.v['euler'])
748 
749 
750  # Velocity vector in inertial frame
751  def velNed(self):
752  if self.__velNED == None:
753  self.__velNED = np.zeros(np.shape(self.v['uvw']))
754 
755  for i in range(0, self.__size):
756  DCM = pose.eulerDCM(self.v['euler'][i,:])
757  velNED = np.dot(DCM.T, self.v['uvw'][i,:]) # body to inertial frame
758  self.__velNED[i,:] = velNED
759 
760  return self.__velNED
761 
762  def course(self):
763  if self.__course == None:
764  self.__course = np.arctan2( self.velNED[:,1], self.velNED[:,0] )
765  return self.__course
766 
767  def ned(self):
768  global refLla
769  if self.__ned == None:
770  self.__ned = pose.lla2ned(refLla, self.v['lla'])
771  return self.__ned
772 
773  def set(self, time):
774  self.time = time
775 
776  def speed2D(self):
777  return np.sqrt( np.square(self.v['uvw'][:,0]) +
778  np.square(self.v['uvw'][:,1]) )
779 
780  def speed3D(self):
781  return np.sqrt( np.square(self.v['uvw'][:,0]) +
782  np.square(self.v['uvw'][:,1]) +
783  np.square(self.v['uvw'][:,2]) )
784 
785 
786 class cRIMU:
787  def __init__(self, _v,
788  accBias = np.r_[0,0,0],
789  pqrBias = np.r_[0,0,0],
790  rotate = np.r_[0,0,0]):
791  self.v = _v
792  self.cornerFreqHz = 30
793  self.__flt = cObj()
794  self.__flt.pqr = None
795  self.__flt.acc = None
796 
797  if accBias[0]!=0 or accBias[1]!=0 or accBias[2]!=0:
798  self.v['acc'] += accBias
799 
800  if pqrBias[0]!=0 or pqrBias[1]!=0 or pqrBias[2]!=0:
801  self.v['pqr'] += pqrBias
802 
803  if rotate[0]!=0 or rotate[1]!=0 or rotate[2]!=0:
804  self.v['acc'] = pose.vectorRotateInertialToBody2(self.v['acc'], rotate)
805  self.v['pqr'] = pose.vectorRotateInertialToBody2(self.v['pqr'], rotate)
806 
807  def fltPqr(self):
808  if self.__flt.pqr==None:
809  self.__flt.pqr = ft.lpfNoDelay(self.v['pqr'], self.cornerFreqHz, time=self.v['time'])
810  return self.__flt.pqr
811 
812  def fltAcc(self):
813  if self.__flt.acc==None:
814  self.__flt.acc = ft.lpfNoDelay(self.v['acc'], self.cornerFreqHz, time=self.v['time'])
815  return self.__flt.acc
816 
817 
818 class cRINS:
819  def __init__(self, _v, rotate = np.r_[0,0,0]):
820  global refLla
821  self.v = _v
822 
823  self.__ned = None
824  self.__nedDotDot = None
825  self.__uvw = None
826  self.__rotate = rotate
827 
828 # self.v['nedDot'] = ft.smooth(self.v['nedDot'], delta=10)
829 # self.v['euler'] = ft.smooth(self.v['euler'], delta=10)
830  if self.__rotate[0]!=0 or self.__rotate[1]!=0 or self.__rotate[2]!=0:
831  self.v['euler'][:,0] += self.__rotate[0]
832  self.v['euler'][:,1] += self.__rotate[1]
833  self.v['euler'][:,2] += self.__rotate[2]
834 
835  def ned(self):
836  if self.__ned==None:
837  self.__ned = pose.lla2ned(refLla, self.v['lla'])
838  return self.__ned
839 
840  def nedDotDot(self):
841  if self.__nedDotDot==None:
842  self.__nedDotDot = ft.derivative(self.v['time'], self.v['nedDot'], delta=2)
843  self.__nedDotDot[:,2] -= 9.80665
844  cornerFreqHz = 10
845  self.__nedDotDot = ft.lpfNoDelay(self.__nedDotDot, cornerFreqHz, time=self.v['time'])
846  return self.__nedDotDot
847 
848  def uvw(self):
849  if self.__uvw==None:
850  self.__uvw = pose.vectorRotateInertialToBody(self.v['nedDot'], self.v['euler'])
851  if self.__rotate[0]!=0 or self.__rotate[1]!=0 or self.__rotate[2]!=0:
852  self.uvw = pose.vectorRotateInertialToBody2(self.uvw, self.__rotate)
853  return self.__uvw
854 
855 
856 class cRGPS:
857  def __init__(self, _v):
858  global refLla
859 
860  self.v = _v
861  self.__ned = None
862  self.__acc = cObj()
863  self.__acc.ned = None
864 
865  def ned(self):
866  if self.__ned==None:
867  self.__ned = pose.lla2ned(refLla, self.v['lla'])
868  return self.__ned
869 
870  def accNed(self):
871  if self.__acc.ned == None:
872  # Create Accelerations from GPS velocities
873 # self.__acc.ned = ft.meanDerivative(self.v['time'], self.v['vel.ned'], 5, 3)
874  self.__acc.ned = ft.meanDerivative(self.v['time'], self.v['vel.ned'], 2, 2)
875  return self.__acc.ned
876 
877 
878 class cGPS:
879  def __init__(self, _v):
880  global refLla
881  self.pos = None
882  self.vel = None
883  self.acc = None
884 
885  if 'pos' in _v:
886  self.pos = cGpsPos(_v['pos'])
887 
888  if 'vel' in _v:
889  self.vel = cGpsVel(_v['vel'])
890 
891 
892 class cGpsVel:
893  def __init__(self, _v):
894  self.v = _v
895 # self.time = _v['timeMs'] * 0.001
896  self.time = gpsTimeToUTC(self.v['week'], (_v['timeMs'] * 0.001))
897  self.__acc = None
898  self.v['course'] = pose.unwrapAngle(self.v['course'])
899 
900  def acc(self):
901  if self.__acc==None:
902  self.__acc = cObj()
903  self.__acc.time = self.time
904  # self.__acc.ned = ft.meanDerivative(self.vel.time, self.v['ned'], 5, 3)
905  self.__acc.ned = ft.meanDerivative(self.time, self.v['ned'], 2, 2)
906  return self.__acc
907 
908 class cGpsAcc:
909  def __init__(self, _v):
910  self.v = _v
911 # self.time = _v['timeMs'] * 0.001
912  self.time = gpsTimeToUTC(self.v['week'], (_v['timeMs'] * 0.001))
913 
914 class cGpsPos:
915  def __init__(self, _v):
916  global refLla
917  self.v = _v
918 # self.time = _v['timeMs'] * 0.001
919  self.time = gpsTimeToUTC(self.v['week'], (_v['timeMs'] * 0.001))
920  self.ned = pose.lla2ned(refLla, _v['lla'])
921 
922 
923 class cBias:
924  def __init__(self, _v):
925  global gpsWeek
926  self.v = _v
927 # self.time = _v['timeMs'] * 0.001
928  self.time = gpsTimeToUTC(gpsWeek, (_v['timeMs'] * 0.001))
929 
930 class cInsRes:
931  def __init__(self, _v):
932  global gpsWeek
933  self.v = _v
934  self.time = gpsTimeToUTC(gpsWeek, (_v['timeMs'] * 0.001))
935 
936 class cDevInfo:
937  def __init__(self, _v):
938  self.v = _v
939 
941  def __init__(self, _v):
942  global gpsWeek
943  self.v = _v
944 
945  if 'time' in _v:
946 # self.time = _v['time']
947  self.time = gpsTimeToUTC(gpsWeek, _v['time'])
948  if 'timeMs' in _v:
949 # self.time = (_v['timeMs']) * 0.001
950  self.time = gpsTimeToUTC(gpsWeek, (_v['timeMs'] * 0.001))
951 
952 
953  self.aligning = cObj()
954  self.aligned = cObj()
955  self.aligned.coarse = cObj()
956 
957  self.aligned.coarse.att = (_v['iStatus'] >> 0) & 1 # 0-3
958  self.aligned.coarse.vel = (_v['iStatus'] >> 1) & 1
959  self.aligned.coarse.pos = (_v['iStatus'] >> 2) & 1
960 
961  self.aligned.att = (_v['iStatus'] >> 4) & 1 # 4-7
962  self.aligned.vel = (_v['iStatus'] >> 5) & 1
963  self.aligned.pos = (_v['iStatus'] >> 6) & 1
964  self.aligned.attFine = (_v['iStatus'] >> 7) & 1
965 
966  self.aligning.attGps = (_v['iStatus'] >> 8) & 1 # 8-11
967  self.aligning.vel = (_v['iStatus'] >> 9) & 1
968  self.aligning.pos = (_v['iStatus'] >> 10) & 1
969  self.aligning.attMag = (_v['iStatus'] >> 11) & 1
970 
971  self.startupAlignedDynamic = (_v['iStatus'] >> 12) & 1
972  self.gpsPosValid = (_v['iStatus'] >> 13) & 1 # 12-15
973  self.gpsVelValid = (_v['iStatus'] >> 14) & 1
974  self.gpsAccValid = (_v['iStatus'] >> 15) & 1
975 
976  self.insAcc2D = (_v['iStatus'] >> 16) & 1 # 16-19
977  self.refAcc2D = (_v['iStatus'] >> 17) & 1
978  self.refVel2D = (_v['iStatus'] >> 18) & 1
979 
980  self.startupAlignedStatic = (_v['iStatus'] >> 20) & 1 # 20-23
981 # self.startupAlignedDynamic = (_v['iStatus'] >> 21) & 1
982  self.magCal = (_v['iStatus'] >> 22) & 1
983 
984  self.biasEstPqr = (_v['iStatus'] >> 24) & 1 # 24-27
985  self.biasEstAcc = (_v['iStatus'] >> 25) & 1
986  self.biasEstBar = (_v['iStatus'] >> 26) & 1
987  self.biasEstPqrStable = (_v['iStatus'] >> 27) & 1
988 
989  # 28-31
990 
991 
992  self.motionGyrSig = (_v['hStatus'] >> 0) & 1 # 0-3
993  self.motionAccSig = (_v['hStatus'] >> 1) & 1
994  self.motionGyrDev = (_v['hStatus'] >> 2) & 1
995  self.motionAccDev = (_v['hStatus'] >> 3) & 1
996 
997  # 4-7
998 
999  self.saturationGyr1 = (_v['hStatus'] >> 8) & 1 # 8-11
1000  self.saturationAcc1 = (_v['hStatus'] >> 9) & 1
1001  self.saturationMag1 = (_v['hStatus'] >> 10) & 1
1002  self.saturationBaro = (_v['hStatus'] >> 11) & 1
1003 
1004  self.saturationGyr2 = (_v['hStatus'] >> 12) & 1 # 12-15
1005  self.saturationAcc2 = (_v['hStatus'] >> 13) & 1
1006  self.saturationMag2 = (_v['hStatus'] >> 14) & 1
1007 
1008  self.errComTxLimited = (_v['hStatus'] >> 16) & 1 # 16-19
1009  self.errComRxOverrun = (_v['hStatus'] >> 17) & 1
1010  self.errGpsTxLimited = (_v['hStatus'] >> 18) & 1
1011  self.errGpsRxOverrun = (_v['hStatus'] >> 19) & 1
1012 
1013  self.errAutobaudFault = (_v['hStatus'] >> 20) & 1 # 20-23
1014  self.errComReadFault = (_v['hStatus'] >> 21) & 1
1015 
1016  self.autobaudDetected = (_v['hStatus'] >> 24) & 1 # 24-27
1017 
1018  self.faultWatchdogReset = (_v['hStatus'] >> 28) & 1 # 28-31
1019  self.faultBODReset = (_v['hStatus'] >> 29) & 1
1020  self.faultPORReset = (_v['hStatus'] >> 30) & 1
1021  self.faultCPUErrReset = (_v['hStatus'] >> 31) & 1
1022 
1023  # 28-31
1024 
1026  def __init__(self, _v):
1027  global refLla
1028  self.v = _v
1029  self.accNed = cObj()
1030  self.velNed = cObj()
1031  self.lla = cObj()
1032  self.uvw = cObj()
1033 
1034 # self.time = _v['timeMs'] * 0.001
1035 # self.accNed.time = _v['accNed.timeMs'] * 0.001
1036 # self.velNed.time = _v['velNed.timeMs'] * 0.001
1037 # self.lla.time = _v['lla.timeMs'] * 0.001
1038 # self.uvw.time = _v['uvw.timeMs'] * 0.001
1039  self.time = gpsTimeToUTC(gpsWeek, (_v['timeMs'] * 0.001))
1040  self.accNed.time = gpsTimeToUTC(gpsWeek, (_v['accNed.timeMs'] * 0.001))
1041  self.velNed.time = gpsTimeToUTC(gpsWeek, (_v['velNed.timeMs'] * 0.001))
1042  self.lla.time = gpsTimeToUTC(gpsWeek, (_v['lla.timeMs'] * 0.001))
1043  self.uvw.time = gpsTimeToUTC(gpsWeek, (_v['uvw.timeMs'] * 0.001))
1044 
1045  self.accNed.refHdg = np.arctan2( self.v['accNed.ref'][:,1], self.v['accNed.ref'][:,0] )
1046  self.accNed.insHdg = np.arctan2( self.v['accNed.ins'][:,1], self.v['accNed.ins'][:,0] )
1047 
1048  self.lla.refNed = pose.lla2ned(refLla, _v['lla.ref'])
1049  self.lla.insNed = pose.lla2ned(refLla, _v['lla.ins'])
1050 
1051 # self.v['mslBar'] += 86;
1052 
1054  def __init__(self, _v):
1055  global gpsWeek
1056  self.v = _v
1057 
1058 # self.time = _v['timeMs'] * 0.001
1059  self.time = gpsTimeToUTC(gpsWeek, (_v['timeMs'] * 0.001))
1060  if 'magTimeMs' in _v:
1061 # self.magTime = (_v['magTimeMs']) * 0.001
1062  self.magTime = gpsTimeToUTC(gpsWeek, (_v['magTimeMs'] * 0.001))
1063 
1064 
1065 
1066 def lla2kml(time, lla, serialNumber, kmlFileName="log.kml", **kwargs ):
1067  kml = simplekml.Kml()
1068 
1069  color = kwargs.pop('color', simplekml.Color.yellow)
1070  altitudeMode = kwargs.pop('altitudeMode', simplekml.constants.AltitudeMode.absolute)
1071  timeStep = kwargs.pop('timeStep', 0)
1072 
1073  latLon = []
1074  tNext = 0
1075  lNext = 0
1076  for i in range( 0, np.shape(lla)[0]):
1077  latLon.append( (lla[i,1], lla[i,0], lla[i,2]) )
1078 
1079  # Add timestamp
1080  if timeStep:
1081 # if timeStep == -1:
1082 # pt = kml.newpoint(name="%.1f" % time[i], coords=[latLon[i]])
1083 # pt.style.iconstyle.color = color
1084 # pt.style.iconstyle.scale = 0.5
1085 # pt.style.labelstyle.scale = 0.7
1086  if time[i] >= tNext:
1087  tNext += timeStep
1088 # round(tNext, timeStep)
1089  if time[i] >= lNext:
1090  if timeStep > lNext:
1091  lNext += timeStep
1092  else:
1093  lNext += 1
1094  pt = kml.newpoint(name="%.2f" % time[i], coords=[latLon[i]])
1095  else:
1096  pt = kml.newpoint(coords=[latLon[i]])
1097  pt.style.iconstyle.color = color
1098  pt.style.iconstyle.scale = 0.4
1099  pt.style.labelstyle.scale = 0.6
1100  pt.altitudemode = altitudeMode
1101 
1102  # Add path
1103  ls = kml.newlinestring(name="Tracks", description=serialNumber+" tracks", coords=latLon)
1104 
1105  # Style
1106  ls.extrude = 1
1107  ls.altitudemode = altitudeMode
1108  ls.style.linestyle.width = 2
1109  ls.style.linestyle.color = color
1110  kml.save(kmlFileName)
1111 
1112  return kmlFileName
1113 
def appendRDat(self, rdat, name, obj)
Definition: ISToolsData.py:433
GeneratorWrapper< T > range(T const &start, T const &end, T const &step)
Definition: catch.hpp:4141
def __parseFile(self, filename)
Definition: ISToolsData.py:488
def lla2kml(time, lla, serialNumber, kmlFileName="log.kml", kwargs)
def __toNumpy(self, rdat, depth=-1)
Definition: ISToolsData.py:538
def __init__(self, _v, rotate=np.r_[0)
Definition: ISToolsData.py:819
def __init__(self, index, directory, serialNumber, refIns=None)
Definition: ISToolsData.py:378
def loadData(self, directory=None, serialNumbers=None, postProcessed=None, refIns=None, startDev=0, devCount=-1)
Definition: ISToolsData.py:588
def __init__(self, _v, accBias=np.r_[0, pqrBias=np.r_[0, rotate=np.r_[0)
Definition: ISToolsData.py:790
motionGyrSig
Hardware Status #####.
Definition: ISToolsData.py:992
def gpsTimeToUTC(gpsWeek, gpsSOW, leapSecs=14)
Definition: ISToolsData.py:663


inertial_sense_ros
Author(s):
autogenerated on Sat Sep 19 2020 03:19:04