From e39879aadc5cb98be10752e2843b55f92c972e07 Mon Sep 17 00:00:00 2001 From: Simon Hancock Date: Tue, 2 Jan 2024 08:47:24 +0000 Subject: [PATCH] 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 --- DFReader.py | 210 ++++++++++++++++++++++++++++++++++++-------- tools/mavlogdump.py | 3 + 2 files changed, 177 insertions(+), 36 deletions(-) diff --git a/DFReader.py b/DFReader.py index 932c3e2a1..8b0212cb4 100644 --- a/DFReader.py +++ b/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) diff --git a/tools/mavlogdump.py b/tools/mavlogdump.py index f02b7688a..acd3bbbe5 100755 --- a/tools/mavlogdump.py +++ b/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",