logPlotter.py
Go to the documentation of this file.
1 import math
2 from typing import List, Any, Union
3 
4 import numpy as np
5 import matplotlib.pyplot as plt
6 import sys
7 import yaml
8 import os
9 from os.path import expanduser
10 
11 BLACK = r"\u001b[30m"
12 RED = r"\u001b[31m"
13 GREEN = r"\u001b[32m"
14 YELLOW = r"\u001b[33m"
15 BLUE = r"\u001b[34m"
16 MAGENTA = r"\u001b[35m"
17 CYAN = r"\u001b[36m"
18 WHITE = r"\u001b[37m"
19 RESET = r"\u001b[0m"
20 
21 RAD2DEG = 180.0 / 3.14159
22 DEG2RAD = 3.14159 / 180.0
23 
24 sys.path.append('..')
25 from logReader import Log
26 from pylib.ISToolsDataSorted import refLla, getTimeFromTowMs, getTimeFromTow, setGpsWeek, getTimeFromGTime
27 from pylib.data_sets import *
28 from pylib.pose import quat2eulerArray, lla2ned, rotate_ecef2ned, quatRotVectArray
29 import datetime
30 
31 class logPlot:
32  def __init__(self, show, save, format, log):
33  self.show = show
34  self.save = save
35  self.directory = log.directory
36  self.format = format
37  self.log = log
38  self.d = 1
39  self.setActiveSerials(self.log.serials)
40 
41  setGpsWeek(self.log.data[0, DID_INS_2]['week'][-1])
42 
43  def setDownSample(self, dwns):
44  self.d = dwns
45 
46  def setActiveSerials(self, serials):
47  self.active_devs = []
48  for d, ser in enumerate(self.log.serials):
49  if ser in serials:
50  self.active_devs.append(d)
51 
52  def configureSubplot(self, ax, title, xlabel):
53  ax.set_title(title)
54  ax.set_xlabel(xlabel)
55 
56  def saveFig(self, fig, name):
57  if self.save:
58  fsize = fig.get_size_inches()
59  # fig.set_size_inches(16,16)
60  fig.set_size_inches(20, 20)
61  directory = os.path.dirname(self.directory + '/figures/')
62  if not os.path.exists(directory):
63  os.makedirs(directory)
64  fig.savefig(os.path.join(directory + "/" + name + '.' + self.format), bbox_inches='tight')
65  fig.set_size_inches(fsize)
66 
67  def getData(self, dev, DID, field):
68  try:
69  return self.log.data[dev, DID][field][::self.d]
70  except:
71  return []
72 
73  def setPlotYSpanMin(self, ax, limit):
74  ylim = ax.get_ylim()
75  yspn = np.max( [ylim[1] - ylim[0], limit] )
76  ylim = (np.mean(ylim)-yspn/2, np.mean(ylim)+yspn/2)
77  ax.set_ylim(ylim)
78 
79  def posNED(self, fig=None):
80  if fig is None:
81  fig = plt.figure()
82  ax = fig.subplots(3,1, sharex=True)
83  self.configureSubplot(ax[0], 'North', 'm')
84  self.configureSubplot(ax[1], 'East', 'm')
85  self.configureSubplot(ax[2], 'Down', 'm')
86  fig.suptitle('INS NED - ' + os.path.basename(os.path.normpath(self.log.directory)))
87  refLla = None
88  for d in self.active_devs:
89  if refLla is None:
90  refLla = self.getData(d, DID_INS_2, 'lla')[0]
91  ned = lla2ned(refLla, self.getData(d, DID_INS_2, 'lla'))
92  time = getTimeFromTow(self.getData(d, DID_INS_2, 'timeOfWeek'))
93  ax[0].plot(time, ned[:,0], label=self.log.serials[d])
94  ax[1].plot(time, ned[:,1])
95  ax[2].plot(time, ned[:,2])
96 
97  if(np.shape(self.active_devs)[0]==1):
98  timeGPS = getTimeFromTowMs(self.getData(d, DID_GPS1_POS, 'timeOfWeekMs'))
99  nedGps = lla2ned(self.getData(d, DID_INS_2, 'lla')[0], self.getData(d, DID_GPS1_POS, 'lla'))
100  ax[0].plot(timeGPS, nedGps[:, 0], label='GPS')
101  ax[1].plot(timeGPS, nedGps[:, 1])
102  ax[2].plot(timeGPS, nedGps[:, 2])
103 
104  ax[0].legend(ncol=2)
105  for a in ax:
106  a.grid(True)
107  self.saveFig(fig, 'posNED')
108 
109  def drawNEDMapArrow(self, ax, ned, heading):
110  # arrowLen = 0.2
111  # arrowWid = arrowLen/2
112  # arrows = np.array([arrowLen * 0.7 * np.cos(heading), arrowLen * 0.7 * np.sin(heading)]).T
113 
114  markersize = 10
115  downsample = 6
116  len = np.shape(heading)[0]
117  for i in range(1, len, downsample):
118  # ax.arrow(ned[i,1], ned[i,0], arrows[i,1], arrows[i,0], head_width=arrowWid, head_length=arrowLen, length_includes_head=True, fc='k', ec='k')
119  ax.plot(ned[i,1], ned[i,0], marker=(3, 0, -heading[i]*(180.0/np.pi)), color='g', markersize=markersize, linestyle='None')
120  ax.plot(ned[i,1], ned[i,0], marker=(2, 0, -heading[i]*(180.0/np.pi)), color='k', markersize=markersize, linestyle='None')
121 
122  def posNEDMap(self, fig=None):
123  if fig is None:
124  fig = plt.figure()
125  ax = fig.subplots(1,1)
126  ax.set_xlabel('East (m)')
127  ax.set_ylabel('North (m)')
128  fig.suptitle('NED Map - ' + os.path.basename(os.path.normpath(self.log.directory)))
129  refLla = None
130  for d in self.active_devs:
131  if refLla is None:
132  refLla = self.getData(d, DID_INS_2, 'lla')[0]
133  ned = lla2ned(refLla, self.getData(d, DID_INS_2, 'lla'))
134  euler = quat2eulerArray(self.getData(d, DID_INS_2, 'qn2b'))
135  ax.plot(ned[:,1], ned[:,0], label=self.log.serials[d])
136 
137  if(np.shape(self.active_devs)[0]==1):
138  self.drawNEDMapArrow(ax, ned, euler[:, 2])
139 
140  nedGps = lla2ned(self.getData(d, DID_INS_2, 'lla')[0], self.getData(d, DID_GPS1_POS, 'lla'))
141  ax.plot(nedGps[:, 1], nedGps[:, 0], label='GPS')
142 
143 
144  ax.set_aspect('equal', 'datalim')
145  ax.legend(ncol=2)
146  ax.grid(True)
147 
148  def posLLA(self, fig=None):
149  if fig is None:
150  fig = plt.figure()
151  ax = fig.subplots(3,1, sharex=True)
152  self.configureSubplot(ax[0], 'Latitude', 'deg')
153  self.configureSubplot(ax[1], 'Longitude', 'deg')
154  self.configureSubplot(ax[2], 'Altitude', 'deg')
155  fig.suptitle('INS LLA - ' + os.path.basename(os.path.normpath(self.log.directory)))
156  for d in self.active_devs:
157  time = getTimeFromTow(self.getData(d, DID_INS_2, 'timeOfWeek'))
158  ax[0].plot(time, self.getData(d, DID_INS_2, 'lla')[:,0], label=self.log.serials[d])
159  ax[1].plot(time, self.getData(d, DID_INS_2, 'lla')[:,1])
160  ax[2].plot(time, self.getData(d, DID_INS_2, 'lla')[:,2], label=self.log.serials[d])
161 
162  if(np.shape(self.active_devs)[0]==1):
163  timeGPS = getTimeFromTowMs(self.getData(d, DID_GPS1_POS, 'timeOfWeekMs'))
164  ax[0].plot(timeGPS, self.getData(d, DID_GPS1_POS, 'lla')[:, 0], label='GPS')
165  ax[1].plot(timeGPS, self.getData(d, DID_GPS1_POS, 'lla')[:, 1])
166  ax[2].plot(timeGPS, self.getData(d, DID_GPS1_POS, 'lla')[:, 2], label='GPS')
167 
168  timeBaro = getTimeFromTow(self.getData(d, DID_BAROMETER, 'time')+ self.getData(d, DID_GPS1_POS, 'towOffset')[-1])
169  ax[2].plot(timeBaro, self.getData(d, DID_BAROMETER, 'mslBar'), label='Baro')
170 
171  ax[0].legend(ncol=2)
172  ax[2].legend(ncol=2)
173  for a in ax:
174  a.grid(True)
175  self.saveFig(fig, 'insLLA')
176 
177  def llaGps(self, fig=None):
178  if fig is None:
179  fig = plt.figure()
180  ax = fig.subplots(3,1, sharex=True)
181  self.configureSubplot(ax[0], 'Latitude', 'deg')
182  self.configureSubplot(ax[1], 'Longitude', 'deg')
183  self.configureSubplot(ax[2], 'Altitude', 'deg')
184  fig.suptitle('GPS LLA - ' + os.path.basename(os.path.normpath(self.log.directory)))
185  for d in self.active_devs:
186  time = getTimeFromTowMs(self.getData(d, DID_GPS1_POS, 'timeOfWeekMs'))
187  ax[0].plot(time, self.getData(d, DID_GPS1_POS, 'lla')[:,0], label=self.log.serials[d])
188  ax[1].plot(time, self.getData(d, DID_GPS1_POS, 'lla')[:,1])
189  ax[2].plot(time, self.getData(d, DID_GPS1_POS, 'lla')[:,2])
190  ax[0].legend(ncol=2)
191  for a in ax:
192  a.grid(True)
193  self.saveFig(fig, 'llaGPS')
194 
195  def velNED(self, fig=None):
196  if fig is None:
197  fig = plt.figure()
198  ax = fig.subplots(3,1, sharex=True)
199  self.configureSubplot(ax[0], 'North', 'm/s')
200  self.configureSubplot(ax[1], 'East', 'm/s')
201  self.configureSubplot(ax[2], 'Down', 'm/s')
202  fig.suptitle('NED Vel - ' + os.path.basename(os.path.normpath(self.log.directory)))
203  for d in self.active_devs:
204  time = getTimeFromTow(self.getData(d, DID_INS_2, 'timeOfWeek'))
205  insVelNed = quatRotVectArray(self.getData(d, DID_INS_2, 'qn2b'), self.getData(d, DID_INS_2, 'uvw'))
206  ax[0].plot(time, insVelNed[:,0], label=self.log.serials[d])
207  ax[1].plot(time, insVelNed[:,1])
208  ax[2].plot(time, insVelNed[:,2])
209 
210  if np.shape(self.active_devs)[0] == 1: # Show GPS if #devs is 1
211  timeGPS = getTimeFromTowMs(self.getData(d, DID_GPS1_VEL, 'timeOfWeekMs'))
212  gpsVelEcef = self.getData(d, DID_GPS1_VEL, 'velEcef')
213  R = rotate_ecef2ned(self.getData(d, DID_GPS1_POS, 'lla')[0]*np.pi/180.0)
214  gpsVelNed = R.dot(gpsVelEcef.T).T
215  ax[0].plot(timeGPS, gpsVelNed[:, 0], label='GPS')
216  ax[1].plot(timeGPS, gpsVelNed[:, 1])
217  ax[2].plot(timeGPS, gpsVelNed[:, 2])
218 
219  ax[0].legend(ncol=2)
220  for a in ax:
221  a.grid(True)
222  self.saveFig(fig, 'velNED')
223 
224  def velUVW(self, fig=None):
225  if fig is None:
226  fig = plt.figure()
227  ax = fig.subplots(3,1, sharex=True)
228  self.configureSubplot(ax[0], 'Vel-X', 'm/s')
229  self.configureSubplot(ax[1], 'Vel-Y', 'm/s')
230  self.configureSubplot(ax[2], 'Vel-Z', 'm/s')
231  fig.suptitle('INS uvw - ' + os.path.basename(os.path.normpath(self.log.directory)))
232  for d in self.active_devs:
233  time = getTimeFromTow(self.getData(d, DID_INS_2, 'timeOfWeek'))
234  ax[0].plot(time, self.getData(d, DID_INS_2, 'uvw')[:,0], label=self.log.serials[d])
235  ax[1].plot(time, self.getData(d, DID_INS_2, 'uvw')[:,1])
236  ax[2].plot(time, self.getData(d, DID_INS_2, 'uvw')[:,2])
237  ax[0].legend(ncol=2)
238  for a in ax:
239  a.grid(True)
240  self.saveFig(fig, 'velUVW')
241 
242  def attitude(self, fig=None):
243  if fig is None:
244  fig = plt.figure()
245  ax = fig.subplots(3, 1, sharex=True)
246  fig.suptitle('INS Attitude - ' + os.path.basename(os.path.normpath(self.log.directory)))
247  self.configureSubplot(ax[0], 'Roll', 'deg')
248  self.configureSubplot(ax[1], 'Pitch', 'deg')
249  self.configureSubplot(ax[2], 'Yaw', 'deg')
250  for d in self.active_devs:
251  euler = quat2eulerArray(self.getData(d, DID_INS_2, 'qn2b'))
252  time = getTimeFromTow(self.getData(d, DID_INS_2, 'timeOfWeek'))
253  ax[0].plot(time, euler[:,0]*RAD2DEG, label=self.log.serials[d])
254  ax[1].plot(time, euler[:,1]*RAD2DEG)
255  ax[2].plot(time, euler[:,2]*RAD2DEG)
256  ax[0].legend(ncol=2)
257  for a in ax:
258  a.grid(True)
259  self.saveFig(fig, 'attINS')
260 
261  def heading(self, fig=None):
262  if fig is None:
263  fig = plt.figure()
264  ax = fig.subplots(3, 1, sharex=True)
265  fig.suptitle('Heading - ' + os.path.basename(os.path.normpath(self.log.directory)))
266  self.configureSubplot(ax[0], 'Magnetic Heading', 'deg')
267  self.configureSubplot(ax[1], 'RTK Compassing', 'deg')
268  self.configureSubplot(ax[2], 'INS Heading', 'deg')
269  for d in self.active_devs:
270  magTime = getTimeFromTowMs(self.getData(d, DID_INL2_MAG_OBS_INFO, 'timeOfWeekMs'))
271  gpsTime = getTimeFromTowMs(self.getData(d, DID_GPS1_RTK_CMP_REL, 'timeOfWeekMs'))
272  insTime = getTimeFromTow(self.getData(d, DID_INS_2, 'timeOfWeek'))
273  magHdg = self.getData(d, DID_INL2_MAG_OBS_INFO, 'magHdg')
274  gpsHdg = self.getData(d, DID_GPS1_RTK_CMP_REL, 'baseToRoverHeading')
275  euler = quat2eulerArray(self.getData(d, DID_INS_2, 'qn2b'))
276  if magTime:
277  ax[0].plot(magTime, magHdg * RAD2DEG)
278  if gpsTime:
279  ax[1].plot(gpsTime, gpsHdg*RAD2DEG)
280  ax[2].plot(insTime, euler[:,2]*RAD2DEG, label=self.log.serials[d])
281  ax[2].legend(ncol=2)
282  for a in ax:
283  a.grid(True)
284  self.saveFig(fig, 'heading')
285 
286  def insStatus(self, fig=None):
287  try:
288  if fig is None:
289  fig = plt.figure()
290  ax = fig.subplots(1, 1, sharex=True)
291  fig.suptitle('INS Status - ' + os.path.basename(os.path.normpath(self.log.directory)))
292 
293  for d in self.active_devs:
294  r = d == self.active_devs[0] # plot text w/ first device
295  cnt = 0
296  instime = getTimeFromTow(self.getData(d, DID_INS_2, 'timeOfWeek'))
297  iStatus = self.getData(d, DID_INS_2, 'insStatus')
298 
299  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x00000001) != 0))
300  p1 = ax.get_xlim()[0] + 0.02 * (ax.get_xlim()[1] - ax.get_xlim()[0])
301  if r: ax.text(p1, -cnt * 1.5, 'Att Coarse')
302  cnt += 1
303  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x00000010) != 0))
304  if r: ax.text(p1, -cnt * 1.5, 'Att Fine')
305  cnt += 1
306  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x00000002) != 0))
307  if r: ax.text(p1, -cnt * 1.5, 'Vel Coarse')
308  cnt += 1
309  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x00000020) != 0))
310  if r: ax.text(p1, -cnt * 1.5, 'Vel Fine')
311  cnt += 1
312  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x00000004) != 0))
313  if r: ax.text(p1, -cnt * 1.5, 'Pos Coarse')
314  cnt += 1
315  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x00000040) != 0))
316  if r: ax.text(p1, -cnt * 1.5, 'Pos Fine')
317  cnt += 1
318  cnt += 1
319 
320  # ax.plot(instime, -cnt * 1.5 + ((iStatus >> 9) & 1))
321  # ax.text(p1, -cnt * 1.5, 'GPS Update')
322  # cnt += 1
323  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x00000100) != 0))
324  if r: ax.text(p1, -cnt * 1.5, 'GPS aiding Pos/Vel')
325  cnt += 1
326  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x00000080) != 0))
327  if r: ax.text(p1, -cnt * 1.5, 'GPS aiding Hdg')
328  cnt += 1
329  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x00000800) != 0))
330  if r: ax.text(p1, -cnt * 1.5, 'MAG aiding Hdg')
331  cnt += 1
332  cnt += 1
333  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x00001000) != 0))
334  if r: ax.text(p1, -cnt * 1.5, 'Nav Mode')
335  cnt += 1
336  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x000F0000) >> 16) / 4.0)
337  if r: ax.text(p1, -cnt * 1.5, 'Solution Status')
338  cnt += 1
339  cnt += 1
340 
341  ax.plot(instime, -cnt * 1.5 + (((iStatus & 0x03000000) >> 24) == 3))
342  if r: ax.text(p1, -cnt * 1.5, 'RTK: Precision Position Valid')
343  cnt += 1
344  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x04000000) != 0))
345  if r: ax.text(p1, -cnt * 1.5, 'RTK: Compassing Valid (fix & hold)')
346  cnt += 1
347  cnt += 1
348 
349  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x00100000) != 0))
350  if r: ax.text(p1, -cnt * 1.5, 'RTK: Compassing Baseline UNSET')
351  cnt += 1
352  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x00200000) != 0))
353  if r: ax.text(p1, -cnt * 1.5, 'RTK: Compassing Baseline BAD')
354  cnt += 1
355  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x08000000) != 0))
356  if r: ax.text(p1, -cnt * 1.5, 'RTK: No Observ.')
357  cnt += 1
358  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x10000000) != 0))
359  if r: ax.text(p1, -cnt * 1.5, 'RTK: Base No Pos.')
360  cnt += 1
361  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x20000000) != 0))
362  if r: ax.text(p1, -cnt * 1.5, 'RTK: Base Pos. Moving')
363  cnt += 1
364  cnt += 1
365 
366  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x00400000) != 0))
367  if r: ax.text(p1, -cnt * 1.5, 'Mag: Recalibrating')
368  cnt += 1
369  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x00800000) != 0))
370  if r: ax.text(p1, -cnt * 1.5, 'Mag: Inter. or Bad Cal')
371  cnt += 1
372  cnt += 1
373 
374  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x40000000) != 0))
375  if r: ax.text(p1, -cnt * 1.5, 'RTOS Task Period Overrun')
376  cnt += 1
377  ax.plot(instime, -cnt * 1.5 + ((iStatus & 0x80000000) != 0))
378  if r: ax.text(p1, -cnt * 1.5, 'General Fault')
379  cnt += 1
380 
381  ax.grid(True)
382  self.saveFig(fig, 'iStatus')
383  except:
384  print(RED + "problem plotting insStatus: " + sys.exc_info()[0] + RESET)
385 
386  def hdwStatus(self, fig=None):
387  try:
388  if fig is None:
389  fig = plt.figure()
390  ax = fig.subplots(1, 1, sharex=True)
391  fig.suptitle('Hardware Status - ' + os.path.basename(os.path.normpath(self.log.directory)))
392 
393  for d in self.active_devs:
394  r = d == self.active_devs[0] # plot text w/ first device
395  cnt = 0
396  instime = getTimeFromTow(self.getData(d, DID_INS_2, 'timeOfWeek'))
397  hStatus = self.getData(d, DID_INS_2, 'hdwStatus')
398 
399  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x00000001) != 0))
400  p1 = ax.get_xlim()[0] + 0.02 * (ax.get_xlim()[1] - ax.get_xlim()[0])
401  if r: ax.text(p1, -cnt * 1.5, 'Motion Gyr Sig')
402  cnt += 1
403  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x00000002) != 0))
404  if r: ax.text(p1, -cnt * 1.5, 'Motion Acc Sig')
405  cnt += 1
406  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x00000004) != 0))
407  if r: ax.text(p1, -cnt * 1.5, 'Motion Gyr Dev')
408  cnt += 1
409  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x00000005) != 0))
410  if r: ax.text(p1, -cnt * 1.5, 'Motion Acc Dev')
411  cnt += 1
412  cnt += 1
413 
414  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x00000010) != 0))
415  if r: ax.text(p1, -cnt * 1.5, 'Satellite Rx')
416  cnt += 1
417  cnt += 1
418 
419  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x00000100) != 0))
420  if r: ax.text(p1, -cnt * 1.5, 'Saturation Gyr')
421  cnt += 1
422  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x00000200) != 0))
423  if r: ax.text(p1, -cnt * 1.5, 'Saturation Acc')
424  cnt += 1
425  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x00000400) != 0))
426  if r: ax.text(p1, -cnt * 1.5, 'Saturation Mag')
427  cnt += 1
428  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x00000800) != 0))
429  if r: ax.text(p1, -cnt * 1.5, 'Saturation Baro')
430  cnt += 1
431  cnt += 1
432 
433  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x00010000) != 0))
434  if r: ax.text(p1, -cnt * 1.5, 'Err Com Tx Limited')
435  cnt += 1
436  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x00020000) != 0))
437  if r: ax.text(p1, -cnt * 1.5, 'Err Com Rx Overrun')
438  cnt += 1
439  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x00040000) != 0))
440  if r: ax.text(p1, -cnt * 1.5, 'Err GPS Tx Limited')
441  cnt += 1
442  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x00080000) != 0))
443  if r: ax.text(p1, -cnt * 1.5, 'Err GPS Rx Overrun')
444  cnt += 1
445  cnt += 1
446 
447  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x00F00000) >> 20) / 4)
448  if r: ax.text(p1, -cnt * 1.5, 'Com Parse Error Count')
449  cnt += 1
450  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x01000000) != 0))
451  if r: ax.text(p1, -cnt * 1.5, 'BIT Self Test Fault')
452  cnt += 1
453  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x02000000) != 0))
454  if r: ax.text(p1, -cnt * 1.5, 'Temperature error')
455  cnt += 1
456  cnt += 1
457 
458  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x10000000) != 0))
459  if r: ax.text(p1, -cnt * 1.5, 'Reset Backup Mode')
460  cnt += 1
461  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x20000000) != 0))
462  if r: ax.text(p1, -cnt * 1.5, 'Watchdog Reset')
463  cnt += 1
464  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x30000000) != 0))
465  if r: ax.text(p1, -cnt * 1.5, 'Software Reset')
466  cnt += 1
467  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x40000000) != 0))
468  if r: ax.text(p1, -cnt * 1.5, 'Hardware Reset')
469  cnt += 1
470  ax.plot(instime, -cnt * 1.5 + ((hStatus & 0x80000000) != 0))
471  if r: ax.text(p1, -cnt * 1.5, 'Critical Sys Fault')
472  cnt += 1
473  cnt += 1
474 
475  ax.grid(True)
476  self.saveFig(fig, 'Hardware Status')
477  except:
478  print(RED + "problem plotting hdwStatus: " + sys.exc_info()[0] + RESET)
479 
480  def gpsStats(self, fig=None, did_gps_pos=DID_GPS1_POS):
481  # try:
482  if fig is None:
483  fig = plt.figure()
484 
485  ax = fig.subplots(4, 1, sharex=True)
486  if did_gps_pos==DID_GPS1_POS:
487  gps_num = 1
488  else:
489  gps_num = 2
490  fig.suptitle('GPS ' + str(gps_num) + ' Stats - ' + os.path.basename(os.path.normpath(self.log.directory)))
491  self.configureSubplot(ax[0], 'Satellites Used in Solution', '')
492  self.configureSubplot(ax[1], 'Accuracy', 'm')
493  self.configureSubplot(ax[2], 'CNO', 'dBHz')
494  self.configureSubplot(ax[3], 'Status', '')
495 
496  for d in self.active_devs:
497  r = d == self.active_devs[0] # plot text w/ first device
498  time = getTimeFromTowMs(self.getData(d, did_gps_pos, 'timeOfWeekMs'))
499  gStatus = self.getData(d, did_gps_pos, 'status')
500 
501  ax[0].plot(time, gStatus & 0xFF, label=self.log.serials[d])
502  ax[1].plot(time, self.getData(d, did_gps_pos, 'pDop'), 'm', label="pDop")
503  ax[1].plot(time, self.getData(d, did_gps_pos, 'hAcc'), 'r', label="hAcc")
504  ax[1].plot(time, self.getData(d, did_gps_pos, 'vAcc'), 'b', label="vAcc")
505  if self.log.data[d, DID_GPS1_RTK_POS] is not []:
506  rtktime = getTimeFromTowMs(self.getData(d, DID_GPS1_RTK_POS, 'timeOfWeekMs'))
507  ax[1].plot(rtktime, self.getData(d, DID_GPS1_RTK_POS, 'vAcc'), 'g', label="rtkHor")
508  if d == 0:
509  ax[1].legend(ncol=2)
510  ax[2].plot(time, self.getData(d, did_gps_pos, 'cnoMean'))
511 
512  cnt = 0
513  ax[3].plot(time, -cnt * 1.5 + ((gStatus & 0x04000000) != 0))
514  p1 = ax[3].get_xlim()[0] + 0.02 * (ax[3].get_xlim()[1] - ax[3].get_xlim()[0])
515  if r: ax[3].text(p1, -cnt * 1.5, 'RTK Positioning Valid')
516  cnt += 1
517  ax[3].plot(time, -cnt * 1.5 + ((gStatus & 0x08000000) != 0))
518  if r: ax[3].text(p1, -cnt * 1.5, 'RTK Compassing Valid (fix & hold)')
519  cnt += 1
520  ax[3].plot(time, -cnt * 1.5 + ((gStatus & 0x00002000) != 0))
521  if r: ax[3].text(p1, -cnt * 1.5, 'GPS Compass Baseline BAD')
522  cnt += 1
523  ax[3].plot(time, -cnt * 1.5 + ((gStatus & 0x00004000) != 0))
524  if r: ax[3].text(p1, -cnt * 1.5, 'GPS Compass Baseline UNSET')
525  cnt += 1
526 
527  self.setPlotYSpanMin(ax[2], 5)
528 
529  ax[0].legend(ncol=2)
530  for a in ax:
531  a.grid(True)
532  self.saveFig(fig, 'Gps Stats')
533  # except:
534  # print(RED + "problem plotting gpsStats: " + sys.exc_info()[0] + RESET)
535 
536  def gps2Stats(self, fig=None):
537  self.gpsStats(fig=fig, did_gps_pos=DID_GPS2_POS)
538 
539  def rtkPosStats(self, fig=None):
540  self.rtkStats("Position", DID_GPS1_RTK_POS_REL, fig=fig)
541 
542  def rtkCmpStats(self, fig=None):
543  self.rtkStats("Compassing", DID_GPS1_RTK_CMP_REL, fig=fig)
544 
545  def rtkStats(self, name, relDid, fig=None):
546  # try:
547  n_plots = 6
548  if fig is None:
549  fig = plt.figure()
550 
551  ax = fig.subplots(n_plots, 1, sharex=True)
552  fig.suptitle('RTK ' + name + ' Stats - ' + os.path.basename(os.path.normpath(self.log.directory)))
553  self.configureSubplot(ax[0], 'GPS Fix Type: 2=2D, 3=3D, 10=Single, 11=Float, 12=Fix', '')
554  self.configureSubplot(ax[1], 'Age of Differential', 's')
555  self.configureSubplot(ax[2], 'AR Ratio', '')
556  self.configureSubplot(ax[3], 'Base to Rover Distance', 'm')
557  self.configureSubplot(ax[4], 'Base to Rover Heading', 'deg')
558  self.configureSubplot(ax[5], 'Base to Rover Heading Accuracy', 'deg')
559 
560  for i, d in enumerate(self.active_devs):
561  rtkRelTime = getTimeFromTowMs(self.getData(d, relDid, 'timeOfWeekMs'))
562  # rtkMiscTime = getTimeFromTowMs(self.getData(d, DID_GPS1_RTK_CMP_MISC, 'timeOfWeekMs'))
563  if not self.log.compassing:
564  gps1PosTime = getTimeFromTowMs(self.getData(d, DID_GPS1_POS, 'timeOfWeekMs'))
565  fixType = self.getData(d, DID_GPS1_POS, 'status') >> 8 & 0x1F
566  ax[0].plot(gps1PosTime, fixType, label=self.log.serials[d])
567  else:
568  fixType = self.getData(d, relDid, 'arRatio').copy()
569  fixType[(fixType > 3)] = 12
570  fixType[(fixType > 0) & (fixType < 3)] = 11
571  fixType[fixType == 0] = 10
572  ax[0].plot(rtkRelTime, fixType, label=self.log.serials[d])
573  ax[1].plot(rtkRelTime, self.getData(d, relDid, 'differentialAge'))
574  if i == 0:
575  ax[2].semilogy(rtkRelTime, np.ones_like(rtkRelTime)*3.0, 'k--')
576  ax[2].semilogy(rtkRelTime, self.getData(d, relDid, 'arRatio'))
577  dist2base = self.getData(d, relDid, 'baseToRoverDistance')
578  dist2base[dist2base > 1e5] = np.nan
579  ax[3].plot(rtkRelTime, dist2base)
580  ax[4].plot(rtkRelTime, self.getData(d, relDid, 'baseToRoverHeading')*180.0/np.pi)
581  ax[5].plot(rtkRelTime, self.getData(d, relDid, 'baseToRoverHeadingAcc')*180.0/np.pi)
582  ax[0].legend(ncol=2)
583 
584  self.setPlotYSpanMin(ax[1], 0.5) # Differential age
585  self.setPlotYSpanMin(ax[3], 1.0) # Distance to base
586 
587  for a in ax:
588  a.grid(True)
589  self.saveFig(fig, 'rtk'+name+'Stats')
590  # except:
591  # print(RED + "problem plotting rtkStats: " + sys.exc_info()[0] + RESET)
592 
593  def rtkPosMisc(self, fig=None):
594  self.rtkMisc("Position", DID_GPS1_RTK_POS_MISC, fig=fig)
595 
596  def rtkCmpMisc(self, fig=None):
597  self.rtkMisc("Position", DID_GPS1_RTK_CMP_MISC, fig=fig)
598 
599  def rtkMisc(self, name, miscDid, fig=None):
600  # try:
601  n_plots = 10
602  if fig is None:
603  fig = plt.figure()
604 
605  ax = fig.subplots(5, 2, sharex=True)
606  fig.suptitle('RTK ' + name + ' Misc - ' + os.path.basename(os.path.normpath(self.log.directory)))
607  self.configureSubplot(ax[0,0], 'Correction checksum failure count', '')
608  self.configureSubplot(ax[1,0], 'Time to First Fix', 's')
609  self.configureSubplot(ax[2,0], 'GPS Observation Count - Rover', '')
610  self.configureSubplot(ax[3,0], 'GPS Observation Count - Base', '')
611  self.configureSubplot(ax[4,0], 'Glonass Observation Count - Rover', '')
612  self.configureSubplot(ax[0,1], 'Glonass Observation Count - Base', '')
613  self.configureSubplot(ax[1,1], 'Galileo Observation Count - Rover', '')
614  self.configureSubplot(ax[2,1], 'Galileo Observation Count - Base', '')
615  self.configureSubplot(ax[3,1], 'SBAS Count Base', '')
616  self.configureSubplot(ax[4,1], 'Base Antenna Position Count', '')
617 
618  for i, d in enumerate(self.active_devs):
619  # rtkRelTime = getTimeFromTowMs(self.getData(d, DID_GPS1_RTK_POS_REL, 'timeOfWeekMs'))
620  rtkMiscTime = getTimeFromTowMs(self.getData(d, miscDid, 'timeOfWeekMs'))
621  ax[0,0].plot(rtkMiscTime, self.getData(d, miscDid, 'correctionChecksumFailures'))
622  ax[1,0].plot(rtkMiscTime, self.getData(d, miscDid, 'timeToFirstFixMs')*0.001)
623  ax[2,0].plot(rtkMiscTime, self.getData(d, miscDid, 'roverGpsObservationCount'))
624  ax[3,0].plot(rtkMiscTime, self.getData(d, miscDid, 'baseGpsObservationCount'))
625  ax[4,0].plot(rtkMiscTime, self.getData(d, miscDid, 'roverGlonassObservationCount'))
626  ax[0,1].plot(rtkMiscTime, self.getData(d, miscDid, 'baseGlonassObservationCount'))
627  ax[1,1].plot(rtkMiscTime, self.getData(d, miscDid, 'roverGalileoObservationCount'))
628  ax[2,1].plot(rtkMiscTime, self.getData(d, miscDid, 'baseGalileoObservationCount'))
629  ax[3,1].plot(rtkMiscTime, self.getData(d, miscDid, 'baseSbasCount'))
630  ax[4,1].plot(rtkMiscTime, self.getData(d, miscDid, 'baseAntennaCount'))
631 
632  # # ax[0].plot(rtkRelTime, self.getData(d, DID_GPS1_RTK_POS_REL, 'differentialAge'))
633  # if i == 0:
634  # ax[2].semilogy(rtkRelTime, np.ones_like(rtkRelTime)*3.0, 'k--')
635  # ax[2].semilogy(rtkRelTime, self.getData(d, DID_GPS1_RTK_POS_REL, 'arRatio'))
636  # dist2base = self.getData(d, DID_GPS1_RTK_POS_REL, 'distanceToBase')
637  # dist2base[dist2base > 1e5] = np.nan
638  # ax[3].plot(rtkRelTime, dist2base)
639  # ax[4].plot(rtkMiscTime, self.getData(d, miscDid, 'cycleSlipCount'))
640  # ax[0].legend(ncol=2)
641  for a in ax:
642  for b in a:
643  b.grid(True)
644 
645  self.saveFig(fig, 'rtk'+name+'Misc')
646  # except:
647  # print(RED + "problem plotting rtkStats: " + sys.exc_info()[0] + RESET)
648 
649  def rtkRel(self, fig=None):
650  # try:
651  n_plots = 3
652  if fig is None:
653  fig = plt.figure()
654 
655  ax = fig.subplots(3, 1, sharex=True)
656  fig.suptitle('RTK Rel - ' + os.path.basename(os.path.normpath(self.log.directory)))
657  self.configureSubplot(ax[0], 'GPS Base to Rover Heading', '')
658  self.configureSubplot(ax[1], 'GPS Base to Rover Distance', '')
659 
660  for i, d in enumerate(self.active_devs):
661  rtkRelTime = getTimeFromTowMs(self.getData(d, DID_GPS1_RTK_POS_REL, 'timeOfWeekMs'))
662  ax[0].plot(rtkRelTime, self.getData(d, DID_GPS1_RTK_POS_REL, 'baseToRoverHeading')*RAD2DEG)
663  ax[1].plot(rtkRelTime, self.getData(d, DID_GPS1_RTK_POS_REL, 'baseToRoverDistance'))
664 
665  for a in ax:
666  a.grid(True)
667 
668  self.saveFig(fig, 'rtkRel')
669 
670  def loadGyros(self, d):
671  return self.loadIMU(d, 0)
672 
673  def loadAccels(self, d):
674  return self.loadIMU(d, 1)
675 
676  def loadIMU(self, d, index): # 0 = gyro, 1 = accelerometer
677  imu0 = []
678  imu1 = []
679 
680  I1 = self.getData(d, DID_DUAL_IMU, 'I')[:, 0]
681  I2 = self.getData(d, DID_DUAL_IMU, 'I')[:, 1]
682  time = self.getData(d, DID_DUAL_IMU, 'time')
683  if np.shape(I1)[0] == 0:
684  I1 = self.getData(d, DID_DUAL_IMU_RAW, 'I')[:, 0]
685  I2 = self.getData(d, DID_DUAL_IMU_RAW, 'I')[:, 1]
686  time = self.getData(d, DID_DUAL_IMU_RAW, 'time')
687 
688  if np.shape(I1)[0] != 0: # DID_DUAL_IMU or DID_DUAL_IMU_RAW
689  dt = time[1:] - time[:-1]
690  dt = np.append(dt, dt[-1])
691  for i in range(0, len(I1)):
692  imu0.append(I1[i][index])
693  imu1.append(I2[i][index])
694  imu0 = np.array(imu0)
695  imu1 = np.array(imu1)
696  else: # DID_PREINTEGRATED_IMU
697  if index==0:
698  imu0 = self.getData(d, DID_PREINTEGRATED_IMU, 'theta1')
699  imu1 = self.getData(d, DID_PREINTEGRATED_IMU, 'theta2')
700  else:
701  imu0 = self.getData(d, DID_PREINTEGRATED_IMU, 'vel1')
702  imu1 = self.getData(d, DID_PREINTEGRATED_IMU, 'vel2')
703  time = self.getData(d, DID_PREINTEGRATED_IMU, 'time')
704  # dt = self.getData(d, DID_PREINTEGRATED_IMU, 'dt') # this doesn't account for LogInspector downsampling
705  dt = time[1:] - time[:-1]
706  dt = np.append(dt, dt[-1])
707  for i in range(3):
708  imu0[:, i] /= dt
709  imu1[:, i] /= dt
710 
711  return (imu0, imu1, time, dt)
712 
713  def imuPQR(self, fig=None):
714  if fig is None:
715  fig = plt.figure()
716  ax = fig.subplots(3, 2, sharex=True)
717  self.configureSubplot(ax[0, 0], 'Gyro0 P (deg/s)', 'sec')
718  self.configureSubplot(ax[0, 1], 'Gyro1 P (deg/s)', 'sec')
719  self.configureSubplot(ax[1, 0], 'Gyro0 Q (deg/s)', 'sec')
720  self.configureSubplot(ax[1, 1], 'Gyro1 Q (deg/s)', 'sec')
721  self.configureSubplot(ax[2, 0], 'Gyro0 R (deg/s)', 'sec')
722  self.configureSubplot(ax[2, 1], 'Gyro1 R (deg/s)', 'sec')
723  fig.suptitle('PQR - ' + os.path.basename(os.path.normpath(self.log.directory)))
724  for d in self.active_devs:
725  (pqr0, pqr1, time, dt) = self.loadGyros(d)
726 
727  for i in range(3):
728  ax[i, 0].plot(time, pqr0[:, 0], label=self.log.serials[d])
729  ax[i, 1].plot(time, pqr1[:, 1], label=self.log.serials[d])
730 
731  ax[0,0].legend(ncol=2)
732  for i in range(3):
733  for j in range(2):
734  ax[i,j].grid(True)
735  self.saveFig(fig, 'pqrIMU')
736 
737  def imuAcc(self, fig=None):
738  if fig is None:
739  fig = plt.figure()
740  ax = fig.subplots(3, 2, sharex=True)
741  self.configureSubplot(ax[0,0], 'Acc0 X (m/s^2)', 'sec')
742  self.configureSubplot(ax[0,1], 'Acc1 X (m/s^2)', 'sec')
743  self.configureSubplot(ax[1,0], 'Acc0 Y (m/s^2)', 'sec')
744  self.configureSubplot(ax[1,1], 'Acc1 Y (m/s^2)', 'sec')
745  self.configureSubplot(ax[2,0], 'Acc0 Z (m/s^2)', 'sec')
746  self.configureSubplot(ax[2,1], 'Acc1 Z (m/s^2)', 'sec')
747  fig.suptitle('Accelerometer - ' + os.path.basename(os.path.normpath(self.log.directory)))
748  for d in self.active_devs:
749  (acc0, acc1, time, dt) = self.loadAccels(d)
750 
751  for i in range(3):
752  ax[i, 0].plot(time, acc0[:, 0], label=self.log.serials[d])
753  ax[i, 1].plot(time, acc1[:, 1], label=self.log.serials[d])
754 
755  ax[0,0].legend(ncol=2)
756  for i in range(3):
757  for j in range(2):
758  ax[i,j].grid(True)
759  self.saveFig(fig, 'accIMU')
760 
761  def accelPSD(self, fig=None):
762  if fig is None:
763  fig = plt.figure()
764  ax = fig.subplots(3, 2, sharex=True)
765  self.configureSubplot(ax[0,0], 'AccX 0 PSD (dB (m/s^2)^2/Hz)', 'Hz')
766  self.configureSubplot(ax[0,1], 'AccX 1 PSD (dB (m/s^2)^2/Hz)', 'Hz')
767  self.configureSubplot(ax[1,0], 'AccY 0 PSD (dB (m/s^2)^2/Hz)', 'Hz')
768  self.configureSubplot(ax[1,1], 'AccY 1 PSD (dB (m/s^2)^2/Hz)', 'Hz')
769  self.configureSubplot(ax[2,0], 'AccZ 0 PSD (dB (m/s^2)^2/Hz)', 'Hz')
770  self.configureSubplot(ax[2,1], 'AccZ 1 PSD (dB (m/s^2)^2/Hz)', 'Hz')
771  fig.suptitle('Power Spectral Density - ' + os.path.basename(os.path.normpath(self.log.directory)))
772  for d in self.active_devs:
773  (acc0, acc1, time, dt) = self.loadAccels(d)
774 
775  N = time.size
776  psd0 = np.zeros((N//2, 3))
777  psd1 = np.zeros((N//2, 3))
778  # 1/T = frequency
779  Fs = 1 / np.mean(dt)
780  f = np.linspace(0, 0.5*Fs, N // 2)
781 
782  for i in range(3):
783  sp0 = np.fft.fft(acc0[:,i] / 9.8)
784  sp0 = sp0[:N // 2]
785  # psd = abssp*abssp
786  # freq = np.fft.fftfreq(time.shape[-1])
787 # np.append(psd0, [1/N/Fs * np.abs(sp0)**2], axis=1)
788  psd0[:,i] = 1/N/Fs * np.abs(sp0)**2
789  psd0[1:-1,i] = 2 * psd0[1:-1,i]
790  sp1 = np.fft.fft(acc1[:,i] / 9.8)
791  sp1 = sp1[:N // 2]
792  # psd = abssp*abssp
793  # freq = np.fft.fftfreq(time.shape[-1])
794 # np.append(psd0, [1/N/Fs * np.abs(sp0)**2], axis=1)
795  psd1[:,i] = 1/N/Fs * np.abs(sp1)**2
796  psd1[1:-1,i] = 2 * psd1[1:-1,i]
797 
798  for i in range(3):
799  # ax[i, 0].loglog(f, psd0[:, i])
800  # ax[i, 1].loglog(f, psd1[:, i])
801  ax[i, 0].plot(f, 10*np.log10(psd0[:, i]))
802  ax[i, 1].plot(f, 10*np.log10(psd1[:, i]))
803 
804  ax[0,0].legend(ncol=2)
805  for i in range(3):
806  for j in range(2):
807  ax[i,j].grid(True)
808  self.saveFig(fig, 'accelPSD')
809 
810  def gyroPSD(self, fig=None):
811  if fig is None:
812  fig = plt.figure()
813  ax = fig.subplots(3, 2, sharex=True)
814  self.configureSubplot(ax[0,0], 'Gyr0 X PSD (dB dps^2/Hz)', 'Hz')
815  self.configureSubplot(ax[0,1], 'Gyr1 X PSD (dB dps^2/Hz)', 'Hz')
816  self.configureSubplot(ax[1,0], 'Gyr0 Y PSD (dB dps^2/Hz)', 'Hz')
817  self.configureSubplot(ax[1,1], 'Gyr1 Y PSD (dB dps^2/Hz)', 'Hz')
818  self.configureSubplot(ax[2,0], 'Gyr0 Z PSD (dB dps^2/Hz)', 'Hz')
819  self.configureSubplot(ax[2,1], 'Gyr1 Z PSD (dB dps^2/Hz)', 'Hz')
820  fig.suptitle('Power Spectral Density - ' + os.path.basename(os.path.normpath(self.log.directory)))
821  for d in self.active_devs:
822  (pqr0, pqr1, time, dt) = self.loadGyros(d)
823 
824  N = time.size
825  Nhalf = N // 2 + 1
826  psd0 = np.zeros((Nhalf, 3))
827  psd1 = np.zeros((Nhalf, 3))
828  # 1/T = frequency
829  Fs = 1 / np.mean(dt)
830  f = np.linspace(0, 0.5*Fs, Nhalf)
831 
832  for i in range(3):
833  sp0 = np.fft.fft(pqr0[:,i] * 180.0/np.pi)
834  sp0 = sp0[:Nhalf]
835  # psd = abssp*abssp
836  # freq = np.fft.fftfreq(time.shape[-1])
837  # np.append(psd0, [1/N/Fs * np.abs(sp0)**2], axis=1)
838  psd0[:,i] = 1/N/Fs * np.abs(sp0)**2
839  psd0[1:-1,i] = 2 * psd0[1:-1,i]
840 
841  sp1 = np.fft.fft(pqr1[:,i] * 180.0/np.pi)
842  sp1 = sp1[:Nhalf]
843  # psd = abssp*abssp
844  # freq = np.fft.fftfreq(time.shape[-1])
845  # np.append(psd0, [1/N/Fs * np.abs(sp0)**2], axis=1)
846  psd1[:,i] = 1/N/Fs * np.abs(sp1)**2
847  psd1[1:-1,i] = 2 * psd1[1:-1,i]
848 
849  for i in range(3):
850  ax[i, 0].plot(f, 10*np.log10(psd0[:, i]))
851  ax[i, 1].plot(f, 10*np.log10(psd1[:, i]))
852 
853  ax[0,0].legend(ncol=2)
854  for i in range(3):
855  for j in range(2):
856  ax[i,j].grid(True)
857  self.saveFig(fig, 'gyroPSD')
858 
859  def magnetometer(self, fig=None):
860  if fig is None:
861  fig = plt.figure()
862  ax = fig.subplots(6, 1, sharex=True)
863 
864  self.configureSubplot(ax[0], 'Mag0 X', 'gauss')
865  self.configureSubplot(ax[1], 'Mag1 X', 'gauss')
866  self.configureSubplot(ax[2], 'Mag0 Y', 'gauss')
867  self.configureSubplot(ax[3], 'Mag1 Y', 'gauss')
868  self.configureSubplot(ax[4], 'Mag0 Z', 'gauss')
869  self.configureSubplot(ax[5], 'Mag1 Z', 'gauss')
870  fig.suptitle('Magnetometer - ' + os.path.basename(os.path.normpath(self.log.directory)))
871  for d in self.active_devs:
872  time0 = self.getData(d, DID_MAGNETOMETER_1, 'time') + self.getData(d, DID_GPS1_POS, 'towOffset')[-1]
873  mag0 = self.getData(d, DID_MAGNETOMETER_1, 'mag')
874  mag0x = mag0[:,0]
875  mag0y = mag0[:,1]
876  mag0z = mag0[:,2]
877  ax[0].plot(time0, mag0x, label=self.log.serials[d])
878  ax[2].plot(time0, mag0y)
879  ax[4].plot(time0, mag0z)
880 
881  ax[0].legend(ncol=2)
882  for a in ax:
883  a.grid(True)
884  self.saveFig(fig, 'magnetometer')
885 
886 
887  def temp(self, fig=None):
888  try:
889  if fig is None:
890  fig = plt.figure()
891  ax = fig.subplots(2, 1, sharex=True)
892  fig.suptitle('Temperature - ' + os.path.basename(os.path.normpath(self.log.directory)))
893 
894  for d in self.active_devs:
895  time = getTimeFromTowMs(self.getData(d, DID_SYS_PARAMS, 'timeOfWeekMs'))
896  ax[0].plot(time, self.getData(d, DID_SYS_PARAMS, 'imuTemp'), label=self.log.serials[d])
897  ax[1].plot(time, self.getData(d, DID_SYS_PARAMS, 'baroTemp'))
898  for a in ax:
899  a.grid(True)
900  self.saveFig(fig, 'Temp')
901  except:
902  print(RED + "problem plotting temp: " + sys.exc_info()[0] + RESET)
903 
904  def debugfArr(self, fig=None):
905  if fig is None:
906  fig = plt.figure()
907  ax = fig.subplots(5,2, sharex=True)
908  fig.suptitle('Debug float Array - ' + os.path.basename(os.path.normpath(self.log.directory)))
909  for d in self.active_devs:
910  debug_f = self.getData(d, DID_DEBUG_ARRAY, 'f')
911  for i in range(9):
912  ax[i%5, i//5].set_ylabel('f[' + str(i) +']')
913  ax[i%5, i//5].plot(debug_f[:,i], label=self.log.serials[d])
914  ax[0,0].legend(ncol=2)
915  for b in ax:
916  for a in b:
917  a.grid(True)
918 
919  def debugiArr(self, fig=None):
920  if fig is None:
921  fig = plt.figure()
922  ax = fig.subplots(5,2, sharex=True)
923  fig.suptitle('Debug int array - ' + os.path.basename(os.path.normpath(self.log.directory)))
924  for d in self.active_devs:
925  debug_i = self.getData(d, DID_DEBUG_ARRAY, 'i')
926  for i in range(9):
927  ax[i%5, i//5].set_ylabel('i[' + str(i) +']')
928  ax[i%5, i//5].plot(debug_i[:,i], label=self.log.serials[d])
929  ax[0,0].legend(ncol=2)
930  for b in ax:
931  for a in b:
932  a.grid(True)
933 
934  def debuglfArr(self, fig=None):
935  if fig is None:
936  fig = plt.figure()
937  ax = fig.subplots(3,1, sharex=True)
938  fig.suptitle('Debug double Array - ' + os.path.basename(os.path.normpath(self.log.directory)))
939  for d in self.active_devs:
940  debug_lf = self.getData(d, DID_DEBUG_ARRAY, 'lf')
941  for i in range(3):
942  ax[i].set_ylabel('lf[' + str(i) +']')
943  ax[i].plot(debug_lf[:,i], label=self.log.serials[d])
944  ax[0].legend(ncol=2)
945  for a in ax:
946  a.grid(True)
947 
948  def magDec(self, fig=None):
949  if fig is None:
950  fig = plt.figure()
951 
952  ax = fig.subplots(2, 1, sharex=True)
953  fig.suptitle('Magnetometer Declination - ' + os.path.basename(os.path.normpath(self.log.directory)))
954  self.configureSubplot(ax[0], 'Declination', 'deg')
955  self.configureSubplot(ax[1], 'Inclination', 'deg')
956 
957  for d in self.active_devs:
958  time = getTimeFromTow(self.getData(d, DID_INL2_STATES, 'timeOfWeek'))
959  declination = 180.0/np.pi * self.getData(d, DID_INL2_STATES, 'magDec')
960  inclination = 180.0/np.pi * self.getData(d, DID_INL2_STATES, 'magInc')
961  ax[0].plot(time, declination, label=self.log.serials[d])
962  ax[1].plot(time, inclination)
963  ax[0].legend(ncol=2)
964  self.saveFig(fig, 'magDec')
965  for a in ax:
966  a.grid(True)
967 
968  def deltatime(self, fig=None):
969  if fig is None:
970  fig = plt.figure()
971 
972  ax = fig.subplots(3, 1, sharex=True)
973  fig.suptitle('Timestamps - ' + os.path.basename(os.path.normpath(self.log.directory)))
974  self.configureSubplot(ax[0], 'INS dt', 's')
975  self.configureSubplot(ax[1], 'GPS dt', 's')
976  self.configureSubplot(ax[2], 'IMU dt', 's')
977 
978  for d in self.active_devs:
979  dtIns = self.getData(d, DID_INS_2, 'timeOfWeek')[1:] - self.getData(d, DID_INS_2, 'timeOfWeek')[0:-1]
980  dtIns = dtIns / self.d
981  timeIns = getTimeFromTow(self.getData(d, DID_INS_2, 'timeOfWeek')[1:])
982 
983  dtGps = 0.001*(self.getData(d, DID_GPS1_POS, 'timeOfWeekMs')[1:] - self.getData(d, DID_GPS1_POS, 'timeOfWeekMs')[0:-1])
984  dtGps = dtGps / self.d
985  timeGps = getTimeFromTowMs(self.getData(d, DID_GPS1_POS, 'timeOfWeekMs')[1:])
986 
987  dtImu = self.getData(d, DID_PREINTEGRATED_IMU, 'time')[1:] - self.getData(d, DID_PREINTEGRATED_IMU, 'time')[0:-1]
988  dtImu = dtImu / self.d
989 
990  towOffset = self.getData(d, DID_GPS1_POS, 'towOffset')
991  if np.size(towOffset) > 0:
992  towOffset = towOffset[-1]
993  else:
994  towOffset = 0
995  timeImu = getTimeFromTow(self.getData(d, DID_PREINTEGRATED_IMU, 'time')[1:] + towOffset)
996 
997  ax[0].plot(timeIns, dtIns, label=self.log.serials[d])
998  ax[1].plot(timeGps, dtGps)
999  ax[2].plot(timeImu, dtImu)
1000 
1001  self.setPlotYSpanMin(ax[0], 0.01)
1002  self.setPlotYSpanMin(ax[1], 0.01)
1003  self.setPlotYSpanMin(ax[2], 0.05)
1004 
1005  ax[0].legend(ncol=2)
1006  for a in ax:
1007  a.grid(True)
1008  self.saveFig(fig, 'deltatime')
1009 
1010  def gpsRawTime(self, fig=None):
1011  if fig is None:
1012  fig = plt.figure()
1013 
1014  ax = fig.subplots(6, 1, sharex=True)
1015  fig.suptitle('Timestamps - ' + os.path.basename(os.path.normpath(self.log.directory)))
1016  self.configureSubplot(ax[0], 'GPS1 Raw dt', 's')
1017  self.configureSubplot(ax[1], 'GPS2 Raw dt', 's')
1018  self.configureSubplot(ax[2], 'GPS Base Raw dt', 's')
1019  self.configureSubplot(ax[3], 'GPS1 Raw Number of Satellites Observed', 's')
1020  self.configureSubplot(ax[4], 'GPS2 Raw Number of Satellites Observed', 's')
1021  self.configureSubplot(ax[5], 'GPS Base Raw Number of Satellites Observed', 's')
1022 
1023  for d in self.active_devs:
1024  N1 = len(self.log.data[d, DID_GPS1_RAW][0])
1025  N2 = len(self.log.data[d, DID_GPS2_RAW][0])
1026  NB = len(self.log.data[d, DID_GPS_BASE_RAW][0])
1027  tgps1 = np.zeros(N1)
1028  nsat1 = np.zeros(N1)
1029  tgps2 = np.zeros(N2)
1030  nsat2 = np.zeros(N2)
1031  tgpsB = np.zeros(NB)
1032  nsatB = np.zeros(NB)
1033  cnt = 0
1034  for iobs in range(N1):
1035  ns = round(len(self.log.data[d, DID_GPS1_RAW][0][iobs]) * 0.5) # 0.5 because there is a bug that pads half of the data with zeros
1036  t0 = self.log.data[d, DID_GPS1_RAW][0][iobs]['time']['time'][-1] + \
1037  self.log.data[d, DID_GPS1_RAW][0][iobs]['time']['sec'][-1]
1038  nsat1[cnt] = nsat1[cnt] + ns
1039  tgps1[cnt] = t0
1040  if iobs < N1 - 1:
1041  t1 = self.log.data[d, DID_GPS1_RAW][0][iobs + 1]['time']['time'][-1] + \
1042  self.log.data[d, DID_GPS1_RAW][0][iobs + 1]['time']['sec'][-1]
1043  if t1 > t0 + 0.01:
1044  cnt = cnt + 1
1045  tgps1 = tgps1[0: cnt + 1]
1046  nsat1 = nsat1[0: cnt + 1]
1047  cnt = 0
1048  for iobs in range(N2):
1049  ns = round(len(self.log.data[d, DID_GPS2_RAW][0][iobs]) * 0.5) # 0.5 because there is a bug that pads half of the data with zeros
1050  t0 = self.log.data[d, DID_GPS2_RAW][0][iobs]['time']['time'][-1] + \
1051  self.log.data[d, DID_GPS2_RAW][0][iobs]['time']['sec'][-1]
1052  nsat2[cnt] = nsat2[cnt] + ns
1053  tgps2[cnt] = t0
1054  if iobs < N2 - 1:
1055  t1 = self.log.data[d, DID_GPS2_RAW][0][iobs + 1]['time']['time'][-1] + \
1056  self.log.data[d, DID_GPS2_RAW][0][iobs + 1]['time']['sec'][-1]
1057  if t1 > t0 + 0.01:
1058  cnt = cnt + 1
1059  tgps2 = tgps2[0: cnt + 1]
1060  nsat2 = nsat2[0: cnt + 1]
1061  cnt = 0
1062  for iobs in range(NB):
1063  ns = round(len(self.log.data[d, DID_GPS_BASE_RAW][0][iobs]) * 0.5) # 0.5 because there is a bug that pads half of the data with zeros
1064  t0 = self.log.data[d, DID_GPS_BASE_RAW][0][iobs]['time']['time'][-1] + \
1065  self.log.data[d, DID_GPS_BASE_RAW][0][iobs]['time']['sec'][-1]
1066  nsatB[cnt] = nsatB[cnt] + ns
1067  tgpsB[cnt] = t0
1068  if iobs < NB - 1:
1069  t1 = self.log.data[d, DID_GPS_BASE_RAW][0][iobs + 1]['time']['time'][-1] + \
1070  self.log.data[d, DID_GPS_BASE_RAW][0][iobs + 1]['time']['sec'][-1]
1071  if t1 > t0 + 0.01:
1072  cnt = cnt + 1
1073  tgpsB = tgpsB[0: cnt + 1]
1074  nsatB = nsatB[0: cnt + 1]
1075  dtGps1 = tgps1[1:] - tgps1[0:-1]
1076 # dtGps1 = dtGps1 / self.d
1077  dtGps2 = tgps2[1:] - tgps2[0:-1]
1078 # dtGps2 = dtGps2 / self.d
1079  dtGpsB = tgpsB[1:] - tgpsB[0:-1]
1080 
1081  ax[0].plot(tgps1[1:], dtGps1, label=self.log.serials[d])
1082  ax[1].plot(tgps2[1:], dtGps2)
1083  ax[2].plot(tgpsB[1:], dtGpsB)
1084  ax[3].plot(tgps1, nsat1, label=self.log.serials[d])
1085  ax[4].plot(tgps2, nsat2)
1086  ax[5].plot(tgpsB, nsatB)
1087 
1088  self.setPlotYSpanMin(ax[0], 2.0)
1089  self.setPlotYSpanMin(ax[1], 2.0)
1090  self.setPlotYSpanMin(ax[2], 2.0)
1091  self.setPlotYSpanMin(ax[3], 25)
1092  self.setPlotYSpanMin(ax[4], 25)
1093  self.setPlotYSpanMin(ax[5], 25)
1094 
1095  ax[0].legend(ncol=2)
1096  for a in ax:
1097  a.grid(True)
1098  self.saveFig(fig, 'gpsRawTime')
1099 
1100  def ekfBiases(self, fig=None):
1101  if fig is None:
1102  fig = plt.figure()
1103  ax = fig.subplots(4, 2, sharex=True)
1104  self.configureSubplot(ax[0,0], 'bias P', 'deg/s')
1105  self.configureSubplot(ax[1,0], 'bias Q', 'deg/s')
1106  self.configureSubplot(ax[2,0], 'bias R', 'deg/s')
1107  self.configureSubplot(ax[3,0], 'bias Barometer', 'm')
1108 
1109  self.configureSubplot(ax[0,1], 'bias acc X', 'm/s^2')
1110  self.configureSubplot(ax[1,1], 'bias acc Y', 'm/s^2')
1111  self.configureSubplot(ax[2,1], 'bias acc Z', 'm/s^2')
1112  fig.suptitle('EKF Biases - ' + os.path.basename(os.path.normpath(self.log.directory)))
1113  for d in self.active_devs:
1114  time = getTimeFromTow(self.getData(d, DID_INL2_STATES, 'timeOfWeek'))
1115  ax[0,0].plot(time, self.getData(d, DID_INL2_STATES, 'biasPqr')[:, 0]*180.0/np.pi, label=self.log.serials[d])
1116  ax[1,0].plot(time, self.getData(d, DID_INL2_STATES, 'biasPqr')[:, 1]*180.0/np.pi)
1117  ax[2,0].plot(time, self.getData(d, DID_INL2_STATES, 'biasPqr')[:, 2]*180.0/np.pi)
1118  ax[3,0].plot(time, self.getData(d, DID_INL2_STATES, 'biasBaro'), label=self.log.serials[d])
1119 
1120  ax[0,1].plot(time, self.getData(d, DID_INL2_STATES, 'biasAcc')[:, 0], label=self.log.serials[d])
1121  ax[1,1].plot(time, self.getData(d, DID_INL2_STATES, 'biasAcc')[:, 1])
1122  ax[2,1].plot(time, self.getData(d, DID_INL2_STATES, 'biasAcc')[:, 2])
1123 
1124  ax[0,0].legend(ncol=2)
1125  for a in ax:
1126  for b in a:
1127  b.grid(True)
1128  self.saveFig(fig, 'ekfBiases')
1129 
1130  def rtkResiduals(self, type, page, fig=None):
1131  if fig is None:
1132  fig = plt.figure()
1133 
1134  if type == 'phase':
1135  did = DID_RTK_PHASE_RESIDUAL
1136  elif type == 'code':
1137  did = DID_RTK_CODE_RESIDUAL
1138 
1139  sat_ids = np.unique(self.log.data[0, did]['sat_id_j'])
1140  sat_ids = sat_ids[sat_ids != 0][page*6:(page+1)*6]
1141 
1142  ax = fig.subplots(6, 1, sharex=True)
1143  fig.suptitle(type + ' Residuals Page ' + str(page+1) + ' - ' + os.path.basename(os.path.normpath(self.log.directory)))
1144 
1145  for i, id in enumerate(sat_ids):
1146  if id == 0: continue
1147  ax[i].set_ylabel(str(id))
1148  for d in self.active_devs:
1149  idx = np.where(self.getData(d, did, 'sat_id_j') == id)
1150  time_idx = idx[0]
1151  sat_state_idx = idx[1]
1152  time = np.array(getTimeFromGTime(self.getData(d, did, 'time')))[time_idx]
1153  residuals = self.getData(d, did, 'v')[time_idx, sat_state_idx]
1154  residuals[np.abs(residuals) > 1e6] = np.nan
1155  ax[i].plot(time, residuals, label=self.log.serials[d])
1156  ax[0].legend(ncol=2)
1157 
1158  def rtkDebug(self, fig=None):
1159  if fig is None:
1160  fig = plt.figure()
1161 
1162  fields = list(self.log.data[0, DID_RTK_DEBUG].dtype.names)
1163  fields.remove('time')
1164  num_plots = 0
1165  for field in fields:
1166  dat = self.log.data[0, DID_RTK_DEBUG][field][0]
1167  if isinstance(dat, np.ndarray):
1168  num_plots += len(dat)
1169  else:
1170  num_plots += 1
1171 
1172  cols = 4
1173  rows = math.ceil(num_plots/float(cols))
1174  ax = fig.subplots(rows, cols, sharex=True)
1175  fig.suptitle('RTK Debug Counters - ' + os.path.basename(os.path.normpath(self.log.directory)))
1176 
1177  for d in self.active_devs:
1178  time = np.array(getTimeFromGTime(self.getData(d, DID_RTK_DEBUG, 'time')))
1179  valid = time > datetime.datetime.strptime('2017', "%Y")
1180 
1181  # Use index rather than time
1182  if 0:
1183  time = np.arange(0, len(time))
1184 
1185  i = 0
1186  for field in fields:
1187  data = self.getData(d, DID_RTK_DEBUG, field)[valid]
1188  if (len(data) == 0):
1189  continue
1190  if isinstance(data[0], np.ndarray):
1191  for j in range(len(data[0])):
1192  ax[ i%rows, i//rows].set_title(field + "_" + str(j))
1193  ax[ i % rows, i // rows ].plot(time[valid], data[:,j], label=self.log.serials[d])
1194  i += 1
1195  else:
1196  ax[i % rows, i // rows].set_title(field)
1197  ax[i % rows, i // rows].title.set_fontsize(8)
1198  # for item in ax[i % rows, i // rows].get_yticklabels():
1199  # item.set_fontsize(8)
1200  ax[i % rows, i // rows].plot(time[valid], data, label=self.log.serials[d])
1201  i += 1
1202  ax[0,0].legend(ncol=2)
1203 
1204  def rtkDebug2(self, fig=None):
1205  if fig is None:
1206  fig = plt.figure()
1207 
1208  ax = fig.subplots(6, 4, sharex=True)
1209 
1210  #max_num_biases = 22 #np.array(self.getData(self.active_devs[0], DID_RTK_DEBUG_2, 'num_biases'))
1211  max_num_biases = self.getData(0, DID_RTK_DEBUG_2, 'num_biases')[-1]
1212  for r in range(0,6):
1213  for c in range(0,4):
1214  self.configureSubplot(ax[r,c], '', '')
1215 
1216  fig.suptitle('RTK Debug2 - ' + os.path.basename(os.path.normpath(self.log.directory)))
1217  for d in self.active_devs:
1218  time = np.array(getTimeFromGTime(self.getData(d, DID_RTK_DEBUG_2, 'time')))
1219  ib = 0
1220  for r in range(0, 6):
1221  for c in range(0, 4):
1222  if ib < max_num_biases:
1223  ax[r,c].plot(time, self.getData(d, DID_RTK_DEBUG_2, 'satBiasFloat')[:, c + r * 4], label=self.log.serials[d])
1224  r1 = r
1225  c1 = c
1226  ib = ib + 1
1227 
1228  # Show serial numbers
1229  ax[r1,c1].legend(ncol=2)
1230 
1231  for a in ax:
1232  for b in a:
1233  b.grid(True)
1234 
1235  def rtkDebug2Sat(self, fig=None):
1236  if fig is None:
1237  fig = plt.figure()
1238 
1239  ax = fig.subplots(6, 4, sharex=True)
1240 
1241  max_num_biases = self.getData(0, DID_RTK_DEBUG_2, 'num_biases')[-1]
1242  for r in range(0,6):
1243  for c in range(0,4):
1244  self.configureSubplot(ax[r,c], '', '')
1245 
1246  fig.suptitle('RTK Debug2 - Sat# - ' + os.path.basename(os.path.normpath(self.log.directory)))
1247  for d in self.active_devs:
1248  time = np.array(getTimeFromGTime(self.getData(d, DID_RTK_DEBUG_2, 'time')))
1249  ib = 0
1250  for r in range(0, 6):
1251  for c in range(0, 4):
1252  if ib < max_num_biases:
1253  ax[r,c].plot(time, self.getData(d, DID_RTK_DEBUG_2, 'sat')[:, c + r * 4], label=self.log.serials[d])
1254  r1 = r
1255  c1 = c
1256  ib = ib + 1
1257 
1258  # Show serial numbers
1259  ax[r1,c1].legend(ncol=2)
1260 
1261  for a in ax:
1262  for b in a:
1263  b.grid(True)
1264 
1265  def rtkDebug2Std(self, fig=None):
1266  if fig is None:
1267  fig = plt.figure()
1268 
1269  ax = fig.subplots(6, 4, sharex=True)
1270 
1271  max_num_biases = self.getData(0, DID_RTK_DEBUG_2, 'num_biases')[-1]
1272  for r in range(0,6):
1273  for c in range(0,4):
1274  self.configureSubplot(ax[r,c], '', '')
1275 
1276  fig.suptitle('RTK Debug 2 - Sat Bias Std - ' + os.path.basename(os.path.normpath(self.log.directory)))
1277  for d in self.active_devs:
1278  time = np.array(getTimeFromGTime(self.getData(d, DID_RTK_DEBUG_2, 'time')))
1279  ib = 0
1280  for r in range(0, 6):
1281  for c in range(0, 4):
1282  if ib < max_num_biases:
1283  ax[r,c].plot(time, self.getData(d, DID_RTK_DEBUG_2, 'satBiasStd')[:, c + r * 4], label=self.log.serials[d])
1284  r1 = r
1285  c1 = c
1286  ib = ib + 1
1287 
1288  # Show serial numbers
1289  ax[r1,c1].legend(ncol=2)
1290 
1291  for a in ax:
1292  for b in a:
1293  b.grid(True)
1294 
1295  def rtkDebug2Lock(self, fig=None):
1296  if fig is None:
1297  fig = plt.figure()
1298 
1299  ax = fig.subplots(6, 4, sharex=True)
1300 
1301  max_num_biases = self.getData(0, DID_RTK_DEBUG_2, 'num_biases')[-1]
1302  for r in range(0,6):
1303  for c in range(0,4):
1304  self.configureSubplot(ax[r,c], '', '')
1305 
1306  fig.suptitle('RTK Debug 2 - Lock Count - ' + os.path.basename(os.path.normpath(self.log.directory)))
1307  for d in self.active_devs:
1308  time = np.array(getTimeFromGTime(self.getData(d, DID_RTK_DEBUG_2, 'time')))
1309  ib = 0
1310  for r in range(0, 6):
1311  for c in range(0, 4):
1312  if ib < max_num_biases:
1313  ax[r,c].plot(time, self.getData(d, DID_RTK_DEBUG_2, 'satLockCnt')[:, c + r * 4], label=self.log.serials[d])
1314  r1 = r
1315  c1 = c
1316  ib = ib + 1
1317 
1318  # Show serial numbers
1319  ax[r1,c1].legend(ncol=2)
1320 
1321  for a in ax:
1322  for b in a:
1323  b.grid(True)
1324 
1325  def wheelEncoder(self, fig=None):
1326  if fig is None:
1327  fig = plt.figure()
1328 
1329  fig.suptitle('Wheel Encoder - ' + os.path.basename(os.path.normpath(self.log.directory)))
1330  ax = fig.subplots(4, 1, sharex=True)
1331  titles = ['Left Wheel Angle', 'Right Wheel Angle', 'Left Wheel Velocity', 'Right Wheel Velocity']
1332  fields = ['theta_l', 'theta_r', 'omega_l', 'omega_r']
1333 
1334  for d in self.active_devs:
1335  time = np.array(getTimeFromTow(self.getData(d, DID_WHEEL_ENCODER, 'timeOfWeek')))
1336  for i, a in enumerate(ax):
1337  a.plot(time, self.getData(d, DID_WHEEL_ENCODER, fields[i]), label=self.log.serials[d])
1338  if i == 0:
1339  a.legend(ncol=2)
1340 
1341  for i, a in enumerate(ax):
1342  a.set_ylabel(fields[i])
1343  a.set_title(titles[i])
1344  a.grid(True)
1345 
1346 
1347 
1348 
1349  def showFigs(self):
1350  if self.show:
1351  plt.show()
1352 
1353 
1354 if __name__ == '__main__':
1355  np.set_printoptions(linewidth=200)
1356  home = expanduser("~")
1357  file = open(home + "/Documents/Inertial_Sense/config.yaml", 'r')
1358  config = yaml.load(file)
1359  directory = config["directory"]
1360  directory = "/home/superjax/Code/IS-src/cpp/SDK/CLTool/build/IS_logs"
1361  directory = r"C:\Users\quaternion\Downloads\20181218_Compass_Drive\20181218 Compass Drive\20181218_101023"
1362  serials = config['serials']
1363 
1364  log2 = Log()
1365  log2.load(directory, serials)
1366 
1367  plotter = logPlot(True, True, 'svg', log2)
1368  plotter.setDownSample(5)
1369  # plotter.deltatime()
1370  # plotter.debugfArr()
1371  # plotter.debugiArr()
1372  # plotter.debuglfArr()
1373  # plotter.gpsStats()
1374  # plotter.rtkStats()
1375  # plotter.insStatus()
1376  # plotter.hdwStatus()
1377  # plotter.temp()
1378  # plotter.att()
1379  # plotter.lla()
1380  # plotter.uvw()
1381  # plotter.ned()
1382  # plotter.nedMap()
1383  # plotter.magDec()
1384  plotter.rtkDebug()
1385  #plotter.wheelEncoder()
1386 
1387  plotter.showFigs()
python.pylib.pose.quatRotVectArray
def quatRotVectArray(q, v)
Definition: pose.py:95
python.logInspector.logPlotter.logPlot.rtkResiduals
def rtkResiduals(self, type, page, fig=None)
Definition: logPlotter.py:1130
python.pylib.pose.lla2ned
def lla2ned(llaRef, lla)
Definition: pose.py:429
python.logInspector.logPlotter.logPlot.setPlotYSpanMin
def setPlotYSpanMin(self, ax, limit)
Definition: logPlotter.py:73
python.logInspector.logPlotter.logPlot.getData
def getData(self, dev, DID, field)
Definition: logPlotter.py:67
if
if(udd_ctrl_interrupt())
Definition: usbhs_device.c:688
python.logInspector.logPlotter.logPlot.rtkMisc
def rtkMisc(self, name, miscDid, fig=None)
Definition: logPlotter.py:599
Catch::Generators::range
GeneratorWrapper< T > range(T const &start, T const &end, T const &step)
Definition: catch.hpp:4141
python.logInspector.logPlotter.logPlot.gpsStats
def gpsStats(self, fig=None, did_gps_pos=DID_GPS1_POS)
Definition: logPlotter.py:480
python.logInspector.logPlotter.logPlot.__init__
def __init__(self, show, save, format, log)
Definition: logPlotter.py:32
python.pylib.pose.rotate_ecef2ned
def rotate_ecef2ned(latlon)
Definition: pose.py:516
python.logInspector.logPlotter.logPlot.rtkStats
def rtkStats(self, name, relDid, fig=None)
Definition: logPlotter.py:545
python.logInspector.logPlotter.logPlot.configureSubplot
def configureSubplot(self, ax, title, xlabel)
Definition: logPlotter.py:52
python.logInspector.logPlotter.logPlot.drawNEDMapArrow
def drawNEDMapArrow(self, ax, ned, heading)
Definition: logPlotter.py:109
python.pylib.pose.quat2eulerArray
def quat2eulerArray(q)
Definition: pose.py:134

inertial_sense_ros
Author(s):
autogenerated on Sun Feb 28 2021 03:17:57