p1_display plotting performance improvements.

python/fusion_engine_client/analysis/data_loader.py

@@ -525,6 +525,7 @@ def _read(self,
                       str(time_range)))
 
         message_count = 0
+        log_each_message = logger.isEnabledFor(logging.DEBUG)
         while True:
             try:
                 header, payload, message_bytes, message_index = \
@@ -533,18 +534,18 @@ def _read(self,
             except StopIteration:
                 break
 
-            message_size_bytes = header.get_message_size()
-            message_offset_bytes = self.reader.get_bytes_read() - message_size_bytes
+            # Describe the message if per-message debug prints are enabled. Building these strings for every message
+            # in the log is expensive enough to dominate the read when they are not going to be printed.
+            if log_each_message:
+                message_size_bytes = header.get_message_size()
+                logger.debug('  %s %s message @ %d. [length=%d B]' %
+                             ('Skipping unsupported' if payload is None else 'Parsed', header.get_type_string(),
+                              self.reader.get_bytes_read() - message_size_bytes, message_size_bytes))
 
             # Unsupported/unrecognized message type.
             if payload is None:
-                logger.debug('  Skipping unsupported %s message @ %d. [length=%d B]' %
-                             (header.get_type_string(), message_offset_bytes, message_size_bytes))
                 continue
 
-            logger.debug('  Parsed %s message @ %d. [length=%d B]' %
-                         (header.get_type_string(), message_offset_bytes, message_size_bytes))
-
             # Extract P1 and system times from this message, if applicable.
             p1_time = payload.get_p1_time()
             system_time_ns = payload.get_system_time_ns()

python/fusion_engine_client/analysis/reference.py

@@ -1,6 +1,7 @@
 from typing import Optional, Union
 
 import re
+import warnings
 
 import numpy as np
 from pymap3d import ecef2geodetic, geodetic2ecef
@@ -285,7 +286,10 @@ def from_own_log(cls, loader: DataLoader, statistic: str, source_id: Optional[in
                                   else position_std_enu_m[:, 0])
         else:
             position_ecef_m = np.median(position_ecef_m, axis=1)
-            with np.errstate(invalid='ignore'):
+            # A component that is entirely NaN (e.g., roll when only estimating yaw and pitch) yields a NaN median.
+            # That's ok, but nanmedian() will still warn about it. Suppress the warning.
+            with warnings.catch_warnings():
+                warnings.filterwarnings('ignore', message='All-NaN slice encountered', category=RuntimeWarning)
                 velocity_enu_mps = (None if np.all(np.isnan(velocity_enu_mps))
                                     else np.nanmedian(velocity_enu_mps, axis=1))
                 ypr_deg = None if np.all(np.isnan(ypr_deg)) else np.nanmedian(ypr_deg, axis=1)

python/fusion_engine_client/messages/defs.py

@@ -250,6 +250,9 @@ class MessageHeader:
     _FORMAT = '<BBHIBBHIII'
     _SIZE: int = struct.calcsize(_FORMAT)
 
+    # The offset of the payload_size_bytes field within the header.
+    PAYLOAD_SIZE_OFFSET_BYTES = 16
+
     _MAX_EXPECTED_SIZE_BYTES = (1 << 24)
 
     def __init__(self, message_type: MessageType = MessageType.INVALID):

python/fusion_engine_client/messages/device.py

@@ -313,7 +313,9 @@ def to_numpy(cls, messages: Sequence['EventNotificationMessage']):
         result = {
             'system_time': np.array([m.system_time_ns * 1e-9 for m in messages]),
             'event_type': np.array([int(m.event_type) for m in messages], dtype=int),
-            'event_flags': np.array([int(m.event_flags) for m in messages], dtype=np.uint64),
+            # unpack() reinterprets the flags of a LOG event as a signed severity, so mask back to the unsigned
+            # value carried on the wire to keep one representation across the whole column.
+            'event_flags': np.array([int(m.event_flags) & 0xFFFFFFFFFFFFFFFF for m in messages], dtype=np.uint64),
         }
         return result
 

python/fusion_engine_client/messages/measurements.py

@@ -1,6 +1,6 @@
 import math
 import struct
-from typing import Sequence
+from typing import Dict, Sequence, Tuple
 
 from construct import Array, Struct, Padding, Float32l, Int16sl, Int32sl
 import numpy as np
@@ -1743,6 +1743,9 @@ class InputDataWrapperMessage(MessagePayload):
 
     _STRUCT = struct.Struct('<IB x H')
 
+    # The location of the data_type field within the message.
+    DATA_TYPE_OFFSET_BYTES = 6
+
     def __init__(self):
         self.system_time_ns = 0
         self.data_type = InputDataType.M_TYPE_UNKNOWN
@@ -1850,3 +1853,40 @@ def __str__(self):
 Input Data Wrapper @ {system_time_to_str(self.system_time_ns)}
   Data type: {self.data_type.to_string(include_value=True)}
   Data: {len(self.data)} B payload{fe_content_str}"""
+
+    @classmethod
+    def count_wrapped_data(cls, input_path: str, message_offsets: Sequence[int]) \
+            -> Dict['InputDataType', Tuple[int, int]]:
+        """!
+        @brief Count the wrapped data packets of each type, without decoding the messages themselves.
+
+        A log may contain millions of @ref InputDataWrapperMessage%s, which is far too many to deserialize
+        individually just to total them up. This reads the handful of bytes needed for each message directly out of
+        the file instead.
+
+        @param input_path The path to the binary file containing the messages.
+        @param message_offsets The offset of each @ref InputDataWrapperMessage within the file (in bytes).
+
+        @return A `dict`, keyed by @ref InputDataType, containing the number of packets of that type and the total
+                size of their wrapped data (in bytes).
+        """
+        offsets = np.asarray(message_offsets, dtype=np.int64)
+        if len(offsets) == 0:
+            return {}
+
+        # Gather the message payload sizes and wrapped data types out of the file. Both fields are at a fixed offset
+        # within each message, so they can be sliced out of a memory map and reinterpreted in bulk.
+        raw = np.memmap(input_path, dtype=np.uint8, mode='r')
+        payload_size_offsets = offsets + MessageHeader.PAYLOAD_SIZE_OFFSET_BYTES
+        payload_size_bytes = raw[(payload_size_offsets[:, np.newaxis] + np.arange(4))].copy().view(np.uint32).ravel()
+        data_type_offsets = offsets + MessageHeader.calcsize() + cls.DATA_TYPE_OFFSET_BYTES
+        data_type_ints = raw[(data_type_offsets[:, np.newaxis] + np.arange(2))].copy().view(np.uint16).ravel()
+
+        # Total up the data sizes by type. The payload holds the wrapper's own fields followed by the wrapped data.
+        data_size_bytes = payload_size_bytes.astype(np.int64) - cls._STRUCT.size
+        result = {}
+        for data_type_int in np.unique(data_type_ints):
+            idx = data_type_ints == data_type_int
+            data_type = InputDataType(int(data_type_int), raise_on_unrecognized=False)
+            result[data_type] = (int(np.sum(idx)), int(np.sum(data_size_bytes[idx])))
+        return result

python/fusion_engine_client/messages/signal_defs.py

@@ -800,6 +800,32 @@ def get_satellite_type(self) -> SatelliteType:
     def get_prn(self) -> int:
         return self.prn
 
+    # Instances returned by @ref intern(), keyed by (system, PRN).
+    _interned = {}
+
+    @classmethod
+    def intern(cls, system: SatelliteType, prn: int) -> 'SatelliteID':
+        """!
+        @brief Get a shared instance for a satellite, creating it if it has not been seen before.
+
+        A log repeats the same handful of satellites for many epochs, so reusing one instance per satellite avoids
+        constructing millions of identical objects while decoding.
+
+        @warning
+        The returned instance is shared with every other caller asking for the same satellite, and must not be
+        modified.
+
+        @param system The GNSS constellation to which the satellite belongs.
+        @param prn The satellite's PRN.
+
+        @return The shared @ref SatelliteID.
+        """
+        key = (system, prn)
+        result = cls._interned.get(key)
+        if result is None:
+            result = cls._interned[key] = cls(system=system, prn=prn)
+        return result
+
     def decode_hash(self, sv_hash: int):
         self.system, self.prn, _ = decode_signal_hash(sv_hash)
         self._hash = int(sv_hash)
@@ -917,7 +943,7 @@ def get_satellite_id(self) -> SatelliteID:
 
         @return The @ref SatelliteID for the satellite transmitting this signal.
         """
-        return SatelliteID(system=self.signal_type.get_satellite_type(), prn=self.prn)
+        return SatelliteID.intern(self.signal_type.get_satellite_type(), self.prn)
 
     def get_satellite_type(self) -> SatelliteType:
         return self.signal_type.get_satellite_type()
@@ -937,6 +963,32 @@ def get_gnss_component(self) -> GNSSComponent:
     def get_prn(self) -> int:
         return self.prn
 
+    # Instances returned by @ref intern(), keyed by (signal type, PRN).
+    _interned = {}
+
+    @classmethod
+    def intern(cls, signal_type: GNSSSignalType, prn: int) -> 'SignalID':
+        """!
+        @brief Get a shared instance for a signal, creating it if it has not been seen before.
+
+        A log repeats the same handful of signals for many epochs, so reusing one instance per signal avoids
+        constructing millions of identical objects while decoding.
+
+        @warning
+        The returned instance is shared with every other caller asking for the same signal, and must not be
+        modified.
+
+        @param signal_type The type of the signal.
+        @param prn The signal's PRN.
+
+        @return The shared @ref SignalID.
+        """
+        key = (signal_type, prn)
+        result = cls._interned.get(key)
+        if result is None:
+            result = cls._interned[key] = cls(signal_type=signal_type, prn=prn)
+        return result
+
     def decode_hash(self, signal_hash: int):
         _, self.prn, self.signal_type = decode_signal_hash(signal_hash)
         if self.signal_type is None:

python/fusion_engine_client/messages/solution.py

@@ -6,7 +6,7 @@
 import numpy as np
 
 from ..utils.construct_utils import AutoEnum, construct_message_to_string
-from ..utils.enum_utils import IntEnum
+from ..utils.enum_utils import IntEnum, lookup_value
 from .defs import *
 
 
@@ -441,7 +441,7 @@ def __init__(self):
         self.cn0_dbhz = np.nan
 
     def get_satellite_id(self) -> SatelliteID:
-        return SatelliteID(system=self.system, prn=self.prn)
+        return SatelliteID.intern(self.system, self.prn)
 
     def pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = True) -> (bytes, int):
         if buffer is None:
@@ -730,7 +730,7 @@ class GNSSSatelliteInfo(_GNSSSatelliteInfo):
     _STRUCT = struct.Struct('<BBBxhH')
 
     def get_satellite_id(self) -> SatelliteID:
-        return SatelliteID(system=self.system, prn=self.prn)
+        return SatelliteID.intern(self.system, self.prn)
 
     def pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = True) -> (bytes, int):
         if buffer is None:
