-
Notifications
You must be signed in to change notification settings - Fork 601
/
DFReader.py
1709 lines (1514 loc) · 61.6 KB
/
DFReader.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#!/usr/bin/env python
'''
APM DataFlash log file reader
Copyright Andrew Tridgell 2011
Released under GNU GPL version 3 or later
Partly based on SDLog2Parser by Anton Babushkin
'''
from __future__ import print_function
from builtins import range
from builtins import object
import array
import math
import sys
import os
import mmap
import platform
import time
import struct
import gzip
import io
from . import mavutil
try:
long # Python 2 has long
except NameError:
long = int # But Python 3 does not
FORMAT_TO_STRUCT = {
"a": ("64s", None, str),
"b": ("b", None, int),
"B": ("B", None, int),
"g": ("e", None, float),
"h": ("h", None, int),
"H": ("H", None, int),
"i": ("i", None, int),
"I": ("I", None, int),
"f": ("f", None, float),
"n": ("4s", None, str),
"N": ("16s", None, str),
"Z": ("64s", None, str),
"c": ("h", 0.01, float),
"C": ("H", 0.01, float),
"e": ("i", 0.01, float),
"E": ("I", 0.01, float),
"L": ("i", 1.0e-7, float),
"d": ("d", None, float),
"M": ("b", None, int),
"q": ("q", None, long), # Backward compat
"Q": ("Q", None, long), # Backward compat
}
MULT_TO_PREFIX = {
0: "",
1: "",
1.0e-1: "d", # deci
1.0e-2: "c", # centi
1.0e-3: "m", # milli
1.0e-6: "µ", # micro
1.0e-9: "n" # nano
}
def u_ord(c):
return ord(c) if sys.version_info.major < 3 else c
def is_quiet_nan(val):
'''determine if the argument is a quiet nan'''
# Is this a float, and some sort of nan?
if isinstance(val, float) and math.isnan(val):
# quiet nans have more non-zero values:
if sys.version_info.major >= 3:
noisy_nan = bytearray([0x7f, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00])
else:
noisy_nan = "\x7f\xf8\x00\x00\x00\x00\x00\x00"
return struct.pack(">d", val) != noisy_nan
else:
return False
class DFFormat(object):
def __init__(self, type, name, flen, format, columns, oldfmt=None):
self.type = type
self.name = null_term(name)
self.len = flen
self.format = format
self.columns = columns.split(',')
self.instance_field = None
self.units = None
if self.columns == ['']:
self.columns = []
msg_struct = "<"
msg_mults = []
msg_types = []
msg_fmts = []
for c in format:
if u_ord(c) == 0:
break
try:
msg_fmts.append(c)
(s, mul, type) = FORMAT_TO_STRUCT[c]
msg_struct += s
msg_mults.append(mul)
if c == "a":
msg_types.append(array.array)
else:
msg_types.append(type)
except KeyError as e:
print("DFFormat: Unsupported format char: '%s' in message %s" %
(c, name))
raise Exception("Unsupported format char: '%s' in message %s" %
(c, name))
self.msg_struct = msg_struct
self.msg_types = msg_types
self.msg_mults = msg_mults
self.msg_fmts = msg_fmts
self.colhash = {}
for i in range(len(self.columns)):
self.colhash[self.columns[i]] = i
self.a_indexes = []
for i in range(0, len(self.msg_fmts)):
if self.msg_fmts[i] == 'a':
self.a_indexes.append(i)
# If this format was alrady defined, copy over units and instance info
if oldfmt is not None:
self.units = oldfmt.units
if oldfmt.instance_field is not None:
self.set_instance_field(self.colhash[oldfmt.instance_field])
def set_instance_field(self, instance_idx):
'''set up the instance field for this format'''
self.instance_field = self.columns[instance_idx]
# work out offset and length of instance field in message
pre_fmt = self.format[:instance_idx]
pre_sfmt = ""
for c in pre_fmt:
(s, mul, type) = FORMAT_TO_STRUCT[c]
pre_sfmt += s
self.instance_ofs = struct.calcsize(pre_sfmt)
(ifmt,) = self.format[instance_idx]
self.instance_len = struct.calcsize(ifmt)
def set_unit_ids(self, unit_ids, unit_lookup):
'''set unit IDs string from FMTU'''
if unit_ids is None:
return
# Does this unit string define an instance field?
instance_idx = unit_ids.find('#')
if instance_idx != -1:
self.set_instance_field(instance_idx)
# Build the units array from the IDs
self.units = [""]*len(self.columns)
for i in range(len(self.columns)):
if i < len(unit_ids):
if unit_ids[i] in unit_lookup:
self.units[i] = unit_lookup[unit_ids[i]]
def set_mult_ids(self, mult_ids, mult_lookup):
'''set mult IDs string from FMTU'''
# Update the units based on the multiplier
for i in range(len(self.units)):
# If the format has its own multiplier, do not adjust the unit,
# and if no unit is specified there is nothing to adjust
if self.msg_mults[i] is not None or self.units[i] == "":
continue
# Get the unit multiplier from the lookup table
if mult_ids[i] in mult_lookup:
unitmult = mult_lookup[mult_ids[i]]
# Combine the multipler and unit to derive the real unit
if unitmult in MULT_TO_PREFIX:
self.units[i] = MULT_TO_PREFIX[unitmult]+self.units[i]
else:
self.units[i] = "%.4g %s" % (unitmult, self.units[i])
def get_unit(self, col):
'''Return the unit for the specified field'''
if self.units is None:
return ""
else:
idx = self.colhash[col]
return self.units[idx]
def __str__(self):
return ("DFFormat(%s,%s,%s,%s)" %
(self.type, self.name, self.format, self.columns))
# Swiped into mavgen_python.py
def to_string(s):
'''desperate attempt to convert a string regardless of what garbage we get'''
if isinstance(s, str):
return s
if sys.version_info[0] == 2:
# In python2 we want to return unicode for passed in unicode
return s
return s.decode(errors="backslashreplace")
def null_term(string):
'''null terminate a string'''
if isinstance(string, bytes):
string = to_string(string)
idx = string.find("\0")
if idx != -1:
string = string[:idx]
return string
class DFMessage(object):
def __init__(self, fmt, elements, apply_multiplier, parent):
self.fmt = fmt
self._elements = elements
self._apply_multiplier = apply_multiplier
self._fieldnames = fmt.columns
self._parent = parent
def to_dict(self):
d = {'mavpackettype': self.fmt.name}
for field in self._fieldnames:
d[field] = self.__getattr__(field)
return d
def __getattr__(self, field):
'''override field getter'''
try:
i = self.fmt.colhash[field]
except Exception:
raise AttributeError(field)
if self.fmt.msg_fmts[i] == 'Z' and self.fmt.name == 'FILE':
# special case for FILE contens as bytes
return self._elements[i]
if isinstance(self._elements[i], bytes):
try:
v = self._elements[i].decode("utf-8")
except UnicodeDecodeError:
# try western europe
v = self._elements[i].decode("ISO-8859-1")
else:
v = self._elements[i]
if self.fmt.format[i] == 'a':
pass
elif self.fmt.format[i] != 'M' or self._apply_multiplier:
v = self.fmt.msg_types[i](v)
if self.fmt.msg_types[i] == str:
v = null_term(v)
if self.fmt.msg_mults[i] is not None and self._apply_multiplier:
# For reasons relating to floating point accuracy, you get a more
# accurate result by dividing by 1e2 or 1e7 than multiplying by
# 1e-2 or 1e-7
if self.fmt.msg_mults[i] > 0.0 and self.fmt.msg_mults[i] < 1.0:
divisor = 1/self.fmt.msg_mults[i]
v /= divisor
else:
v *= self.fmt.msg_mults[i]
return v
def __setattr__(self, field, value):
'''override field setter'''
if not field[0].isupper() or not field in self.fmt.colhash:
super(DFMessage,self).__setattr__(field, value)
else:
i = self.fmt.colhash[field]
if self.fmt.msg_mults[i] is not None and self._apply_multiplier:
value /= self.fmt.msg_mults[i]
self._elements[i] = value
def get_type(self):
return self.fmt.name
def __str__(self):
is_py3 = sys.version_info >= (3,0)
ret = "%s {" % self.fmt.name
col_count = 0
for c in self.fmt.columns:
val = self.__getattr__(c)
if is_quiet_nan(val):
val = "qnan"
# Add the value to the return string
if is_py3:
ret += "%s : %s, " % (c, val)
else:
try:
ret += "%s : %s, " % (c, val)
except UnicodeDecodeError:
ret += "%s : %s, " % (c, to_string(val))
col_count += 1
if col_count != 0:
ret = ret[:-2]
return ret + '}'
def dump_verbose_bitmask(self, f, c, val, field_metadata):
try:
try:
bitmask = field_metadata["bitmask"]
except Exception:
return
# work out how many bits to show:
t = field_metadata.get("type")
bit_count = None
if t == "uint8_t":
bit_count = 8
elif t == "uint16_t":
bit_count = 16
elif t == "uint32_t":
bit_count = 32
if bit_count is None:
return
highest = -1
# we show bit values at least up to the highest bit set:
for i in range(bit_count):
if val & (1<<i):
highest = i
# we show bit values at least up until the highest
# bit we have a description for:
for bit in bitmask.bit:
bit_offset = int(math.log(bit["value"], 2))
if bit_offset > highest:
highest = bit_offset
for i in range(highest+1):
bit_value = 1 << i
if val & bit_value:
bang = " "
else:
bang = "!"
done = False
for bit in bitmask.bit:
if bit["value"] != bit_value:
continue
bit_name = bit.get('name')
f.write(" %s %s" % (bang, bit_name,))
if hasattr(bit, 'description'):
f.write(" (%s)\n" % bit["description"])
else:
f.write("\n")
done = True
break
if not done:
f.write(" %s UNKNOWN_BIT%d\n" % (bang, i))
except Exception as e:
# print(e)
pass
def dump_verbose(self, f):
is_py3 = sys.version_info >= (3,0)
timestamp = "%s.%03u" % (
time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(self._timestamp)),
int(self._timestamp*1000.0)%1000)
f.write("%s: %s\n" % (timestamp, self.fmt.name))
field_metadata_by_name = {}
try:
metadata_tree = self._parent.metadata.metadata_tree()
metadata = metadata_tree[self.fmt.name]
for fm in metadata["fields"].field:
field_metadata_by_name[fm.get("name")] = fm
except Exception as e:
# print(e)
pass
for c in self.fmt.columns:
# Get the value
val = self.__getattr__(c)
# Handle quiet nan
if is_quiet_nan(val):
val = "qnan"
# Output the field label and value
if is_py3:
f.write(" %s: %s" % (c, val))
else:
try:
f.write(" %s: %s" % (c, val))
except UnicodeDecodeError:
f.write(" %s: %s" % (c, to_string(val)))
# If this is a bitmask, then append the hex value
if c in field_metadata_by_name:
if hasattr(field_metadata_by_name[c], 'bitmask'):
f.write(" (0x%x)" % val)
# see if this is an enumeration entry, emit enumeration
# entry name if it is
if c in field_metadata_by_name:
fm = field_metadata_by_name[c]
fm_enum = getattr(fm, "enum", None)
if fm_enum is not None:
enum_entry_name = "?????" # default, "not found" value
for entry in fm_enum.iterchildren():
if int(entry.value) == int(val):
enum_entry_name = entry.get('name')
break
f.write(f" ({enum_entry_name})")
# Append the unit to the output
unit = self.fmt.get_unit(c)
if unit.startswith("rad"):
# For rad or rad/s, add the degrees conversion too
f.write(" %s (%s %s)" % (unit, math.degrees(val), unit.replace("rad","deg")))
else:
# Append the unit
f.write(" %s" % (unit))
# output the newline
f.write("\n")
# if this is a bitmask then print out all bits set:
if c in field_metadata_by_name:
self.dump_verbose_bitmask(f, c, val, field_metadata_by_name[c])
def get_msgbuf(self):
'''create a binary message buffer for a message'''
values = []
is_py2 = sys.version_info < (3,0)
for i in range(len(self.fmt.columns)):
if i >= len(self.fmt.msg_mults):
continue
mul = self.fmt.msg_mults[i]
name = self.fmt.columns[i]
if name == 'Mode' and 'ModeNum' in self.fmt.columns:
name = 'ModeNum'
v = self.__getattr__(name)
if is_py2:
if isinstance(v,unicode): # NOQA
v = str(v)
elif isinstance(v, array.array):
v = v.tostring()
else:
if isinstance(v,str):
try:
v = bytes(v,'ascii')
except UnicodeEncodeError:
v = v.encode()
elif isinstance(v, array.array):
v = v.tobytes()
if mul is not None:
v /= mul
v = int(round(v))
values.append(v)
ret1 = struct.pack("BBB", 0xA3, 0x95, self.fmt.type)
try:
ret2 = struct.pack(self.fmt.msg_struct, *values)
except Exception as ex:
return None
return ret1 + ret2
def get_fieldnames(self):
return self._fieldnames
def __getitem__(self, key):
'''support indexing, allowing for multi-instance sensors in one message'''
if self.fmt.instance_field is None:
raise IndexError()
k = '%s[%s]' % (self.fmt.name, str(key))
if not k in self._parent.messages:
raise IndexError()
return self._parent.messages[k]
class DFReaderClock(object):
'''base class for all the different ways we count time in logs'''
def __init__(self):
self.set_timebase(0)
self.timestamp = 0
def _gpsTimeToTime(self, week, msec):
'''convert GPS week and TOW to a time in seconds since 1970'''
epoch = 86400*(10*365 + int((1980-1969)/4) + 1 + 6 - 2)
return epoch + 86400*7*week + msec*0.001 - 18
def set_timebase(self, base):
self.timebase = base
def message_arrived(self, m):
pass
def rewind_event(self):
pass
class DFReaderClock_usec(DFReaderClock):
'''DFReaderClock_usec - use microsecond timestamps from messages'''
def __init__(self):
DFReaderClock.__init__(self)
def find_time_base(self, gps, first_us_stamp):
'''work out time basis for the log - even newer style'''
t = self._gpsTimeToTime(gps.GWk, gps.GMS)
self.set_timebase(t - gps.TimeUS*0.000001)
# this ensures FMT messages get appropriate timestamp:
self.timestamp = self.timebase + first_us_stamp*0.000001
def type_has_good_TimeMS(self, type):
'''The TimeMS in some messages is not from *our* clock!'''
if type.startswith('ACC'):
return False
if type.startswith('GYR'):
return False
return True
def should_use_msec_field0(self, m):
if not self.type_has_good_TimeMS(m.get_type()):
return False
if 'TimeMS' != m._fieldnames[0]:
return False
if self.timebase + m.TimeMS*0.001 < self.timestamp:
return False
return True
def set_message_timestamp(self, m):
if 'TimeUS' == m._fieldnames[0]:
# only format messages don't have a TimeUS in them...
m._timestamp = self.timebase + m.TimeUS*0.000001
elif self.should_use_msec_field0(m):
# ... in theory. I expect there to be some logs which are not
# "pure":
m._timestamp = self.timebase + m.TimeMS*0.001
else:
m._timestamp = self.timestamp
self.timestamp = m._timestamp
class DFReaderClock_msec(DFReaderClock):
'''DFReaderClock_msec - a format where many messages have TimeMS in
their formats, and GPS messages have a "T" field giving msecs'''
def find_time_base(self, gps, first_ms_stamp):
'''work out time basis for the log - new style'''
t = self._gpsTimeToTime(gps.Week, gps.TimeMS)
self.set_timebase(t - gps.T*0.001)
self.timestamp = self.timebase + first_ms_stamp*0.001
def set_message_timestamp(self, m):
if 'TimeMS' == m._fieldnames[0]:
m._timestamp = self.timebase + m.TimeMS*0.001
elif m.get_type() in ['GPS', 'GPS2']:
m._timestamp = self.timebase + m.T*0.001
else:
m._timestamp = self.timestamp
self.timestamp = m._timestamp
class DFReaderClock_px4(DFReaderClock):
'''DFReaderClock_px4 - a format where a starting time is explicitly
given in a message'''
def __init__(self):
DFReaderClock.__init__(self)
self.px4_timebase = 0
def find_time_base(self, gps):
'''work out time basis for the log - PX4 native'''
t = gps.GPSTime * 1.0e-6
self.timebase = t - self.px4_timebase
def set_px4_timebase(self, time_msg):
self.px4_timebase = time_msg.StartTime * 1.0e-6
def set_message_timestamp(self, m):
m._timestamp = self.timebase + self.px4_timebase
def message_arrived(self, m):
type = m.get_type()
if type == 'TIME' and 'StartTime' in m._fieldnames:
self.set_px4_timebase(m)
class DFReaderClock_gps_interpolated(DFReaderClock):
'''DFReaderClock_gps_interpolated - for when the only real references
in a message are GPS timestamps'''
def __init__(self):
DFReaderClock.__init__(self)
self.msg_rate = {}
self.counts = {}
self.counts_since_gps = {}
def rewind_event(self):
'''reset counters on rewind'''
self.counts = {}
self.counts_since_gps = {}
def message_arrived(self, m):
type = m.get_type()
if type not in self.counts:
self.counts[type] = 1
else:
self.counts[type] += 1
# this preserves existing behaviour - but should we be doing this
# if type == 'GPS'?
if type not in self.counts_since_gps:
self.counts_since_gps[type] = 1
else:
self.counts_since_gps[type] += 1
if type == 'GPS' or type == 'GPS2':
self.gps_message_arrived(m)
def gps_message_arrived(self, m):
'''adjust time base from GPS message'''
# msec-style GPS message?
gps_week = getattr(m, 'Week', None)
gps_timems = getattr(m, 'TimeMS', None)
if gps_week is None:
# usec-style GPS message?
gps_week = getattr(m, 'GWk', None)
gps_timems = getattr(m, 'GMS', None)
if gps_week is None:
if getattr(m, 'GPSTime', None) is not None:
# PX4-style timestamp; we've only been called
# because we were speculatively created in case no
# better clock was found.
return
if gps_week is None and hasattr(m,'Wk'):
# AvA-style logs
gps_week = getattr(m, 'Wk')
gps_timems = getattr(m, 'TWk')
if gps_week is None or gps_timems is None:
return
t = self._gpsTimeToTime(gps_week, gps_timems)
deltat = t - self.timebase
if deltat <= 0:
return
for type in self.counts_since_gps:
rate = self.counts_since_gps[type] / deltat
if rate > self.msg_rate.get(type, 0):
self.msg_rate[type] = rate
self.msg_rate['IMU'] = 50.0
self.timebase = t
self.counts_since_gps = {}
def set_message_timestamp(self, m):
rate = self.msg_rate.get(m.fmt.name, 50.0)
if int(rate) == 0:
rate = 50
count = self.counts_since_gps.get(m.fmt.name, 0)
m._timestamp = self.timebase + count/rate
class DFMetaData(object):
'''handle dataflash messages metadata'''
def __init__(self, parent):
self.parent = parent
self.data = None
self.metadata_load_attempted = False
def reset(self):
'''clear cached data'''
self.data = None
self.metadata_load_attempted = False
@staticmethod
def dot_pymavlink(*args):
'''return a path to store pymavlink data'''
if 'HOME' not in os.environ:
dir = os.path.join(os.environ['LOCALAPPDATA'], '.pymavlink')
else:
dir = os.path.join(os.environ['HOME'], '.pymavlink')
if len(args) == 0:
return dir
return os.path.join(dir, *args)
@staticmethod
def download_url(url):
'''download a URL and return the content'''
if sys.version_info.major < 3:
from urllib2 import urlopen as url_open
from urllib2 import URLError as url_error
else:
from urllib.request import urlopen as url_open
from urllib.error import URLError as url_error
try:
resp = url_open(url)
except url_error as e:
print('Error downloading %s : %s' % (url, e))
return None
return resp.read()
@staticmethod
def download():
# Make sure the folder to store XML in has been created
os.makedirs(DFMetaData.dot_pymavlink('LogMessages'), exist_ok=True)
# Loop through vehicles to download
for vehicle in ['Rover', 'Copter', 'Plane', 'Tracker', 'Blimp', 'Sub']:
url = 'http://autotest.ardupilot.org/LogMessages/%s/LogMessages.xml.gz' % vehicle
file = DFMetaData.dot_pymavlink('LogMessages', "%s.xml" % vehicle)
print("Downloading %s as %s" % (url, file))
data = DFMetaData.download_url(url)
if data is None:
continue
# decompress it...
with gzip.GzipFile(fileobj=io.BytesIO(data)) as gz:
data = gz.read()
try:
open(file, mode='wb').write(data)
except Exception as e:
print("Failed to save to %s : %s" % (file, e))
def metadata_tree(self, verbose=False):
''' return a map between a log message and its metadata. May return
None if data is not available '''
# If we've already tried loading data, use it if we have it
# This avoid repeated attempts, when the file is not there
if self.metadata_load_attempted:
return self.data
self.metadata_load_attempted = True
# Get file name, based on vehicle type
mapping = {mavutil.mavlink.MAV_TYPE_GROUND_ROVER : "Rover",
mavutil.mavlink.MAV_TYPE_FIXED_WING : "Plane",
mavutil.mavlink.MAV_TYPE_QUADROTOR : "Copter",
mavutil.mavlink.MAV_TYPE_HELICOPTER : "Copter",
mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER : "Tracker",
mavutil.mavlink.MAV_TYPE_SUBMARINE : "Sub",
mavutil.mavlink.MAV_TYPE_AIRSHIP : "Blimp",
}
if self.parent.mav_type not in mapping:
return None
path = DFMetaData.dot_pymavlink("LogMessages", "%s.xml" % mapping[self.parent.mav_type])
# Does the file exist?
if not os.path.exists(path):
if verbose:
print("Can't find '%s'" % path)
print("Please run 'logmessage download' from MAVExplorer, or call")
print("DFMetaData.download() from Python.")
return None
# Read in the XML
xml = open(path, 'rb').read()
from lxml import objectify
objectify.enable_recursive_str()
tree = objectify.fromstring(xml)
data = {}
for p in tree.logformat:
n = p.get('name')
data[n] = p
# Cache and return data
self.data = data
return self.data
def print_help(self, msg):
'''print help for a log message'''
data = self.metadata_tree(verbose=True)
if data is None:
return
if msg not in data:
print("No help found for message: %s" % msg)
return
node = data[msg]
# Message name and description
print("Log Message: %s\n%s\n" % (msg, node.description.text))
# Protect against replay messages which dont list their fields
if not hasattr(node.fields, 'field'):
return
namelist = []
unitlist = []
# Loop through fields to build list of name/units
for f in node.fields.field:
namelist.append(f.get('name'))
units = f.get('units')
dtype = f.get('type')
if units:
unitlist.append("[%s] " % units)
elif 'char' in dtype:
unitlist.append("[%s] " % dtype)
elif hasattr(f, 'enum'):
unitlist.append("[enum] ")
elif hasattr(f, 'bitmask'):
unitlist.append("[bitmask] ")
else:
unitlist.append("")
# Now get the max string length from each list
namelen = len(max(namelist, key=len))
unitlen = len(max(unitlist, key=len))
# Loop through fields again to do the actual printing
for i in range(0, len(namelist)):
desc = node.fields.field[i].description.text
print("%-*s %-*s: %s" % (namelen, namelist[i], unitlen, unitlist[i], desc))
def get_description(self, msg):
'''get the description of a log message'''
data = self.metadata_tree()
if data is None:
return None
if msg in data:
return data[msg].description.text
return ""
class DFReader(object):
'''parse a generic dataflash file'''
def __init__(self):
# read the whole file into memory for simplicity
self.clock = None
self.timestamp = 0
self.mav_type = mavutil.mavlink.MAV_TYPE_FIXED_WING
self.verbose = False
self.params = {}
self._flightmodes = None
self.messages = {
'MAV': self,
'__MAV__': self, # avoids conflicts with messages actually called "MAV"
}
self.percent = 0
self.unit_lookup = {} # lookup table of units defined by UNIT messages
self.mult_lookup = {} # lookup table of multipliers defined by MULT messages
self.metadata = DFMetaData(self)
def _rewind(self):
'''reset state on rewind'''
# be careful not to replace self.messages with a new hash;
# some people have taken a reference to self.messages and we
# need their messages to disappear to. If they want their own
# copy they can copy.copy it!
self.messages.clear()
self.messages = {
'MAV': self,
'__MAV__': self, # avoids conflicts with messages actually called "MAV"
}
if self._flightmodes is not None and len(self._flightmodes) > 0:
self.flightmode = self._flightmodes[0][0]
else:
self.flightmode = "UNKNOWN"
self.percent = 0
if self.clock:
self.clock.rewind_event()
def init_clock_px4(self, px4_msg_time, px4_msg_gps):
self.clock = DFReaderClock_px4()
if not self._zero_time_base:
self.clock.set_px4_timebase(px4_msg_time)
self.clock.find_time_base(px4_msg_gps)
return True
def init_clock_msec(self):
# it is a new style flash log with full timestamps
self.clock = DFReaderClock_msec()
def init_clock_usec(self):
self.clock = DFReaderClock_usec()
def init_clock_gps_interpolated(self, clock):
self.clock = clock
def init_clock(self):
'''work out time basis for the log'''
self._rewind()
# speculatively create a gps clock in case we don't find anything
# better
gps_clock = DFReaderClock_gps_interpolated()
self.clock = gps_clock
px4_msg_time = None
px4_msg_gps = None
gps_interp_msg_gps1 = None
first_us_stamp = None
first_ms_stamp = None
have_good_clock = False
while True:
m = self.recv_msg()
if m is None:
break
type = m.get_type()
if first_us_stamp is None:
first_us_stamp = getattr(m, "TimeUS", None)
if first_ms_stamp is None and (type != 'GPS' and type != 'GPS2'):
# Older GPS messages use TimeMS for msecs past start
# of gps week
first_ms_stamp = getattr(m, "TimeMS", None)
if type == 'GPS' or type == 'GPS2':
if getattr(m, "TimeUS", 0) != 0 and \
getattr(m, "GWk", 0) != 0: # everything-usec-timestamped
self.init_clock_usec()
if not self._zero_time_base:
self.clock.find_time_base(m, first_us_stamp)
have_good_clock = True
break
if getattr(m, "T", 0) != 0 and \
getattr(m, "Week", 0) != 0: # GPS is msec-timestamped
if first_ms_stamp is None:
first_ms_stamp = m.T
self.init_clock_msec()
if not self._zero_time_base:
self.clock.find_time_base(m, first_ms_stamp)
have_good_clock = True
break
if getattr(m, "GPSTime", 0) != 0: # px4-style-only
px4_msg_gps = m
if getattr(m, "Week", 0) != 0:
if (gps_interp_msg_gps1 is not None and
(gps_interp_msg_gps1.TimeMS != m.TimeMS or
gps_interp_msg_gps1.Week != m.Week)):
# we've received two distinct, non-zero GPS
# packets without finding a decent clock to
# use; fall back to interpolation. Q: should
# we wait a few more messages befoe doing
# this?
self.init_clock_gps_interpolated(gps_clock)
have_good_clock = True
break
gps_interp_msg_gps1 = m
elif type == 'TIME':
'''only px4-style logs use TIME'''
if getattr(m, "StartTime", None) is not None:
px4_msg_time = m
if px4_msg_time is not None and px4_msg_gps is not None:
self.init_clock_px4(px4_msg_time, px4_msg_gps)
have_good_clock = True
break
# print("clock is " + str(self.clock))
if not have_good_clock:
# we failed to find any GPS messages to set a time
# base for usec and msec clocks. Also, not a
# PX4-style log
if first_us_stamp is not None:
self.init_clock_usec()
elif first_ms_stamp is not None:
self.init_clock_msec()
self._rewind()
return
def _set_time(self, m):
'''set time for a message'''
# really just left here for profiling
m._timestamp = self.timestamp
if len(m._fieldnames) > 0 and self.clock is not None:
self.clock.set_message_timestamp(m)
def recv_msg(self):
return self._parse_next()
def _add_msg(self, m):
'''add a new message'''
type = m.get_type()
self.messages[type] = m
if m.fmt.instance_field is not None:
i = m.__getattr__(m.fmt.instance_field)
self.messages["%s[%s]" % (type, str(i))] = m
if self.clock:
self.clock.message_arrived(m)
if type == 'MSG' and hasattr(m,'Message'):
if m.Message.find("Rover") != -1:
self.mav_type = mavutil.mavlink.MAV_TYPE_GROUND_ROVER
elif m.Message.find("Plane") != -1:
self.mav_type = mavutil.mavlink.MAV_TYPE_FIXED_WING
elif m.Message.find("Copter") != -1:
self.mav_type = mavutil.mavlink.MAV_TYPE_QUADROTOR
elif m.Message.startswith("Antenna"):
self.mav_type = mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER
elif m.Message.find("ArduSub") != -1:
self.mav_type = mavutil.mavlink.MAV_TYPE_SUBMARINE
elif m.Message.find("Blimp") != -1:
self.mav_type = mavutil.mavlink.MAV_TYPE_AIRSHIP
if type == 'VER' and hasattr(m,'BU'):
build_types = { 1: mavutil.mavlink.MAV_TYPE_GROUND_ROVER,
2: mavutil.mavlink.MAV_TYPE_QUADROTOR,
3: mavutil.mavlink.MAV_TYPE_FIXED_WING,
4: mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER,
7: mavutil.mavlink.MAV_TYPE_SUBMARINE,
13: mavutil.mavlink.MAV_TYPE_HELICOPTER,
12: mavutil.mavlink.MAV_TYPE_AIRSHIP,
}
mavtype = build_types.get(m.BU,None)
if mavtype is not None:
self.mav_type = mavtype
if type == 'MODE':
if hasattr(m,'Mode') and isinstance(m.Mode, str):
self.flightmode = m.Mode.upper()
elif 'ModeNum' in m._fieldnames:
mapping = mavutil.mode_mapping_bynumber(self.mav_type)
if mapping is not None and m.ModeNum in mapping:
self.flightmode = mapping[m.ModeNum]
else:
self.flightmode = 'UNKNOWN'