Skip to content

Commit

Permalink
DFReader: Read unit data from log and add dump_verbose function to DF…
Browse files Browse the repository at this point in the history 
…Message

Cache units and multipliers in lookup tables when first scanning the file
Handles both DFBinary and DFText files
Store derived unit for each field as DFFormat class attribute
Create get_unit method on DFFormat to return unit if defined or empty string

Create dump_verbose function on DFMessage class, which outputs both value and
unit for each field
Also show the deg or deg/s value for rad or rad/s fields

Separate code to detect quiet nan into a utility function, so it can be used
by both __str__ and dump_verbose

Improve display precision of values with a format multiplier, e.g.:
(401952592*1e-7 => 40.195259199999995 vs 401952592/1e7 => 40.1952592)

mavlogdump.py updated to call the dump_verbose method when --verbose specified
Use hasattr to check method exists, in case of misaligned files
  • Loading branch information
@shancock884 @peterbarker
shancock884 authored and peterbarker committed Jan 27, 2024
1 parent 35bc0c1 commit e39879a
Show file tree
Hide file tree
Showing 2 changed files with 177 additions and 36 deletions.
210 changes: 174 additions & 36 deletions DFReader.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
import os
import mmap
import platform
import time

import struct
import sys
Expand Down Expand Up @@ -50,9 +51,32 @@
"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
Expand All @@ -61,8 +85,7 @@ def __init__(self, type, name, flen, format, columns, oldfmt=None):
self.format = format
self.columns = columns.split(',')
self.instance_field = None
self.unit_ids = None
self.mult_ids = None
self.units = None

if self.columns == ['']:
self.columns = []
Expand Down Expand Up @@ -101,32 +124,64 @@ def __init__(self, type, name, flen, format, columns, oldfmt=None):
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.set_unit_ids(oldfmt.unit_ids)
self.set_mult_ids(oldfmt.mult_ids)

def set_unit_ids(self, unit_ids):
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
self.unit_ids = unit_ids
# Does this unit string define an instance field?
instance_idx = unit_ids.find('#')
if instance_idx != -1:
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)

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):
def set_mult_ids(self, mult_ids, mult_lookup):
'''set mult IDs string from FMTU'''
self.mult_ids = mult_ids
# 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)" %
Expand Down Expand Up @@ -192,7 +247,14 @@ def __getattr__(self, field):
if self.fmt.msg_types[i] == str:
v = null_term(v)
if self.fmt.msg_mults[i] is not None and self._apply_multiplier:
v *= self.fmt.msg_mults[i]
# 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):
Expand All @@ -214,15 +276,9 @@ def __str__(self):
col_count = 0
for c in self.fmt.columns:
val = self.__getattr__(c)
if isinstance(val, float) and math.isnan(val):
# quiet nans have more non-zero values:
if is_py3:
noisy_nan = bytearray([0x7f, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00])
else:
noisy_nan = "\x7f\xf8\x00\x00\x00\x00\x00\x00"
if struct.pack(">d", val) != noisy_nan:
val = "qnan"

if is_quiet_nan(val):
val = "qnan"
# Add the value to the return string
if is_py3:
ret += "%s : %s, " % (c, val)
else:
Expand All @@ -235,6 +291,38 @@ def __str__(self):
ret = ret[:-2]
return ret + '}'

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))
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)))
# Append the unit to the output
unit = self.fmt.get_unit(c)
if unit == "":
# No unit specified - just output the newline
f.write("\n")
elif unit.startswith("rad"):
# For rad or rad/s, add the degrees conversion too
f.write(" %s (%s %s)\n" % (unit, math.degrees(val), unit.replace("rad","deg")))
else:
# Append the unit
f.write(" %s\n" % (unit))

