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 imuPQR(self, fig=None):
671  if fig is None:
672  fig = plt.figure()
673  ax = fig.subplots(6, 1, sharex=True)
674  self.configureSubplot(ax[0], 'Roll Rate 1', 'deg/s')
675  self.configureSubplot(ax[1], 'Roll Rate 2', 'deg/s')
676  self.configureSubplot(ax[2], 'Pitch Rate 1', 'deg/s')
677  self.configureSubplot(ax[3], 'Pitch Rate 2', 'deg/s')
678  self.configureSubplot(ax[4], 'Yaw Rate 1', 'deg/s')
679  self.configureSubplot(ax[5], 'Yaw Rate 2', 'deg/s')
680  fig.suptitle('PQR - ' + os.path.basename(os.path.normpath(self.log.directory)))
681  # ax[0].set_ylim([-.05, .05])
682  # ax[1].set_ylim([-.05, .05])
683  # ax[2].set_ylim([-.05, .05])
684  # ax[3].set_ylim([-.05, .05])
685  # ax[4].set_ylim([-.05, .05])
686  # ax[5].set_ylim([-.05, .05])
687  for d in self.active_devs:
688  #imu = self.getData(d, DID_DUAL_IMU, 'I')
689  I1 = self.getData(d, DID_DUAL_IMU, 'I')[:,0]
690  I2 = self.getData(d, DID_DUAL_IMU, 'I')[:,1]
691  p0 = []
692  q0 = []
693  r0 = []
694  p1 = []
695  q1 = []
696  r1 = []
697  if np.shape(I1)[0] != 0:
698  time = self.getData(d, DID_DUAL_IMU, 'time') # + self.getData(d, DID_GPS1_POS, 'towOffset')[-1]
699  for i in range(0, len(I1)):
700  p0.append(I1[i][0][0]*RAD2DEG)
701  q0.append(I1[i][0][1]*RAD2DEG)
702  r0.append(I1[i][0][2]*RAD2DEG)
703  p1.append(I2[i][0][0]*RAD2DEG)
704  q1.append(I2[i][0][1]*RAD2DEG)
705  r1.append(I2[i][0][2]*RAD2DEG)
706  else:
707  time = self.getData(d, DID_PREINTEGRATED_IMU, 'time') # + self.getData(d, DID_GPS1_POS, 'towOffset')[-1]
708  dt = self.getData(d, DID_PREINTEGRATED_IMU, 'dt')
709  pqr0 = self.getData(d, DID_PREINTEGRATED_IMU, 'theta1')
710  pqr1 = self.getData(d, DID_PREINTEGRATED_IMU, 'theta2')
711  p0 = pqr0[:,0] / dt*RAD2DEG
712  q0 = pqr0[:,1] / dt*RAD2DEG
713  r0 = pqr0[:,2] / dt*RAD2DEG
714  p1 = pqr1[:,0] / dt*RAD2DEG
715  q1 = pqr1[:,1] / dt*RAD2DEG
716  r1 = pqr1[:,2] / dt*RAD2DEG
717 
718  ax[0].plot(time, p0, label=self.log.serials[d])
719  ax[1].plot(time, p1, label=self.log.serials[d])
720  ax[2].plot(time, q0)
721  ax[3].plot(time, q1)
722  ax[4].plot(time, r0)
723  ax[5].plot(time, r1)
724  ax[0].legend(ncol=2)
725  for a in ax:
726  a.grid(True)
727  self.saveFig(fig, 'pqrIMU')
728 
729  def imuAcc(self, fig=None):
730  if fig is None:
731  fig = plt.figure()
732  ax = fig.subplots(6, 1, sharex=True)
733  self.configureSubplot(ax[0], 'Acc X 0', 'm/s^2')
734  self.configureSubplot(ax[1], 'Acc X 1', 'm/s^2')
735  self.configureSubplot(ax[2], 'Acc Y 0', 'm/s^2')
736  self.configureSubplot(ax[3], 'Acc Y 1', 'm/s^2')
737  self.configureSubplot(ax[4], 'Acc Z 0', 'm/s^2')
738  self.configureSubplot(ax[5], 'Acc Z 1', 'm/s^2')
739  fig.suptitle('Accelerometer - ' + os.path.basename(os.path.normpath(self.log.directory)))
740  for d in self.active_devs:
741  I1 = self.getData(d, DID_DUAL_IMU, 'I')[:,0]
742  I2 = self.getData(d, DID_DUAL_IMU, 'I')[:,1]
743  acc0x = []
744  acc0y = []
745  acc0z = []
746  acc1x = []
747  acc1y = []
748  acc1z = []
749 
750  if np.shape(I1)[0] != 0:
751  if len(self.getData(d, DID_GPS1_POS, 'towOffset')) == 0:
752  time = self.getData(d, DID_DUAL_IMU, 'time')
753  else:
754  time = self.getData(d, DID_DUAL_IMU, 'time') + self.getData(d, DID_GPS1_POS, 'towOffset')[-1]
755  for i in range(0, len(I1)):
756  acc0x.append(I1[i][1][0])
757  acc0y.append(I1[i][1][1])
758  acc0z.append(I1[i][1][2])
759  acc1x.append(I2[i][1][0])
760  acc1y.append(I2[i][1][1])
761  acc1z.append(I2[i][1][2])
762  else:
763  if len(self.getData(d, DID_GPS1_POS, 'towOffset')) == 0:
764  time = self.getData(d, DID_PREINTEGRATED_IMU, 'time')
765  else:
766  time = self.getData(d, DID_PREINTEGRATED_IMU, 'time') + self.getData(d, DID_GPS1_POS, 'towOffset')[-1]
767  dt = self.getData(d, DID_PREINTEGRATED_IMU, 'dt')
768  acc0 = self.getData(d, DID_PREINTEGRATED_IMU, 'vel1')
769  acc1 = self.getData(d, DID_PREINTEGRATED_IMU, 'vel2')
770  acc0x = acc0[:,0] / dt
771  acc0y = acc0[:,1] / dt
772  acc0z = acc0[:,2] / dt
773  acc1x = acc1[:,0] / dt
774  acc1y = acc1[:,1] / dt
775  acc1z = acc1[:,2] / dt
776 
777  ax[0].plot(time, acc0x, label=self.log.serials[d])
778  ax[1].plot(time, acc1x)
779  ax[2].plot(time, acc0y)
780  ax[3].plot(time, acc1y)
781  ax[4].plot(time, acc0z)
782  ax[5].plot(time, acc1z)
783  ax[0].legend(ncol=2)
784  for a in ax:
785  a.grid(True)
786  self.saveFig(fig, 'accIMU')
787 
788  def imuPSD(self, fig=None):
789  if fig is None:
790  fig = plt.figure()
791  ax = fig.subplots(3, 2, sharex=True)
792  self.configureSubplot(ax[0,0], 'AccX 0 ((m/s^2)^2)', 'Hz')
793  self.configureSubplot(ax[1,0], 'AccX 1 ((m/s^2)^2)', 'Hz')
794  self.configureSubplot(ax[0,0], 'AccX 0 ((m/s^2)^2)', 'Hz')
795  self.configureSubplot(ax[0,1], 'AccX 1 ((m/s^2)^2)', 'Hz')
796  self.configureSubplot(ax[1,0], 'AccY 0 ((m/s^2)^2)', 'Hz')
797  self.configureSubplot(ax[1,1], 'AccY 1 ((m/s^2)^2)', 'Hz')
798  self.configureSubplot(ax[2,0], 'AccZ 0 ((m/s^2)^2)', 'Hz')
799  self.configureSubplot(ax[2,1], 'AccZ 1 ((m/s^2)^2)', 'Hz')
800  # self.configureSubplot(ax[2], 'PQR 0', '(rad/s)^2')
801  # self.configureSubplot(ax[2], 'PQR 1', '(rad/s)^2')
802  fig.suptitle('Power Spectrum Density - ' + os.path.basename(os.path.normpath(self.log.directory)))
803  for d in self.active_devs:
804  I1 = self.getData(d, DID_DUAL_IMU, 'I')[:,0]
805  I2 = self.getData(d, DID_DUAL_IMU, 'I')[:,1]
806  acc0x = []
807  acc0y = []
808  acc0z = []
809  acc1x = []
810  acc1y = []
811  acc1z = []
812 
813  if np.shape(I1)[0] != 0:
814  time = self.getData(d, DID_DUAL_IMU, 'time')
815 
816  for i in range(0, len(I1)):
817  acc0x.append(I1[i][1][0])
818  acc0y.append(I1[i][1][1])
819  acc0z.append(I1[i][1][2])
820  acc1x.append(I2[i][1][0])
821  acc1y.append(I2[i][1][1])
822  acc1z.append(I2[i][1][2])
823 
824  sp = np.fft.fft(acc0x)
825  freq = np.fft.fftfreq(time.shape[-1])
826  # plt.plot(freq, sp.real, freq, sp.imag)
827 
828  else:
829  time = self.getData(d, DID_PREINTEGRATED_IMU, 'time')
830  dt = self.getData(d, DID_PREINTEGRATED_IMU, 'dt')
831  acc0 = self.getData(d, DID_PREINTEGRATED_IMU, 'vel1')
832  acc1 = self.getData(d, DID_PREINTEGRATED_IMU, 'vel2')
833  acc0x = acc0[:,0] / dt
834  acc0y = acc0[:,1] / dt
835  acc0z = acc0[:,2] / dt
836  acc1x = acc1[:,0] / dt
837  acc1y = acc1[:,1] / dt
838  acc1z = acc1[:,2] / dt
839  N = time.size
840  psd0 = np.zeros((N//2, 3))
841  psd1 = np.zeros((N//2, 3))
842  # 1/T = frequency
843  Fs = 1 / np.mean(dt)
844  f = np.linspace(0, 0.5*Fs, N // 2)
845 
846  for i in range(3):
847  sp0 = np.fft.fft(acc0[:,i] / dt / 9.8)
848  sp0 = sp0[:N // 2]
849  # psd = abssp*abssp
850  # freq = np.fft.fftfreq(time.shape[-1])
851 # np.append(psd0, [1/N/Fs * np.abs(sp0)**2], axis=1)
852  psd0[:,i] = 1/N/Fs * np.abs(sp0)**2
853  psd0[1:-1,i] = 2 * psd0[1:-1,i]
854  sp1 = np.fft.fft(acc1[:,i] / dt / 9.8)
855  sp1 = sp1[:N // 2]
856  # psd = abssp*abssp
857  # freq = np.fft.fftfreq(time.shape[-1])
858 # np.append(psd0, [1/N/Fs * np.abs(sp0)**2], axis=1)
859  psd1[:,i] = 1/N/Fs * np.abs(sp1)**2
860  psd1[1:-1,i] = 2 * psd1[1:-1,i]
861 
862 
863  # plt.plot(freq, sp.real, freq, sp.imag)
864 
865  ax[0,0].loglog(f, psd0[:,0])
866  ax[1,0].loglog(f, psd0[:,1])
867  ax[2,0].loglog(f, psd0[:,2])
868  ax[0,1].loglog(f, psd1[:,0])
869  ax[1,1].loglog(f, psd1[:,1])
870  ax[2,1].loglog(f, psd1[:,2])
871 
872  # Set x limits
873  xlim = [10, 500]
874  ax[0,0].set_xlim(xlim)
875  ax[1,0].set_xlim(xlim)
876  ax[2,0].set_xlim(xlim)
877  ax[0,1].set_xlim(xlim)
878  ax[1,1].set_xlim(xlim)
879  ax[2,1].set_xlim(xlim)
880 
881  # ax[0].plot(freq, sp.real, freq, sp.imag, label=self.log.serials[d])
882  # ax[1].plot(time, acc1x)
883  # ax[2].plot(time, acc0y)
884  # ax[3].plot(time, acc1y)
885  # ax[4].plot(time, acc0z)
886  # ax[5].plot(time, acc1z)
887  ax[0,0].legend(ncol=2)
888  for i in range(3):
889  for j in range(2):
890  ax[i,j].grid(True)
891  self.saveFig(fig, 'imuPSD')
892 
893  def magnetometer(self, fig=None):
894  if fig is None:
895  fig = plt.figure()
896  ax = fig.subplots(6, 1, sharex=True)
897 
898  self.configureSubplot(ax[0], 'Mag X 0', 'gauss')
899  self.configureSubplot(ax[1], 'Mag X 1', 'gauss')
900  self.configureSubplot(ax[2], 'Mag Y 0', 'gauss')
901  self.configureSubplot(ax[3], 'Mag Y 1', 'gauss')
902  self.configureSubplot(ax[4], 'Mag Z 0', 'gauss')
903  self.configureSubplot(ax[5], 'Mag Z 1', 'gauss')
904  fig.suptitle('Magnetometer - ' + os.path.basename(os.path.normpath(self.log.directory)))
905  for d in self.active_devs:
906  time0 = self.getData(d, DID_MAGNETOMETER_1, 'time') + self.getData(d, DID_GPS1_POS, 'towOffset')[-1]
907  mag0 = self.getData(d, DID_MAGNETOMETER_1, 'mag')
908  mag0x = mag0[:,0]
909  mag0y = mag0[:,1]
910  mag0z = mag0[:,2]
911  ax[0].plot(time0, mag0x, label=self.log.serials[d])
912  ax[2].plot(time0, mag0y)
913  ax[4].plot(time0, mag0z)
914 
915  ax[0].legend(ncol=2)
916  for a in ax:
917  a.grid(True)
918  self.saveFig(fig, 'magnetometer')
919 
920 
921  def temp(self, fig=None):
922  try:
923  if fig is None:
924  fig = plt.figure()
925  ax = fig.subplots(2, 1, sharex=True)
926  fig.suptitle('Temperature - ' + os.path.basename(os.path.normpath(self.log.directory)))
927 
928  for d in self.active_devs:
929  time = getTimeFromTowMs(self.getData(d, DID_SYS_PARAMS, 'timeOfWeekMs'))
930  ax[0].plot(time, self.getData(d, DID_SYS_PARAMS, 'imuTemp'), label=self.log.serials[d])
931  ax[1].plot(time, self.getData(d, DID_SYS_PARAMS, 'baroTemp'))
932  for a in ax:
933  a.grid(True)
934  self.saveFig(fig, 'Temp')
935  except:
936  print(RED + "problem plotting temp: " + sys.exc_info()[0] + RESET)
937 
938  def debugfArr(self, fig=None):
939  if fig is None:
940  fig = plt.figure()
941  ax = fig.subplots(5,2, sharex=True)
942  fig.suptitle('Debug float Array - ' + os.path.basename(os.path.normpath(self.log.directory)))
943  for d in self.active_devs:
944  debug_f = self.getData(d, DID_DEBUG_ARRAY, 'f')
945  for i in range(9):
946  ax[i%5, i//5].set_ylabel('f[' + str(i) +']')
947  ax[i%5, i//5].plot(debug_f[:,i], label=self.log.serials[d])
948  ax[0,0].legend(ncol=2)
949  for b in ax:
950  for a in b:
951  a.grid(True)
952 
953  def debugiArr(self, fig=None):
954  if fig is None:
955  fig = plt.figure()
956  ax = fig.subplots(5,2, sharex=True)
957  fig.suptitle('Debug int array - ' + os.path.basename(os.path.normpath(self.log.directory)))
958  for d in self.active_devs:
959  debug_i = self.getData(d, DID_DEBUG_ARRAY, 'i')
960  for i in range(9):
961  ax[i%5, i//5].set_ylabel('i[' + str(i) +']')
962  ax[i%5, i//5].plot(debug_i[:,i], label=self.log.serials[d])
963  ax[0,0].legend(ncol=2)
964  for b in ax:
965  for a in b:
966  a.grid(True)
967 
968  def debuglfArr(self, fig=None):
969  if fig is None:
970  fig = plt.figure()
971  ax = fig.subplots(3,1, sharex=True)
972  fig.suptitle('Debug double Array - ' + os.path.basename(os.path.normpath(self.log.directory)))
973  for d in self.active_devs:
974  debug_lf = self.getData(d, DID_DEBUG_ARRAY, 'lf')
975  for i in range(3):
976  ax[i].set_ylabel('lf[' + str(i) +']')
977  ax[i].plot(debug_lf[:,i], label=self.log.serials[d])
978  ax[0].legend(ncol=2)
979  for a in ax:
980  a.grid(True)
981 
982  def magDec(self, fig=None):
983  if fig is None:
984  fig = plt.figure()
985 
986  ax = fig.subplots(2, 1, sharex=True)
987  fig.suptitle('Magnetometer Declination - ' + os.path.basename(os.path.normpath(self.log.directory)))
988  self.configureSubplot(ax[0], 'Declination', 'deg')
989  self.configureSubplot(ax[1], 'Inclination', 'deg')
990 
991  for d in self.active_devs:
992  time = getTimeFromTow(self.getData(d, DID_INL2_STATES, 'timeOfWeek'))
993  declination = 180.0/np.pi * self.getData(d, DID_INL2_STATES, 'magDec')
994  inclination = 180.0/np.pi * self.getData(d, DID_INL2_STATES, 'magInc')
995  ax[0].plot(time, declination, label=self.log.serials[d])
996  ax[1].plot(time, inclination)
997  ax[0].legend(ncol=2)
998  self.saveFig(fig, 'magDec')
999  for a in ax:
1000  a.grid(True)
1001 
1002  def deltatime(self, fig=None):
1003  if fig is None:
1004  fig = plt.figure()
1005 
1006  ax = fig.subplots(3, 1, sharex=True)
1007  fig.suptitle('Timestamps - ' + os.path.basename(os.path.normpath(self.log.directory)))
1008  self.configureSubplot(ax[0], 'INS dt', 's')
1009  self.configureSubplot(ax[1], 'GPS dt', 's')
1010  self.configureSubplot(ax[2], 'IMU dt', 's')
1011 
1012  for d in self.active_devs:
1013  dtIns = self.getData(d, DID_INS_2, 'timeOfWeek')[1:] - self.getData(d, DID_INS_2, 'timeOfWeek')[0:-1]
1014  dtIns = dtIns / self.d
1015  timeIns = getTimeFromTow(self.getData(d, DID_INS_2, 'timeOfWeek')[1:])
1016 
1017  dtGps = 0.001*(self.getData(d, DID_GPS1_POS, 'timeOfWeekMs')[1:] - self.getData(d, DID_GPS1_POS, 'timeOfWeekMs')[0:-1])
1018  dtGps = dtGps / self.d
1019  timeGps = getTimeFromTowMs(self.getData(d, DID_GPS1_POS, 'timeOfWeekMs')[1:])
1020 
1021  dtImu = self.getData(d, DID_PREINTEGRATED_IMU, 'time')[1:] - self.getData(d, DID_PREINTEGRATED_IMU, 'time')[0:-1]
1022  dtImu = dtImu / self.d
1023  timeImu = getTimeFromTow(self.getData(d, DID_PREINTEGRATED_IMU, 'time')[1:] + self.getData(d, DID_GPS1_POS, 'towOffset')[-1])
1024 
1025  ax[0].plot(timeIns, dtIns, label=self.log.serials[d])
1026  ax[1].plot(timeGps, dtGps)
1027  ax[2].plot(timeImu, dtImu)
1028 
1029  self.setPlotYSpanMin(ax[0], 0.01)
1030  self.setPlotYSpanMin(ax[1], 0.01)
1031  self.setPlotYSpanMin(ax[2], 0.05)
1032 
1033  ax[0].legend(ncol=2)
1034  for a in ax:
1035  a.grid(True)
1036  self.saveFig(fig, 'deltatime')
1037 
1038  def gpsRawTime(self, fig=None):
1039  if fig is None:
1040  fig = plt.figure()
1041 
1042  ax = fig.subplots(6, 1, sharex=True)
1043  fig.suptitle('Timestamps - ' + os.path.basename(os.path.normpath(self.log.directory)))
1044  self.configureSubplot(ax[0], 'GPS1 Raw dt', 's')
1045  self.configureSubplot(ax[1], 'GPS2 Raw dt', 's')
1046  self.configureSubplot(ax[2], 'GPS Base Raw dt', 's')
1047  self.configureSubplot(ax[3], 'GPS1 Raw Number of Satellites Observed', 's')
1048  self.configureSubplot(ax[4], 'GPS2 Raw Number of Satellites Observed', 's')
1049  self.configureSubplot(ax[5], 'GPS Base Raw Number of Satellites Observed', 's')
1050 
1051  for d in self.active_devs:
1052  N1 = len(self.log.data[d, DID_GPS1_RAW][0])
1053  N2 = len(self.log.data[d, DID_GPS2_RAW][0])
1054  NB = len(self.log.data[d, DID_GPS_BASE_RAW][0])
1055  tgps1 = np.zeros(N1)
1056  nsat1 = np.zeros(N1)
1057  tgps2 = np.zeros(N2)
1058  nsat2 = np.zeros(N2)
1059  tgpsB = np.zeros(NB)
1060  nsatB = np.zeros(NB)
1061  cnt = 0
1062  for iobs in range(N1):
1063  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
1064  t0 = self.log.data[d, DID_GPS1_RAW][0][iobs]['time']['time'][-1] + \
1065  self.log.data[d, DID_GPS1_RAW][0][iobs]['time']['sec'][-1]
1066  nsat1[cnt] = nsat1[cnt] + ns
1067  tgps1[cnt] = t0
1068  if iobs < N1 - 1:
1069  t1 = self.log.data[d, DID_GPS1_RAW][0][iobs + 1]['time']['time'][-1] + \
1070  self.log.data[d, DID_GPS1_RAW][0][iobs + 1]['time']['sec'][-1]
1071  if t1 > t0 + 0.01:
1072  cnt = cnt + 1
1073  tgps1 = tgps1[0: cnt + 1]
1074  nsat1 = nsat1[0: cnt + 1]
1075  cnt = 0
1076  for iobs in range(N2):
1077  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
1078  t0 = self.log.data[d, DID_GPS2_RAW][0][iobs]['time']['time'][-1] + \
1079  self.log.data[d, DID_GPS2_RAW][0][iobs]['time']['sec'][-1]
1080  nsat2[cnt] = nsat2[cnt] + ns
1081  tgps2[cnt] = t0
1082  if iobs < N2 - 1:
1083  t1 = self.log.data[d, DID_GPS2_RAW][0][iobs + 1]['time']['time'][-1] + \
1084  self.log.data[d, DID_GPS2_RAW][0][iobs + 1]['time']['sec'][-1]
1085  if t1 > t0 + 0.01:
1086  cnt = cnt + 1
1087  tgps2 = tgps2[0: cnt + 1]
1088  nsat2 = nsat2[0: cnt + 1]
1089  cnt = 0
1090  for iobs in range(NB):
1091  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
1092  t0 = self.log.data[d, DID_GPS_BASE_RAW][0][iobs]['time']['time'][-1] + \
1093  self.log.data[d, DID_GPS_BASE_RAW][0][iobs]['time']['sec'][-1]
1094  nsatB[cnt] = nsatB[cnt] + ns
1095  tgpsB[cnt] = t0
1096  if iobs < NB - 1:
1097  t1 = self.log.data[d, DID_GPS_BASE_RAW][0][iobs + 1]['time']['time'][-1] + \
1098  self.log.data[d, DID_GPS_BASE_RAW][0][iobs + 1]['time']['sec'][-1]
1099  if t1 > t0 + 0.01:
1100  cnt = cnt + 1
1101  tgpsB = tgpsB[0: cnt + 1]
1102  nsatB = nsatB[0: cnt + 1]
1103  dtGps1 = tgps1[1:] - tgps1[0:-1]
1104 # dtGps1 = dtGps1 / self.d
1105  dtGps2 = tgps2[1:] - tgps2[0:-1]
1106 # dtGps2 = dtGps2 / self.d
1107  dtGpsB = tgpsB[1:] - tgpsB[0:-1]
1108 
1109  ax[0].plot(tgps1[1:], dtGps1, label=self.log.serials[d])
1110  ax[1].plot(tgps2[1:], dtGps2)
1111  ax[2].plot(tgpsB[1:], dtGpsB)
1112  ax[3].plot(tgps1, nsat1, label=self.log.serials[d])
1113  ax[4].plot(tgps2, nsat2)
1114  ax[5].plot(tgpsB, nsatB)
1115 
1116  self.setPlotYSpanMin(ax[0], 2.0)
1117  self.setPlotYSpanMin(ax[1], 2.0)
1118  self.setPlotYSpanMin(ax[2], 2.0)
1119  self.setPlotYSpanMin(ax[3], 25)
1120  self.setPlotYSpanMin(ax[4], 25)
1121  self.setPlotYSpanMin(ax[5], 25)
1122 
1123  ax[0].legend(ncol=2)
1124  for a in ax:
1125  a.grid(True)
1126  self.saveFig(fig, 'gpsRawTime')
1127 
1128  def ekfBiases(self, fig=None):
1129  if fig is None:
1130  fig = plt.figure()
1131  ax = fig.subplots(4, 2, sharex=True)
1132  self.configureSubplot(ax[0,0], 'bias P', 'deg/s')
1133  self.configureSubplot(ax[1,0], 'bias Q', 'deg/s')
1134  self.configureSubplot(ax[2,0], 'bias R', 'deg/s')
1135  self.configureSubplot(ax[3,0], 'bias Barometer', 'm')
1136 
1137  self.configureSubplot(ax[0,1], 'bias acc X', 'm/s^2')
1138  self.configureSubplot(ax[1,1], 'bias acc Y', 'm/s^2')
1139  self.configureSubplot(ax[2,1], 'bias acc Z', 'm/s^2')
1140  fig.suptitle('EKF Biases - ' + os.path.basename(os.path.normpath(self.log.directory)))
1141  for d in self.active_devs:
1142  time = getTimeFromTow(self.getData(d, DID_INL2_STATES, 'timeOfWeek'))
1143  ax[0,0].plot(time, self.getData(d, DID_INL2_STATES, 'biasPqr')[:, 0]*180.0/np.pi, label=self.log.serials[d])
1144  ax[1,0].plot(time, self.getData(d, DID_INL2_STATES, 'biasPqr')[:, 1]*180.0/np.pi)
1145  ax[2,0].plot(time, self.getData(d, DID_INL2_STATES, 'biasPqr')[:, 2]*180.0/np.pi)
1146  ax[3,0].plot(time, self.getData(d, DID_INL2_STATES, 'biasBaro'), label=self.log.serials[d])
1147 
1148  ax[0,1].plot(time, self.getData(d, DID_INL2_STATES, 'biasAcc')[:, 0], label=self.log.serials[d])
1149  ax[1,1].plot(time, self.getData(d, DID_INL2_STATES, 'biasAcc')[:, 1])
1150  ax[2,1].plot(time, self.getData(d, DID_INL2_STATES, 'biasAcc')[:, 2])
1151 
1152  ax[0,0].legend(ncol=2)
1153  for a in ax:
1154  for b in a:
1155  b.grid(True)
1156  self.saveFig(fig, 'ekfBiases')
1157 
1158  def rtkResiduals(self, type, page, fig=None):
1159  if fig is None:
1160  fig = plt.figure()
1161 
1162  if type == 'phase':
1163  did = DID_RTK_PHASE_RESIDUAL
1164  elif type == 'code':
1165  did = DID_RTK_CODE_RESIDUAL
1166 
1167  sat_ids = np.unique(self.log.data[0, did]['sat_id_j'])
1168  sat_ids = sat_ids[sat_ids != 0][page*6:(page+1)*6]
1169 
1170  ax = fig.subplots(6, 1, sharex=True)
1171  fig.suptitle(type + ' Residuals Page ' + str(page+1) + ' - ' + os.path.basename(os.path.normpath(self.log.directory)))
1172 
1173  for i, id in enumerate(sat_ids):
1174  if id == 0: continue
1175  ax[i].set_ylabel(str(id))
1176  for d in self.active_devs:
1177  idx = np.where(self.getData(d, did, 'sat_id_j') == id)
1178  time_idx = idx[0]
1179  sat_state_idx = idx[1]
1180  time = np.array(getTimeFromGTime(self.getData(d, did, 'time')))[time_idx]
1181  residuals = self.getData(d, did, 'v')[time_idx, sat_state_idx]
1182  residuals[np.abs(residuals) > 1e6] = np.nan
1183  ax[i].plot(time, residuals, label=self.log.serials[d])
1184  ax[0].legend(ncol=2)
1185 
1186  def rtkDebug(self, fig=None):
1187  if fig is None:
1188  fig = plt.figure()
1189 
1190  fields = list(self.log.data[0, DID_RTK_DEBUG].dtype.names)
1191  fields.remove('time')
1192  num_plots = 0
1193  for field in fields:
1194  dat = self.log.data[0, DID_RTK_DEBUG][field][0]
1195  if isinstance(dat, np.ndarray):
1196  num_plots += len(dat)
1197  else:
1198  num_plots += 1
1199 
1200  cols = 4
1201  rows = math.ceil(num_plots/float(cols))
1202  ax = fig.subplots(rows, cols, sharex=True)
1203  fig.suptitle('RTK Debug Counters - ' + os.path.basename(os.path.normpath(self.log.directory)))
1204 
1205  for d in self.active_devs:
1206  time = np.array(getTimeFromGTime(self.getData(d, DID_RTK_DEBUG, 'time')))
1207  valid = time > datetime.datetime.strptime('2017', "%Y")
1208 
1209  # Use index rather than time
1210  if 0:
1211  time = np.arange(0, len(time))
1212 
1213  i = 0
1214  for field in fields:
1215  data = self.getData(d, DID_RTK_DEBUG, field)[valid]
1216  if (len(data) == 0):
1217  continue
1218  if isinstance(data[0], np.ndarray):
1219  for j in range(len(data[0])):
1220  ax[ i%rows, i//rows].set_title(field + "_" + str(j))
1221  ax[ i % rows, i // rows ].plot(time[valid], data[:,j], label=self.log.serials[d])
1222  i += 1
1223  else:
1224  ax[i % rows, i // rows].set_title(field)
1225  ax[i % rows, i // rows].title.set_fontsize(8)
1226  # for item in ax[i % rows, i // rows].get_yticklabels():
1227  # item.set_fontsize(8)
1228  ax[i % rows, i // rows].plot(time[valid], data, label=self.log.serials[d])
1229  i += 1
1230  ax[0,0].legend(ncol=2)
1231 
1232  def rtkDebug2(self, fig=None):
1233  if fig is None:
1234  fig = plt.figure()
1235 
1236  ax = fig.subplots(6, 4, sharex=True)
1237 
1238  #max_num_biases = 22 #np.array(self.getData(self.active_devs[0], DID_RTK_DEBUG_2, 'num_biases'))
1239  max_num_biases = self.getData(0, DID_RTK_DEBUG_2, 'num_biases')[-1]
1240  for r in range(0,6):
1241  for c in range(0,4):
1242  self.configureSubplot(ax[r,c], '', '')
1243 
1244  fig.suptitle('RTK Debug2 - ' + os.path.basename(os.path.normpath(self.log.directory)))
1245  for d in self.active_devs:
1246  time = np.array(getTimeFromGTime(self.getData(d, DID_RTK_DEBUG_2, 'time')))
1247  ib = 0
1248  for r in range(0, 6):
1249  for c in range(0, 4):
1250  if ib < max_num_biases:
1251  ax[r,c].plot(time, self.getData(d, DID_RTK_DEBUG_2, 'satBiasFloat')[:, c + r * 4], label=self.log.serials[d])
1252  r1 = r
1253  c1 = c
1254  ib = ib + 1
1255 
1256  # Show serial numbers
1257  ax[r1,c1].legend(ncol=2)
1258 
1259  for a in ax:
1260  for b in a:
1261  b.grid(True)
1262 
1263  def rtkDebug2Sat(self, fig=None):
1264  if fig is None:
1265  fig = plt.figure()
1266 
1267  ax = fig.subplots(6, 4, sharex=True)
1268 
1269  max_num_biases = self.getData(0, DID_RTK_DEBUG_2, 'num_biases')[-1]
1270  for r in range(0,6):
1271  for c in range(0,4):
1272  self.configureSubplot(ax[r,c], '', '')
1273 
1274  fig.suptitle('RTK Debug2 - Sat# - ' + os.path.basename(os.path.normpath(self.log.directory)))
1275  for d in self.active_devs:
1276  time = np.array(getTimeFromGTime(self.getData(d, DID_RTK_DEBUG_2, 'time')))
1277  ib = 0
1278  for r in range(0, 6):
1279  for c in range(0, 4):
1280  if ib < max_num_biases:
1281  ax[r,c].plot(time, self.getData(d, DID_RTK_DEBUG_2, 'sat')[:, c + r * 4], label=self.log.serials[d])
1282  r1 = r
1283  c1 = c
1284  ib = ib + 1
1285 
1286  # Show serial numbers
1287  ax[r1,c1].legend(ncol=2)
1288 
1289  for a in ax:
1290  for b in a:
1291  b.grid(True)
1292 
1293  def rtkDebug2Std(self, fig=None):
1294  if fig is None:
1295  fig = plt.figure()
1296 
1297  ax = fig.subplots(6, 4, sharex=True)
1298 
1299  max_num_biases = self.getData(0, DID_RTK_DEBUG_2, 'num_biases')[-1]
1300  for r in range(0,6):
1301  for c in range(0,4):
1302  self.configureSubplot(ax[r,c], '', '')
1303 
1304  fig.suptitle('RTK Debug 2 - Sat Bias Std - ' + os.path.basename(os.path.normpath(self.log.directory)))
1305  for d in self.active_devs:
1306  time = np.array(getTimeFromGTime(self.getData(d, DID_RTK_DEBUG_2, 'time')))
1307  ib = 0
1308  for r in range(0, 6):
1309  for c in range(0, 4):
1310  if ib < max_num_biases:
1311  ax[r,c].plot(time, self.getData(d, DID_RTK_DEBUG_2, 'satBiasStd')[:, c + r * 4], label=self.log.serials[d])
1312  r1 = r
1313  c1 = c
1314  ib = ib + 1
1315 
1316  # Show serial numbers
1317  ax[r1,c1].legend(ncol=2)
1318 
1319  for a in ax:
1320  for b in a:
1321  b.grid(True)
1322 
1323  def rtkDebug2Lock(self, fig=None):
1324  if fig is None:
1325  fig = plt.figure()
1326 
1327  ax = fig.subplots(6, 4, sharex=True)
1328 
1329  max_num_biases = self.getData(0, DID_RTK_DEBUG_2, 'num_biases')[-1]
1330  for r in range(0,6):
1331  for c in range(0,4):
1332  self.configureSubplot(ax[r,c], '', '')
1333 
1334  fig.suptitle('RTK Debug 2 - Lock Count - ' + os.path.basename(os.path.normpath(self.log.directory)))
1335  for d in self.active_devs:
1336  time = np.array(getTimeFromGTime(self.getData(d, DID_RTK_DEBUG_2, 'time')))
1337  ib = 0
1338  for r in range(0, 6):
1339  for c in range(0, 4):
1340  if ib < max_num_biases:
1341  ax[r,c].plot(time, self.getData(d, DID_RTK_DEBUG_2, 'satLockCnt')[:, c + r * 4], label=self.log.serials[d])
1342  r1 = r
1343  c1 = c
1344  ib = ib + 1
1345 
1346  # Show serial numbers
1347  ax[r1,c1].legend(ncol=2)
1348 
1349  for a in ax:
1350  for b in a:
1351  b.grid(True)
1352 
1353  def wheelEncoder(self, fig=None):
1354  if fig is None:
1355  fig = plt.figure()
1356 
1357  fig.suptitle('Wheel Encoder - ' + os.path.basename(os.path.normpath(self.log.directory)))
1358  ax = fig.subplots(4, 1, sharex=True)
1359  titles = ['Left Wheel Angle', 'Right Wheel Angle', 'Left Wheel Velocity', 'Right Wheel Velocity']
1360  fields = ['theta_l', 'theta_r', 'omega_l', 'omega_r']
1361 
1362  for d in self.active_devs:
1363  time = np.array(getTimeFromTow(self.getData(d, DID_WHEEL_ENCODER, 'timeOfWeek')))
1364  for i, a in enumerate(ax):
1365  a.plot(time, self.getData(d, DID_WHEEL_ENCODER, fields[i]), label=self.log.serials[d])
1366  if i == 0:
1367  a.legend(ncol=2)
1368 
1369  for i, a in enumerate(ax):
1370  a.set_ylabel(fields[i])
1371  a.set_title(titles[i])
1372  a.grid(True)
1373 
1374 
1375 
1376 
1377  def showFigs(self):
1378  if self.show:
1379  plt.show()
1380 
1381 
1382 if __name__ == '__main__':
1383  np.set_printoptions(linewidth=200)
1384  home = expanduser("~")
1385  file = open(home + "/Documents/Inertial_Sense/config.yaml", 'r')
1386  config = yaml.load(file)
1387  directory = config["directory"]
1388  directory = "/home/superjax/Code/IS-src/cpp/SDK/CLTool/build/IS_logs"
1389  directory = r"C:\Users\quaternion\Downloads\20181218_Compass_Drive\20181218 Compass Drive\20181218_101023"
1390  serials = config['serials']
1391 
1392  log2 = Log()
1393  log2.load(directory, serials)
1394 
1395  plotter = logPlot(True, True, 'svg', log2)
1396  plotter.setDownSample(5)
1397  # plotter.deltatime()
1398  # plotter.debugfArr()
1399  # plotter.debugiArr()
1400  # plotter.debuglfArr()
1401  # plotter.gpsStats()
1402  # plotter.rtkStats()
1403  # plotter.insStatus()
1404  # plotter.hdwStatus()
1405  # plotter.temp()
1406  # plotter.att()
1407  # plotter.lla()
1408  # plotter.uvw()
1409  # plotter.ned()
1410  # plotter.nedMap()
1411  # plotter.magDec()
1412  plotter.rtkDebug()
1413  #plotter.wheelEncoder()
1414 
1415  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:1158
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 Sat Sep 19 2020 03:19:04