DFReader.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 '''
3 APM DataFlash log file reader
4 
5 Copyright Andrew Tridgell 2011
6 Released under GNU GPL version 3 or later
7 
8 Partly based on SDLog2Parser by Anton Babushkin
9 '''
10 from __future__ import print_function
11 from builtins import range
12 from builtins import object
13 
14 import array
15 import math
16 import sys
17 import os
18 import mmap
19 import platform
20 
21 import struct
22 import sys
23 from . import mavutil
24 
25 try:
26  long # Python 2 has long
27 except NameError:
28  long = int # But Python 3 does not
29 
30 FORMAT_TO_STRUCT = {
31  "a": ("64s", None, str),
32  "b": ("b", None, int),
33  "B": ("B", None, int),
34  "h": ("h", None, int),
35  "H": ("H", None, int),
36  "i": ("i", None, int),
37  "I": ("I", None, int),
38  "f": ("f", None, float),
39  "n": ("4s", None, str),
40  "N": ("16s", None, str),
41  "Z": ("64s", None, str),
42  "c": ("h", 0.01, float),
43  "C": ("H", 0.01, float),
44  "e": ("i", 0.01, float),
45  "E": ("I", 0.01, float),
46  "L": ("i", 1.0e-7, float),
47  "d": ("d", None, float),
48  "M": ("b", None, int),
49  "q": ("q", None, long), # Backward compat
50  "Q": ("Q", None, long), # Backward compat
51  }
52 
53 def u_ord(c):
54  return ord(c) if sys.version_info.major < 3 else c
55 
56 class DFFormat(object):
57  def __init__(self, type, name, flen, format, columns):
58  self.type = type
59  self.name = null_term(name)
60  self.len = flen
61  self.format = format
62  self.columns = columns.split(',')
63 
64  if self.columns == ['']:
65  self.columns = []
66 
67  msg_struct = "<"
68  msg_mults = []
69  msg_types = []
70  msg_fmts = []
71  for c in format:
72  if u_ord(c) == 0:
73  break
74  try:
75  msg_fmts.append(c)
76  (s, mul, type) = FORMAT_TO_STRUCT[c]
77  msg_struct += s
78  msg_mults.append(mul)
79  if c == "a":
80  msg_types.append(array.array)
81  else:
82  msg_types.append(type)
83  except KeyError as e:
84  print("DFFormat: Unsupported format char: '%s' in message %s" %
85  (c, name))
86  raise Exception("Unsupported format char: '%s' in message %s" %
87  (c, name))
88 
89  self.msg_struct = msg_struct
90  self.msg_types = msg_types
91  self.msg_mults = msg_mults
92  self.msg_fmts = msg_fmts
93  self.colhash = {}
94  for i in range(len(self.columns)):
95  self.colhash[self.columns[i]] = i
96  self.a_indexes = []
97  for i in range(0, len(self.msg_fmts)):
98  if self.msg_fmts[i] == 'a':
99  self.a_indexes.append(i)
100 
101  def __str__(self):
102  return ("DFFormat(%s,%s,%s,%s)" %
103  (self.type, self.name, self.format, self.columns))
104 
105 
106 def to_string(s):
107  '''desperate attempt to convert a string regardless of what garbage we get'''
108  try:
109  return s.decode("utf-8")
110  except Exception as e:
111  pass
112  try:
113  s2 = s.encode('utf-8', 'ignore')
114  x = u"%s" % s2
115  return s2
116  except Exception:
117  pass
118  # so its a nasty one. Let's grab as many characters as we can
119  r = ''
120  while s != '':
121  try:
122  r2 = r + s[0]
123  s = s[1:]
124  r2 = r2.encode('ascii', 'ignore')
125  x = u"%s" % r2
126  r = r2
127  except Exception:
128  break
129  return r + '_XXX'
130 
131 def null_term(str):
132  '''null terminate a string'''
133  if isinstance(str, bytes):
134  str = to_string(str)
135  idx = str.find("\0")
136  if idx != -1:
137  str = str[:idx]
138  return str
139 
140 
141 class DFMessage(object):
142  def __init__(self, fmt, elements, apply_multiplier):
143  self.fmt = fmt
144  self._elements = elements
145  self._apply_multiplier = apply_multiplier
146  self._fieldnames = fmt.columns
147 
148  def to_dict(self):
149  d = {'mavpackettype': self.fmt.name}
150 
151  for field in self._fieldnames:
152  d[field] = self.__getattr__(field)
153 
154  return d
155 
156  def __getattr__(self, field):
157  '''override field getter'''
158  try:
159  i = self.fmt.colhash[field]
160  except Exception:
161  raise AttributeError(field)
162  if isinstance(self._elements[i], bytes):
163  v = self._elements[i].decode("utf-8")
164  else:
165  v = self._elements[i]
166  if self.fmt.format[i] == 'a':
167  pass
168  elif self.fmt.format[i] != 'M' or self._apply_multiplier:
169  v = self.fmt.msg_types[i](v)
170  if self.fmt.msg_types[i] == str:
171  v = null_term(v)
172  if self.fmt.msg_mults[i] is not None and self._apply_multiplier:
173  v *= self.fmt.msg_mults[i]
174  return v
175 
176  def get_type(self):
177  return self.fmt.name
178 
179  def __str__(self):
180  ret = "%s {" % self.fmt.name
181  col_count = 0
182  for c in self.fmt.columns:
183  val = self.__getattr__(c)
184  if isinstance(val, float) and math.isnan(val):
185  # quiet nans have more non-zero values:
186  noisy_nan = "\x7f\xf8\x00\x00\x00\x00\x00\x00"
187  if struct.pack(">d", val) != noisy_nan:
188  val = "qnan"
189  ret += "%s : %s, " % (c, val)
190  col_count += 1
191  if col_count != 0:
192  ret = ret[:-2]
193  return ret + '}'
194 
195  def get_msgbuf(self):
196  '''create a binary message buffer for a message'''
197  values = []
198  is_py2 = sys.version_info < (3,0)
199  for i in range(len(self.fmt.columns)):
200  if i >= len(self.fmt.msg_mults):
201  continue
202  mul = self.fmt.msg_mults[i]
203  name = self.fmt.columns[i]
204  if name == 'Mode' and 'ModeNum' in self.fmt.columns:
205  name = 'ModeNum'
206  v = self.__getattr__(name)
207  if is_py2:
208  if isinstance(v,unicode): # NOQA
209  v = str(v)
210  else:
211  if isinstance(v,str):
212  v = bytes(v,'ascii')
213  if isinstance(v, array.array):
214  v = v.tostring()
215  if mul is not None:
216  v /= mul
217  v = int(round(v))
218  values.append(v)
219 
220  ret1 = struct.pack("BBB", 0xA3, 0x95, self.fmt.type)
221  try:
222  ret2 = struct.pack(self.fmt.msg_struct, *values)
223  except Exception as ex:
224  return None
225  return ret1 + ret2
226 
227  def get_fieldnames(self):
228  return self._fieldnames
229 
230 
231 class DFReaderClock(object):
232  '''base class for all the different ways we count time in logs'''
233 
234  def __init__(self):
235  self.set_timebase(0)
236  self.timestamp = 0
237 
238  def _gpsTimeToTime(self, week, msec):
239  '''convert GPS week and TOW to a time in seconds since 1970'''
240  epoch = 86400*(10*365 + (1980-1969)/4 + 1 + 6 - 2)
241  return epoch + 86400*7*week + msec*0.001 - 15
242 
243  def set_timebase(self, base):
244  self.timebase = base
245 
246  def message_arrived(self, m):
247  pass
248 
249  def rewind_event(self):
250  pass
251 
252 
253 class DFReaderClock_usec(DFReaderClock):
254  '''DFReaderClock_usec - use microsecond timestamps from messages'''
255  def __init__(self):
256  DFReaderClock.__init__(self)
257 
258  def find_time_base(self, gps, first_us_stamp):
259  '''work out time basis for the log - even newer style'''
260  t = self._gpsTimeToTime(gps.GWk, gps.GMS)
261  self.set_timebase(t - gps.TimeUS*0.000001)
262  # this ensures FMT messages get appropriate timestamp:
263  self.timestamp = self.timebase + first_us_stamp*0.000001
264 
265  def type_has_good_TimeMS(self, type):
266  '''The TimeMS in some messages is not from *our* clock!'''
267  if type.startswith('ACC'):
268  return False
269  if type.startswith('GYR'):
270  return False
271  return True
272 
274  if not self.type_has_good_TimeMS(m.get_type()):
275  return False
276  if 'TimeMS' != m._fieldnames[0]:
277  return False
278  if self.timebase + m.TimeMS*0.001 < self.timestamp:
279  return False
280  return True
281 
282  def set_message_timestamp(self, m):
283  if 'TimeUS' == m._fieldnames[0]:
284  # only format messages don't have a TimeUS in them...
285  m._timestamp = self.timebase + m.TimeUS*0.000001
286  elif self.should_use_msec_field0(m):
287  # ... in theory. I expect there to be some logs which are not
288  # "pure":
289  m._timestamp = self.timebase + m.TimeMS*0.001
290  else:
291  m._timestamp = self.timestamp
292  self.timestamp = m._timestamp
293 
294 
296  '''DFReaderClock_msec - a format where many messages have TimeMS in
297  their formats, and GPS messages have a "T" field giving msecs'''
298  def find_time_base(self, gps, first_ms_stamp):
299  '''work out time basis for the log - new style'''
300  t = self._gpsTimeToTime(gps.Week, gps.TimeMS)
301  self.set_timebase(t - gps.T*0.001)
302  self.timestamp = self.timebase + first_ms_stamp*0.001
303 
304  def set_message_timestamp(self, m):
305  if 'TimeMS' == m._fieldnames[0]:
306  m._timestamp = self.timebase + m.TimeMS*0.001
307  elif m.get_type() in ['GPS', 'GPS2']:
308  m._timestamp = self.timebase + m.T*0.001
309  else:
310  m._timestamp = self.timestamp
311  self.timestamp = m._timestamp
312 
313 
315  '''DFReaderClock_px4 - a format where a starting time is explicitly
316  given in a message'''
317  def __init__(self):
318  DFReaderClock.__init__(self)
319  self.px4_timebase = 0
320 
321  def find_time_base(self, gps):
322  '''work out time basis for the log - PX4 native'''
323  t = gps.GPSTime * 1.0e-6
324  self.timebase = t - self.px4_timebase
325 
326  def set_px4_timebase(self, time_msg):
327  self.px4_timebase = time_msg.StartTime * 1.0e-6
328 
329  def set_message_timestamp(self, m):
330  m._timestamp = self.timebase + self.px4_timebase
331 
332  def message_arrived(self, m):
333  type = m.get_type()
334  if type == 'TIME' and 'StartTime' in m._fieldnames:
335  self.set_px4_timebase(m)
336 
337 
339  '''DFReaderClock_gps_interpolated - for when the only real references
340  in a message are GPS timestamps'''
341  def __init__(self):
342  DFReaderClock.__init__(self)
343  self.msg_rate = {}
344  self.counts = {}
346 
347  def rewind_event(self):
348  '''reset counters on rewind'''
349  self.counts = {}
350  self.counts_since_gps = {}
351 
352  def message_arrived(self, m):
353  type = m.get_type()
354  if type not in self.counts:
355  self.counts[type] = 1
356  else:
357  self.counts[type] += 1
358  # this preserves existing behaviour - but should we be doing this
359  # if type == 'GPS'?
360  if type not in self.counts_since_gps:
361  self.counts_since_gps[type] = 1
362  else:
363  self.counts_since_gps[type] += 1
364 
365  if type == 'GPS' or type == 'GPS2':
366  self.gps_message_arrived(m)
367 
368  def gps_message_arrived(self, m):
369  '''adjust time base from GPS message'''
370  # msec-style GPS message?
371  gps_week = getattr(m, 'Week', None)
372  gps_timems = getattr(m, 'TimeMS', None)
373  if gps_week is None:
374  # usec-style GPS message?
375  gps_week = getattr(m, 'GWk', None)
376  gps_timems = getattr(m, 'GMS', None)
377  if gps_week is None:
378  if getattr(m, 'GPSTime', None) is not None:
379  # PX4-style timestamp; we've only been called
380  # because we were speculatively created in case no
381  # better clock was found.
382  return
383 
384  if gps_week is None:
385  # AvA-style logs
386  gps_week = getattr(m, 'Wk')
387  gps_timems = getattr(m, 'TWk')
388  if gps_week is None or gps_timems is None:
389  return
390 
391  t = self._gpsTimeToTime(gps_week, gps_timems)
392 
393  deltat = t - self.timebase
394  if deltat <= 0:
395  return
396 
397  for type in self.counts_since_gps:
398  rate = self.counts_since_gps[type] / deltat
399  if rate > self.msg_rate.get(type, 0):
400  self.msg_rate[type] = rate
401  self.msg_rate['IMU'] = 50.0
402  self.timebase = t
403  self.counts_since_gps = {}
404 
405  def set_message_timestamp(self, m):
406  rate = self.msg_rate.get(m.fmt.name, 50.0)
407  if int(rate) == 0:
408  rate = 50
409  count = self.counts_since_gps.get(m.fmt.name, 0)
410  m._timestamp = self.timebase + count/rate
411 
412 
413 class DFReader(object):
414  '''parse a generic dataflash file'''
415  def __init__(self):
416  # read the whole file into memory for simplicity
417  self.clock = None
418  self.timestamp = 0
419  self.mav_type = mavutil.mavlink.MAV_TYPE_FIXED_WING
420  self.verbose = False
421  self.params = {}
422  self._flightmodes = None
423 
424  def _rewind(self):
425  '''reset state on rewind'''
426  self.messages = {'MAV': self}
427  if self._flightmodes is not None and len(self._flightmodes) > 0:
428  self.flightmode = self._flightmodes[0][0]
429  else:
430  self.flightmode = "UNKNOWN"
431  self.percent = 0
432  if self.clock:
433  self.clock.rewind_event()
434 
435  def init_clock_px4(self, px4_msg_time, px4_msg_gps):
436  self.clock = DFReaderClock_px4()
437  if not self._zero_time_base:
438  self.clock.set_px4_timebase(px4_msg_time)
439  self.clock.find_time_base(px4_msg_gps)
440  return True
441 
442  def init_clock_msec(self):
443  # it is a new style flash log with full timestamps
444  self.clock = DFReaderClock_msec()
445 
446  def init_clock_usec(self):
447  self.clock = DFReaderClock_usec()
448 
449  def init_clock_gps_interpolated(self, clock):
450  self.clock = clock
451 
452  def init_clock(self):
453  '''work out time basis for the log'''
454 
455  self._rewind()
456 
457  # speculatively create a gps clock in case we don't find anything
458  # better
459  gps_clock = DFReaderClock_gps_interpolated()
460  self.clock = gps_clock
461 
462  px4_msg_time = None
463  px4_msg_gps = None
464  gps_interp_msg_gps1 = None
465  first_us_stamp = None
466  first_ms_stamp = None
467 
468  have_good_clock = False
469  while True:
470  m = self.recv_msg()
471  if m is None:
472  break
473 
474  type = m.get_type()
475 
476  if first_us_stamp is None:
477  first_us_stamp = getattr(m, "TimeUS", None)
478 
479  if first_ms_stamp is None and (type != 'GPS' and type != 'GPS2'):
480  # Older GPS messages use TimeMS for msecs past start
481  # of gps week
482  first_ms_stamp = getattr(m, "TimeMS", None)
483 
484  if type == 'GPS' or type == 'GPS2':
485  if getattr(m, "TimeUS", 0) != 0 and \
486  getattr(m, "GWk", 0) != 0: # everything-usec-timestamped
487  self.init_clock_usec()
488  if not self._zero_time_base:
489  self.clock.find_time_base(m, first_us_stamp)
490  have_good_clock = True
491  break
492  if getattr(m, "T", 0) != 0 and \
493  getattr(m, "Week", 0) != 0: # GPS is msec-timestamped
494  if first_ms_stamp is None:
495  first_ms_stamp = m.T
496  self.init_clock_msec()
497  if not self._zero_time_base:
498  self.clock.find_time_base(m, first_ms_stamp)
499  have_good_clock = True
500  break
501  if getattr(m, "GPSTime", 0) != 0: # px4-style-only
502  px4_msg_gps = m
503  if getattr(m, "Week", 0) != 0:
504  if (gps_interp_msg_gps1 is not None and
505  (gps_interp_msg_gps1.TimeMS != m.TimeMS or
506  gps_interp_msg_gps1.Week != m.Week)):
507  # we've received two distinct, non-zero GPS
508  # packets without finding a decent clock to
509  # use; fall back to interpolation. Q: should
510  # we wait a few more messages befoe doing
511  # this?
512  self.init_clock_gps_interpolated(gps_clock)
513  have_good_clock = True
514  break
515  gps_interp_msg_gps1 = m
516 
517  elif type == 'TIME':
518  '''only px4-style logs use TIME'''
519  if getattr(m, "StartTime", None) is not None:
520  px4_msg_time = m
521 
522  if px4_msg_time is not None and px4_msg_gps is not None:
523  self.init_clock_px4(px4_msg_time, px4_msg_gps)
524  have_good_clock = True
525  break
526 
527 # print("clock is " + str(self.clock))
528  if not have_good_clock:
529  # we failed to find any GPS messages to set a time
530  # base for usec and msec clocks. Also, not a
531  # PX4-style log
532  if first_us_stamp is not None:
533  self.init_clock_usec()
534  elif first_ms_stamp is not None:
535  self.init_clock_msec()
536 
537  self._rewind()
538 
539  return
540 
541  def _set_time(self, m):
542  '''set time for a message'''
543  # really just left here for profiling
544  m._timestamp = self.timestamp
545  if len(m._fieldnames) > 0 and self.clock is not None:
546  self.clock.set_message_timestamp(m)
547 
548  def recv_msg(self):
549  return self._parse_next()
550 
551  def _add_msg(self, m):
552  '''add a new message'''
553  type = m.get_type()
554  self.messages[type] = m
555 
556  if self.clock:
557  self.clock.message_arrived(m)
558 
559  if type == 'MSG':
560  if m.Message.find("Rover") != -1:
561  self.mav_type = mavutil.mavlink.MAV_TYPE_GROUND_ROVER
562  elif m.Message.find("Plane") != -1:
563  self.mav_type = mavutil.mavlink.MAV_TYPE_FIXED_WING
564  elif m.Message.find("Copter") != -1:
565  self.mav_type = mavutil.mavlink.MAV_TYPE_QUADROTOR
566  elif m.Message.startswith("Antenna"):
567  self.mav_type = mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER
568  elif m.Message.find("ArduSub") != -1:
569  self.mav_type = mavutil.mavlink.MAV_TYPE_SUBMARINE
570  if type == 'MODE':
571  if isinstance(m.Mode, str):
572  self.flightmode = m.Mode.upper()
573  elif 'ModeNum' in m._fieldnames:
574  mapping = mavutil.mode_mapping_bynumber(self.mav_type)
575  if mapping is not None and m.ModeNum in mapping:
576  self.flightmode = mapping[m.ModeNum]
577  else:
578  self.flightmode = 'UNKNOWN'
579  else:
580  self.flightmode = mavutil.mode_string_acm(m.Mode)
581  if type == 'STAT' and 'MainState' in m._fieldnames:
582  self.flightmode = mavutil.mode_string_px4(m.MainState)
583  if type == 'PARM' and getattr(m, 'Name', None) is not None:
584  self.params[m.Name] = m.Value
585  self._set_time(m)
586 
587  def recv_match(self, condition=None, type=None, blocking=False):
588  '''recv the next message that matches the given condition
589  type can be a string or a list of strings'''
590  if type is not None:
591  if isinstance(type, str):
592  type = set([type])
593  elif isinstance(type, list):
594  type = set(type)
595  while True:
596  if type is not None:
597  self.skip_to_type(type)
598  m = self.recv_msg()
599  if m is None:
600  return None
601  if type is not None and not m.get_type() in type:
602  continue
603  if not mavutil.evaluate_condition(condition, self.messages):
604  continue
605  return m
606 
607  def check_condition(self, condition):
608  '''check if a condition is true'''
609  return mavutil.evaluate_condition(condition, self.messages)
610 
611  def param(self, name, default=None):
612  '''convenient function for returning an arbitrary MAVLink
613  parameter with a default'''
614  if name not in self.params:
615  return default
616  return self.params[name]
617 
618  def flightmode_list(self):
619  '''return an array of tuples for all flightmodes in log. Tuple is (modestring, t0, t1)'''
620  tstamp = None
621  fmode = None
622  if self._flightmodes is None:
623  self._rewind()
624  self._flightmodes = []
625  types = set(['MODE'])
626  while True:
627  m = self.recv_match(type=types)
628  if m is None:
629  break
630  tstamp = m._timestamp
631  if self.flightmode == fmode:
632  continue
633  if len(self._flightmodes) > 0:
634  (mode, t0, t1) = self._flightmodes[-1]
635  self._flightmodes[-1] = (mode, t0, tstamp)
636  self._flightmodes.append((self.flightmode, tstamp, None))
637  fmode = self.flightmode
638  if tstamp is not None:
639  (mode, t0, t1) = self._flightmodes[-1]
640  self._flightmodes[-1] = (mode, t0, self.last_timestamp())
641 
642  self._rewind()
643  return self._flightmodes
644 
646  '''parse a binary dataflash file'''
647  def __init__(self, filename, zero_time_base=False, progress_callback=None):
648  DFReader.__init__(self)
649  # read the whole file into memory for simplicity
650  self.filehandle = open(filename, 'rb')
651  self.filehandle.seek(0, 2)
652  self.data_len = self.filehandle.tell()
653  self.filehandle.seek(0)
654  if platform.system() == "Windows":
655  self.data_map = mmap.mmap(self.filehandle.fileno(), self.data_len, None, mmap.ACCESS_READ)
656  else:
657  self.data_map = mmap.mmap(self.filehandle.fileno(), self.data_len, mmap.MAP_PRIVATE, mmap.PROT_READ)
658 
659  self.HEAD1 = 0xA3
660  self.HEAD2 = 0x95
661  self.unpackers = {}
662  if sys.version_info.major < 3:
663  self.HEAD1 = chr(self.HEAD1)
664  self.HEAD2 = chr(self.HEAD2)
665  self.formats = {
666  0x80: DFFormat(0x80,
667  'FMT',
668  89,
669  'BBnNZ',
670  "Type,Length,Name,Format,Columns")
671  }
672  self._zero_time_base = zero_time_base
673  self.init_clock()
674  self._rewind()
675  self.init_arrays(progress_callback)
676 
677  def _rewind(self):
678  '''rewind to start of log'''
679  DFReader._rewind(self)
680  self.offset = 0
681  self.remaining = self.data_len
682  self.type_nums = None
683  self.timestamp = 0
684 
685  def rewind(self):
686  '''rewind to start of log'''
687  self._rewind()
688 
689  def init_arrays(self, progress_callback=None):
690  '''initialise arrays for fast recv_match()'''
691  self.offsets = []
692  self.counts = []
693  self._count = 0
694  self.name_to_id = {}
695  self.id_to_name = {}
696  for i in range(256):
697  self.offsets.append([])
698  self.counts.append(0)
699  fmt_type = 0x80
700  ofs = 0
701  pct = 0
702  HEAD1 = self.HEAD1
703  HEAD2 = self.HEAD2
704  lengths = [-1] * 256
705 
706  while ofs+3 < self.data_len:
707  hdr = self.data_map[ofs:ofs+3]
708  if hdr[0] != HEAD1 or hdr[1] != HEAD2:
709  print("bad header 0x%02x 0x%02x" % (u_ord(hdr[0]), u_ord(hdr[1])), file=sys.stderr)
710  break
711  mtype = u_ord(hdr[2])
712  self.offsets[mtype].append(ofs)
713 
714  if lengths[mtype] == -1:
715  if not mtype in self.formats:
716  print("unknown msg type 0x%02x" % mtype, file=sys.stderr)
717  break
718  self.offset = ofs
719  self._parse_next()
720  fmt = self.formats[mtype]
721  lengths[mtype] = fmt.len
722 
723  self.counts[mtype] += 1
724  mlen = lengths[mtype]
725 
726  if mtype == fmt_type:
727  body = self.data_map[ofs+3:ofs+mlen]
728  if len(body)+3 < mlen:
729  break
730  fmt = self.formats[mtype]
731  elements = list(struct.unpack(fmt.msg_struct, body))
732  mfmt = DFFormat(
733  elements[0],
734  null_term(elements[2]), elements[1],
735  null_term(elements[3]), null_term(elements[4]))
736  self.formats[elements[0]] = mfmt
737  self.name_to_id[mfmt.name] = mfmt.type
738  self.id_to_name[mfmt.type] = mfmt.name
739 
740  ofs += mlen
741  new_pct = (100 * ofs) // self.data_len
742  if progress_callback is not None and new_pct != pct:
743  progress_callback(new_pct)
744  pct = new_pct
745 
746  for i in range(256):
747  self._count += self.counts[i]
748  self.offset = 0
749 
750  def last_timestamp(self):
751  '''get the last timestamp in the log'''
752  highest_offset = 0
753  second_highest_offset = 0
754  for i in range(256):
755  if self.counts[i] == -1:
756  continue
757  if len(self.offsets[i]) == 0:
758  continue
759  ofs = self.offsets[i][-1]
760  if ofs > highest_offset:
761  second_highest_offset = highest_offset
762  highest_offset = ofs
763  elif ofs > second_highest_offset:
764  second_highest_offset = ofs
765  self.offset = highest_offset
766  m = self.recv_msg()
767  if m is None:
768  self.offset = second_highest_offset
769  m = self.recv_msg()
770  return m._timestamp
771 
772 
773  def skip_to_type(self, type):
774  '''skip fwd to next msg matching given type set'''
775 
776  if self.type_nums is None:
777  # always add some key msg types so we can track flightmode, params etc
778  type = type.copy()
779  type.update(set(['MODE','MSG','PARM','STAT']))
780  self.indexes = []
781  self.type_nums = []
782  for t in type:
783  if not t in self.name_to_id:
784  continue
785  self.type_nums.append(self.name_to_id[t])
786  self.indexes.append(0)
787  smallest_index = -1
788  smallest_offset = self.data_len
789  for i in range(len(self.type_nums)):
790  mtype = self.type_nums[i]
791  if self.indexes[i] >= self.counts[mtype]:
792  continue
793  ofs = self.offsets[mtype][self.indexes[i]]
794  if ofs < smallest_offset:
795  smallest_offset = ofs
796  smallest_index = i
797  if smallest_index >= 0:
798  self.indexes[smallest_index] += 1
799  self.offset = smallest_offset
800 
801  def _parse_next(self):
802  '''read one message, returning it as an object'''
803  if self.data_len - self.offset < 3:
804  return None
805 
806  hdr = self.data_map[self.offset:self.offset+3]
807  skip_bytes = 0
808  skip_type = None
809  # skip over bad messages
810  msg_type = u_ord(hdr[2])
811  while (hdr[0] != self.HEAD1 or hdr[1] != self.HEAD2 or
812  msg_type not in self.formats):
813  if skip_type is None:
814  skip_type = (u_ord(hdr[0]), u_ord(hdr[1]), u_ord(hdr[2]))
815  skip_start = self.offset
816  skip_bytes += 1
817  self.offset += 1
818  if self.data_len - self.offset < 3:
819  return None
820  hdr = self.data_map[self.offset:self.offset+3]
821  msg_type = u_ord(hdr[2])
822  if skip_bytes != 0:
823  if self.remaining < 528:
824  return None
825  print("Skipped %u bad bytes in log at offset %u, type=%s" %
826  (skip_bytes, skip_start, skip_type), file=sys.stderr)
827 
828  self.offset += 3
829  self.remaining = self.data_len - self.offset
830 
831  fmt = self.formats[msg_type]
832  if self.remaining < fmt.len-3:
833  # out of data - can often happen half way through a message
834  if self.verbose:
835  print("out of data", file=sys.stderr)
836  return None
837  body = self.data_map[self.offset:self.offset+fmt.len-3]
838  elements = None
839  try:
840  if not msg_type in self.unpackers:
841  self.unpackers[msg_type] = struct.Struct(fmt.msg_struct).unpack
842  elements = list(self.unpackers[msg_type](body))
843  except Exception as ex:
844  print(ex)
845  if self.remaining < 528:
846  # we can have garbage at the end of an APM2 log
847  return None
848  # we should also cope with other corruption; logs
849  # transfered via DataFlash_MAVLink may have blocks of 0s
850  # in them, for example
851  print("Failed to parse %s/%s with len %u (remaining %u)" %
852  (fmt.name, fmt.msg_struct, len(body), self.remaining),
853  file=sys.stderr)
854  if elements is None:
855  return self._parse_next()
856  name = fmt.name
857  # transform elements which can't be done at unpack time:
858  for a_index in fmt.a_indexes:
859  try:
860  elements[a_index] = array.array('h', elements[a_index])
861  except Exception as e:
862  print("Failed to transform array: %s" % str(e),
863  file=sys.stderr)
864 
865  if name == 'FMT':
866  # add to formats
867  # name, len, format, headings
868  try:
869  self.formats[elements[0]] = DFFormat(
870  elements[0],
871  null_term(elements[2]), elements[1],
872  null_term(elements[3]), null_term(elements[4]))
873  except Exception:
874  return self._parse_next()
875 
876  self.offset += fmt.len - 3
877  self.remaining = self.data_len - self.offset
878  m = DFMessage(fmt, elements, True)
879  self._add_msg(m)
880  self.percent = 100.0 * (self.offset / float(self.data_len))
881 
882  return m
883 
884 
885 def DFReader_is_text_log(filename):
886  '''return True if a file appears to be a valid text log'''
887  with open(filename, 'r') as f:
888  ret = (f.read(8000).find('FMT, ') != -1)
889 
890  return ret
891 
892 
894  '''parse a text dataflash file'''
895  def __init__(self, filename, zero_time_base=False, progress_callback=None):
896  DFReader.__init__(self)
897  # read the whole file into memory for simplicity
898  self.filehandle = open(filename, 'r')
899  self.filehandle.seek(0, 2)
900  self.data_len = self.filehandle.tell()
901  if platform.system() == "Windows":
902  self.data_map = mmap.mmap(self.filehandle.fileno(), self.data_len, None, mmap.ACCESS_READ)
903  else:
904  self.data_map = mmap.mmap(self.filehandle.fileno(), self.data_len, mmap.MAP_PRIVATE, mmap.PROT_READ)
905  self.offset = 0
906 
907  self.formats = {
908  'FMT': DFFormat(0x80,
909  'FMT',
910  89,
911  'BBnNZ',
912  "Type,Length,Name,Format,Columns")
913  }
914  self._rewind()
915  self._zero_time_base = zero_time_base
916  self.init_clock()
917  self._rewind()
918  self.init_arrays(progress_callback)
919 
920  def _rewind(self):
921  '''rewind to start of log'''
922  DFReader._rewind(self)
923  # find the first valid line
924  self.offset = self.data_map.find(b'FMT, ')
925  self.type_list = None
926 
927  def rewind(self):
928  '''rewind to start of log'''
929  self._rewind()
930 
931  def init_arrays(self, progress_callback=None):
932  '''initialise arrays for fast recv_match()'''
933  self.offsets = {}
934  self.counts = {}
935  self._count = 0
936  ofs = self.offset
937  pct = 0
938 
939  while ofs+16 < self.data_len:
940  mtype = self.data_map[ofs:ofs+4]
941  if mtype[3] == ',':
942  mtype = mtype[0:3]
943  if not mtype in self.offsets:
944  self.counts[mtype] = 0
945  self.offsets[mtype] = []
946  self.offset = ofs
947  self._parse_next()
948  self.offsets[mtype].append(ofs)
949 
950  self.counts[mtype] += 1
951 
952  if mtype == "FMT":
953  self.offset = ofs
954  self._parse_next()
955 
956  ofs = self.data_map.find(b"\n", ofs)
957  if ofs == -1:
958  break
959  ofs += 1
960  new_pct = (100 * ofs) // self.data_len
961  if progress_callback is not None and new_pct != pct:
962  progress_callback(new_pct)
963  pct = new_pct
964 
965  for mtype in self.counts.keys():
966  self._count += self.counts[mtype]
967  self.offset = 0
968 
969  def skip_to_type(self, type):
970  '''skip fwd to next msg matching given type set'''
971 
972  if self.type_list is None:
973  # always add some key msg types so we can track flightmode, params etc
974  self.type_list = type.copy()
975  self.type_list.update(set(['MODE','MSG','PARM','STAT']))
976  self.type_list = list(self.type_list)
977  self.indexes = []
978  self.type_nums = []
979  for t in self.type_list:
980  self.indexes.append(0)
981  smallest_index = -1
982  smallest_offset = self.data_len
983  for i in range(len(self.type_list)):
984  mtype = self.type_list[i]
985  if not mtype in self.counts:
986  continue
987  if self.indexes[i] >= self.counts[mtype]:
988  continue
989  ofs = self.offsets[mtype][self.indexes[i]]
990  if ofs < smallest_offset:
991  smallest_offset = ofs
992  smallest_index = i
993  if smallest_index >= 0:
994  self.indexes[smallest_index] += 1
995  self.offset = smallest_offset
996 
997  def _parse_next(self):
998  '''read one message, returning it as an object'''
999 
1000  while True:
1001  endline = self.data_map.find(b'\n',self.offset)
1002  if endline == -1:
1003  endline = self.data_len
1004  if endline < self.offset:
1005  break
1006  s = self.data_map[self.offset:endline].rstrip()
1007  if sys.version_info.major >= 3:
1008  s = s.decode('utf-8')
1009  elements = s.split(", ")
1010  self.offset = endline+1
1011  if len(elements) >= 2:
1012  # this_line is good
1013  break
1014 
1015  if self.offset > self.data_len:
1016  return None
1017 
1018  # cope with empty structures
1019  if len(elements) == 5 and elements[-1] == ',':
1020  elements[-1] = ''
1021  elements.append('')
1022 
1023  self.percent = 100.0 * (self.offset / float(self.data_len))
1024 
1025  msg_type = elements[0]
1026 
1027  if msg_type not in self.formats:
1028  return self._parse_next()
1029 
1030  fmt = self.formats[msg_type]
1031 
1032  if len(elements) < len(fmt.format)+1:
1033  # not enough columns
1034  return self._parse_next()
1035 
1036  elements = elements[1:]
1037 
1038  name = fmt.name.rstrip('\0')
1039  if name == 'FMT':
1040  # add to formats
1041  # name, len, format, headings
1042  if elements[2] == 'FMT' and elements[4] == 'Type,Length,Name,Format':
1043  # some logs have the 'Columns' column missing from text logs
1044  elements[4] = "Type,Length,Name,Format,Columns"
1045  new_fmt = DFFormat(int(elements[0]),
1046  elements[2],
1047  int(elements[1]),
1048  elements[3],
1049  elements[4])
1050  self.formats[elements[2]] = new_fmt
1051 
1052  try:
1053  m = DFMessage(fmt, elements, False)
1054  except ValueError:
1055  return self._parse_next()
1056 
1057  self._add_msg(m)
1058 
1059  return m
1060 
1061  def last_timestamp(self):
1062  '''get the last timestamp in the log'''
1063  highest_offset = 0
1064  for mtype in self.counts.keys():
1065  if len(self.offsets[mtype]) == 0:
1066  continue
1067  ofs = self.offsets[mtype][-1]
1068  if ofs > highest_offset:
1069  highest_offset = ofs
1070  self.offset = highest_offset
1071  m = self.recv_msg()
1072  return m._timestamp
1073 
1074 if __name__ == "__main__":
1075  use_profiler = False
1076  if use_profiler:
1077  from line_profiler import LineProfiler
1078  profiler = LineProfiler()
1079  profiler.add_function(DFReader_binary._parse_next)
1080  profiler.add_function(DFReader_binary._add_msg)
1081  profiler.add_function(DFReader._set_time)
1082  profiler.enable_by_count()
1083 
1084  filename = sys.argv[1]
1085  if filename.endswith('.log'):
1086  log = DFReader_text(filename)
1087  else:
1088  log = DFReader_binary(filename)
1089  while True:
1090  m = log.recv_msg()
1091  if m is None:
1092  break
1093  if use_profiler:
1094  profiler.print_stats()


mavlink
Author(s): Lorenz Meier
autogenerated on Sun Apr 7 2019 02:06:02