def get_msgbuf(self):
'''create a binary message buffer for a message'''
values = []
Expand Down Expand Up @@ -482,6 +570,8 @@ def __init__(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

def _rewind(self):
'''reset state on rewind'''
Expand Down Expand Up @@ -798,6 +888,8 @@ def init_arrays(self, progress_callback=None):
self.counts.append(0)
fmt_type = 0x80
fmtu_type = None
unit_type = None
mult_type = None
ofs = 0
pct = 0
HEAD1 = self.HEAD1
Expand Down Expand Up @@ -859,7 +951,13 @@ def init_arrays(self, progress_callback=None):
self.id_to_name[mfmt.type] = mfmt.name
if mfmt.name == 'FMTU':
fmtu_type = mfmt.type
if mfmt.name == 'UNIT':
unit_type = mfmt.type
if mfmt.name == 'MULT':
mult_type = mfmt.type

# Handle FMTU messages by updating the DFFormat class with the
# unit/multiplier information
if fmtu_type is not None and mtype == fmtu_type:
fmt = self.formats[mtype]
body = self.data_map[ofs+3:ofs+mlen]
Expand All @@ -870,9 +968,33 @@ def init_arrays(self, progress_callback=None):
if ftype in self.formats:
fmt2 = self.formats[ftype]
if 'UnitIds' in fmt.colhash:
fmt2.set_unit_ids(null_term(elements[fmt.colhash['UnitIds']]))
fmt2.set_unit_ids(null_term(elements[fmt.colhash['UnitIds']]), self.unit_lookup)
if 'MultIds' in fmt.colhash:
fmt2.set_mult_ids(null_term(elements[fmt.colhash['MultIds']]))
fmt2.set_mult_ids(null_term(elements[fmt.colhash['MultIds']]), self.mult_lookup)

# Handle UNIT messages by updating the unit_lookup dictionary
if unit_type is not None and mtype == unit_type:
fmt = self.formats[mtype]
body = self.data_map[ofs+3:ofs+mlen]
if len(body)+3 < mlen:
break
elements = list(struct.unpack(fmt.msg_struct, body))
self.unit_lookup[chr(elements[1])] = null_term(elements[2])

# Handle MULT messages by updating the mult_lookup dictionary
if mult_type is not None and mtype == mult_type:
fmt = self.formats[mtype]
body = self.data_map[ofs+3:ofs+mlen]
if len(body)+3 < mlen:
break
elements = list(struct.unpack(fmt.msg_struct, body))
# Even though the multiplier value is logged as a double, the
# values in log files look to be single-precision values that have
# been cast to a double.
# To ensure that the values saved here can be used to index the
# MULT_TO_PREFIX table, we round them to 7 significant decimal digits
mult = float("%.7g" % (elements[2]))
self.mult_lookup[chr(elements[1])] = mult

ofs += mlen
if progress_callback is not None:
Expand Down Expand Up @@ -1038,8 +1160,8 @@ def _parse_next(self):
MultIds = elements[2]
if FmtType in self.formats:
fmt = self.formats[FmtType]
fmt.set_unit_ids(UnitIds)
fmt.set_mult_ids(MultIds)
fmt.set_unit_ids(UnitIds, self.unit_lookup)
fmt.set_mult_ids(MultIds, self.mult_lookup)

try:
self._add_msg(m)
Expand Down Expand Up @@ -1279,8 +1401,24 @@ def _parse_next(self):
fmtid = getattr(m, 'FmtType', None)
if fmtid is not None and fmtid in self.id_to_name:
fmtu = self.formats[self.id_to_name[fmtid]]
fmtu.set_unit_ids(getattr(m, 'UnitIds', None))
fmtu.set_mult_ids(getattr(m, 'MultIds', None))
fmtu.set_unit_ids(getattr(m, 'UnitIds', None), self.unit_lookup)
fmtu.set_mult_ids(getattr(m, 'MultIds', None), self.mult_lookup)

if m.get_type() == 'UNIT':
unitid = getattr(m, 'Id', None)
label = getattr(m, 'Label', None)
self.unit_lookup[chr(unitid)] = null_term(label)

if m.get_type() == 'MULT':
multid = getattr(m, 'Id', None)
mult = getattr(m, 'Mult', None)
# Even though the multiplier value is logged as a double, the
# values in log files look to be single-precision values that have
# been cast to a double.
# To ensure that the values saved here can be used to index the
# MULT_TO_PREFIX table, we round them to 7 significant decimal digits
mult = float("%.7g" % (mult))
self.mult_lookup[chr(multid)] = mult

self._add_msg(m)

Expand Down
3 changes: 3 additions & 0 deletions tools/mavlogdump.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -379,6 +379,9 @@ def match_type(mtype, patterns):
elif args.verbose and istlog:
mavutil.dump_message_verbose(sys.stdout, m)
print("")
elif args.verbose and hasattr(m,"dump_verbose"):
m.dump_verbose(sys.stdout)
print("")
else:
# Otherwise we output in a standard Python dict-style format
s = "%s.%02u: %s" % (time.strftime("%Y-%m-%d %H:%M:%S",
Expand Down

0 comments on commit e39879a

Please sign in to comment.