DFReader: Read unit data from log and add dump_verbose function to DFMessage

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

Author: shancock884

DFReader.py

@@ -17,6 +17,7 @@
 import os
 import mmap
 import platform
+import time
 
 import struct
 import sys
@@ -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
@@ -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 = []
@@ -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)" %
@@ -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):
@@ -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:
@@ -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 = []
@@ -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'''
@@ -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
@@ -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]
@@ -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:
@@ -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)
@@ -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)
 

tools/mavlogdump.py

@@ -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",