@@ -764,7 +764,7 @@ def unpack(self, buffer: bytes, offset: int = 0, version: int = MessagePayload._
             self._STRUCT.unpack_from(buffer=buffer, offset=offset)
         offset += self._STRUCT.size
 
-        self.system = SatelliteType(system, raise_on_unrecognized=False)
+        self.system = lookup_value(SatelliteType, system)
         self.elevation_deg = np.nan if elev_int == self._INVALID_ELEVATION else (elev_int * 0.01)
         self.azimuth_deg = np.nan if azim_int == self._INVALID_AZIMUTH else (azim_int * 0.01)
 
@@ -804,10 +804,10 @@ class GNSSSignalInfo(_GNSSSignalInfo):
     _STRUCT = struct.Struct('<HBBH2x')
 
     def get_signal_id(self) -> SignalID:
-        return SignalID(signal_type=self.signal_type, prn=self.prn)
+        return SignalID.intern(self.signal_type, self.prn)
 
     def get_satellite_id(self) -> SatelliteID:
-        return SatelliteID(system=self.signal_type.get_satellite_type(), prn=self.prn)
+        return SatelliteID.intern(self.signal_type.get_satellite_type(), self.prn)
 
     def pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = True) -> (bytes, int):
         if buffer is None:
@@ -839,7 +839,7 @@ def unpack(self, buffer: bytes, offset: int = 0, version: int = MessagePayload._
             self._STRUCT.unpack_from(buffer=buffer, offset=offset)
         offset += self._STRUCT.size
 
-        self.signal_type = GNSSSignalType(signal_type, raise_on_unrecognized=False)
+        self.signal_type = lookup_value(GNSSSignalType, signal_type)
         self.cn0_dbhz = np.nan if cn0_int == self._INVALID_CN0 else cn0_int * 0.25
 
         return offset - initial_offset

python/fusion_engine_client/parsers/fast_indexer.py

@@ -38,13 +38,15 @@ def _search_blocks_for_fe(input_path: str, thread_idx: int, block_starts: List[i
 
     @return The raw index data corresponding to this thread's data blocks.
     """
+    # Resolved once here rather than inside the search below, where it would run for every message found.
+    trace3_enabled = _logger.isEnabledFor(logging.getTraceLevel(depth=3))
+
     if len(block_starts) == 0:
-        _logger.trace(f'Skipping search thread {thread_idx}. [num_blocks={len(block_starts)}]', depth=2)
+        _logger.trace('Skipping search thread %d. [num_blocks=0]', thread_idx, depth=2)
         return np.array([], dtype=_RAW_DTYPE_WITH_SIZE)
     else:
-        _logger.trace(f'Starting search thread {thread_idx}. '
-                      f'[num_blocks={len(block_starts)}, first={block_starts[0]} B, last={block_starts[-1]} B]',
-                      depth=2)
+        _logger.trace('Starting search thread %d. [num_blocks=%d, first=%d B, last=%d B]',
+                      thread_idx, len(block_starts), block_starts[0], block_starts[-1], depth=2)
     header = MessageHeader()
     message_end = 0
     num_syncs = 0
@@ -83,7 +85,7 @@ def _search_blocks_for_fe(input_path: str, thread_idx: int, block_starts: List[i
             # This is lot faster then doing this check in raw Python due to numpy optimizations.
             sync_matches = np.where(np_data == _PREAMBLE)[0]
 
-            _logger.trace(f'Thread {thread_idx}, block {i}: {len(sync_matches)} matches', depth=2)
+            _logger.trace('Thread %d, block %d: %d matches', thread_idx, i, len(sync_matches), depth=2)
             num_syncs += len(sync_matches)
 
             # To do the CRC check and find a p1_time the full message needs to be parsed. This
@@ -118,14 +120,14 @@ def _search_blocks_for_fe(input_path: str, thread_idx: int, block_starts: List[i
                     # Convert the Timestamp to an integer.
                     p1_time_raw = Timestamp._INVALID if math.isnan(p1_time.seconds) else int(p1_time.seconds)
                     message_end = absolute_offset + header.get_message_size()
-                    if _logger.isEnabledFor(logging.getTraceLevel(depth=3)):
-                        _logger.trace(f'Thread {thread_idx}, block {i}: message={header.message_type.to_string()}, '
-                                      f'file_offset={absolute_offset} B, p1_time={p1_time}',
+                    if trace3_enabled:
+                        _logger.trace('Thread %d, block %d: message=%s, file_offset=%d B, p1_time=%s',
+                                      thread_idx, i, header.message_type.to_string(), absolute_offset, p1_time,
                                       depth=3)
                     raw_list.append((p1_time_raw, int(header.message_type), absolute_offset, header.get_message_size()))
                 except Exception:
                     pass
-    _logger.trace(f'Thread {thread_idx}: {num_syncs} sync with {len(raw_list)} valid FE.')
+    _logger.trace('Thread %d: %d sync with %d valid FE.', thread_idx, num_syncs, len(raw_list))
     # Return the index data for this section of the file.
     return np.array(raw_list, dtype=_RAW_DTYPE_WITH_SIZE)