diff --git a/python/fusion_engine_client/analysis/analyzer.py b/python/fusion_engine_client/analysis/analyzer.py index 13c3e06f..4150eb80 100755 --- a/python/fusion_engine_client/analysis/analyzer.py +++ b/python/fusion_engine_client/analysis/analyzer.py @@ -25,7 +25,7 @@ from ..messages import * from .attitude import get_enu_rotation_matrix -from .data_loader import DataLoader, TimeRange +from .data_loader import DataLoader, MessageData, TimeRange from .reference import ReferenceData, _OWN_LOG_STATISTICS from ..parsers.file_index import HostTimeIndexMap from ..utils import trace as logging @@ -196,7 +196,8 @@ def __init__(self, self._mapbox_token_missing = False - self._gnss_signals_data = None + self._gnss_signals_data = {} + self._gnss_antenna_source_ids = None if self.output_dir is not None: if not os.path.exists(self.output_dir): @@ -1199,16 +1200,22 @@ def _plot_data(name, selected_idx, flags, source_id, marker_style=None): self._add_figure(name="map", figure=figure, title="Vehicle Trajectory (Map)", config={'scrollZoom': True}) def plot_gnss_skyplot(self, decimate=True): + for source_id in self._get_gnss_antenna_source_ids(): + self._plot_gnss_skyplot_for_source(source_id, decimate=decimate) + + def _plot_gnss_skyplot_for_source(self, source_id: int, decimate=True): + label = self._gnss_antenna_label(source_id) + # Read the GNSS signal data. - data = self._get_gnss_signals_data() + data = self._get_gnss_signals_data(source_id) if len(data.messages) == 0: - self.logger.info('No GNSS signal data available. Skipping sky plot.') + self.logger.info(f'No GNSS signal data available for source ID {source_id}. Skipping sky plot.') return have_gnss_signals_message = not data.using_legacy_satellite_message # Setup the figure. figure = go.Figure() - figure['layout'].update(title='GNSS Sky Plot') + figure['layout'].update(title=f'{label} GNSS Sky Plot') figure['layout']['polar']['radialaxis'].update(range=[90, 0]) figure['layout']['polar']['angularaxis'].update(visible=False) @@ -1332,13 +1339,20 @@ def plot_gnss_skyplot(self, decimate=True): figure['layout']['updatemenus'] = updatemenus - self._add_figure(name='gnss_skyplot', figure=figure, title='GNSS Sky Plot') + name = self._gnss_plot_filename('gnss_skyplot', source_id) + self._add_figure(name=name, figure=figure, title=f'{label} GNSS Sky Plot') def plot_gnss_cn0(self): + for source_id in self._get_gnss_antenna_source_ids(): + self._plot_gnss_cn0_for_source(source_id) + + def _plot_gnss_cn0_for_source(self, source_id: int): + label = self._gnss_antenna_label(source_id) + # Read the GNSS signal data. - data = self._get_gnss_signals_data() + data = self._get_gnss_signals_data(source_id) if len(data.messages) == 0: - self.logger.info('No GNSS signal data available. Skipping C/N0 plot.') + self.logger.info(f'No GNSS signal data available for source ID {source_id}. Skipping C/N0 plot.') return have_gnss_signals_message = not data.using_legacy_satellite_message @@ -1396,14 +1410,15 @@ def plot_gnss_cn0(self): 'yanchor': 'top' }] - self._add_figure(name='gnss_cn0', figure=figure, title='GNSS C/N0 vs. Time') + name = self._gnss_plot_filename('gnss_cn0', source_id) + self._add_figure(name=name, figure=figure, title=f'{label} GNSS C/N0 vs Time') def plot_gnss_azimuth_elevation(self): """! @brief Plot GNSS azimuth/elevation angles. """ # Read the GNSS signal data. - data = self._get_gnss_signals_data() + data = self._get_gnss_signals_data(self.default_source_id) if len(data.messages) == 0: self.logger.info('No GNSS signal data available. Skipping azimuth/elevation time series plot.') return @@ -1483,13 +1498,19 @@ def plot_gnss_azimuth_elevation(self): self._add_figure(name='gnss_azimuth_elevation', figure=figure, title='GNSS Azimuth & Elevation Vs. Time') def plot_gnss_signal_status(self): - filename = 'gnss_signal_status' - figure_title = "GNSS Signal Status" + for source_id in self._get_gnss_antenna_source_ids(): + self._plot_gnss_signal_status_for_source(source_id) + + def _plot_gnss_signal_status_for_source(self, source_id: int): + label = self._gnss_antenna_label(source_id) + filename = self._gnss_plot_filename('gnss_signal_status', source_id) + figure_title = f'{label} GNSS Signal Status' # Read the GNSS signal data. - data = self._get_gnss_signals_data() + data = self._get_gnss_signals_data(source_id) if len(data.messages) == 0: - self.logger.info('No GNSS signal data available. Skipping signal status plot.') + self.logger.info(f'No GNSS signal data available for source ID {source_id}. Skipping signal status ' + 'plot.') return have_gnss_signals_message = not data.using_legacy_satellite_message @@ -1795,31 +1816,75 @@ def _count_selected(selected_p1_times, return_nonzero_time=False): self._add_figure(name=filename, figure=figure, title=figure_title, inject_js=hover_js) - def _get_gnss_signals_data(self): + def _get_gnss_antenna_source_ids(self) -> List[int]: + """! + @brief Get the source IDs, restricted to known GNSS antenna identifiers, present in this log. + + `self.source_ids` includes every source ID seen across all message types (and may be further restricted by + the user's --source-id argument), so it isn't specific to GNSS antennas. Restrict it here to the known + antenna identifiers so we don't attempt (and skip) a GNSS signal read for unrelated source IDs, e.g. ones + only used for IMU or wheel speed data. + """ + if self._gnss_antenna_source_ids is None: + # 0/1 are the legacy primary/secondary antenna identifiers, predating the SourceIdentifier reserved + # ranges. 300-399 is reserved for GNSS receivers/antennae. + self._gnss_antenna_source_ids = sorted( + sid for sid in self.source_ids if sid in (0, 1) or 300 <= sid <= 399) + return self._gnss_antenna_source_ids + + def _gnss_antenna_label(self, source_id: int) -> str: + if source_id in (0, SourceIdentifier.PRIMARY_GNSS_ANTENNA): + return 'Primary' + elif source_id in (1, SourceIdentifier.SECONDARY_GNSS_ANTENNA): + return 'Secondary' + else: + return f'Source {source_id}' + + def _gnss_plot_filename(self, base_name: str, source_id: int) -> str: + # Keep the primary antenna's filename unsuffixed for backward compatibility with existing links/tooling. + if source_id in (0, SourceIdentifier.PRIMARY_GNSS_ANTENNA): + return base_name + else: + return f'{base_name}_{self._gnss_antenna_label(source_id).lower().replace(" ", "_")}' + + def _get_gnss_signals_data(self, source_id: int): # If we already have data cached, return it. - if self._gnss_signals_data is not None: - return self._gnss_signals_data + if self._gnss_signals_data.get(source_id) is not None: + return self._gnss_signals_data[source_id] # See if we have GNSSSignalsMessages. If so, prefer those. params = copy.deepcopy(self.params) params['return_numpy'] = False + params['source_ids'] = {source_id} - result = self.reader.read(message_types=[GNSSSignalsMessage], **params) - data = result[GNSSSignalsMessage.MESSAGE_TYPE] + available_source_ids = self.reader.get_available_source_ids() + + # DataLoader/MixedLogReader treat an empty (but non-None) source_ids filter as "no filter" rather than + # "match nothing", so if this source ID isn't actually present in the log, skip the read entirely instead + # of getting back every source ID's data. + if source_id not in available_source_ids: + data = MessageData(message_type=GNSSSignalsMessage.MESSAGE_TYPE, params=params) + else: + result = self.reader.read(message_types=[GNSSSignalsMessage], **params) + data = result[GNSSSignalsMessage.MESSAGE_TYPE] # We store the result now, even if there were no GNSSSignalsMessage messages. That way if we also don't have any # GNSSSatelliteMessage messages below, we'll have _something_ to return and we won't try to reload from disk on # each call to this function. - self._gnss_signals_data = data - self._gnss_signals_data.using_legacy_satellite_message = False + self._gnss_signals_data[source_id] = data + data.using_legacy_satellite_message = False # If we don't have any GNSSSignalsMessages, see if we have the legacy GNSSSatelliteMessage and fall back to - # that. + # that. Current firmware can still emit the legacy message, and does so per-antenna just like + # GNSSSignalsMessage, so apply the same source ID filtering here. if len(data.messages) == 0: # The legacy GNSSSatelliteMessage contains data per satellite, not per signal. The plotted C/N0 values will # reflect the L1 signal, unless L1 is not being tracked. - result = self.reader.read(message_types=[GNSSSatelliteMessage], **params) - data = result[GNSSSatelliteMessage.MESSAGE_TYPE] + if source_id not in available_source_ids: + data = MessageData(message_type=GNSSSatelliteMessage.MESSAGE_TYPE, params=params) + else: + result = self.reader.read(message_types=[GNSSSatelliteMessage], **params) + data = result[GNSSSatelliteMessage.MESSAGE_TYPE] # Convert to GNSSSignalsMessages. Some of the fields, like signal type and tracking/usage status, will be # approximated and may not be plotted. @@ -1832,18 +1897,18 @@ def _get_gnss_signals_data(self): data.messages = [m.to_gnss_signals_message() for m in data.messages] data.message_type = GNSSSignalsMessage.MESSAGE_TYPE data.message_class = GNSSSignalsMessage - self._gnss_signals_data = data - self._gnss_signals_data.using_legacy_satellite_message = True + self._gnss_signals_data[source_id] = data + data.using_legacy_satellite_message = True - self._gnss_signals_data.to_numpy() + self._gnss_signals_data[source_id].to_numpy() - return self._gnss_signals_data + return self._gnss_signals_data[source_id] def clear_gnss_signal_data_cache(self): """! @brief Clear cached GNSSSignalsMessage data to free memory when finished plotting. """ - self._gnss_signals_data = None + self._gnss_signals_data = {} def plot_dop(self): """! diff --git a/python/fusion_engine_client/messages/defs.py b/python/fusion_engine_client/messages/defs.py index 30e88b00..84ea71a9 100644 --- a/python/fusion_engine_client/messages/defs.py +++ b/python/fusion_engine_client/messages/defs.py @@ -225,6 +225,19 @@ def is_response(message_type: MessageType) -> bool: return message_type in RESPONSE_MESSAGES +class SourceIdentifier(IntEnum): + # 0 - 99 is reserved for pose solutions. + ## The location on the vehicle defined by the device's output lever arm setting. + OUTPUT_LEVER_ARM = 0 + # 100 - 199 is reserved for IMUs. + # 300 - 399 is reserved for GNSS receivers/antennae. + PRIMARY_GNSS_ANTENNA = 300 + SECONDARY_GNSS_ANTENNA = 301 + # 500 - 599 is reserved for external pose sources, such as an external SLAM or VIO/LIO. + ## Invalid source identifier. + INVALID = 0xFFFFFFFF + + class MessageHeader: INVALID_SOURCE_ID = 0xFFFFFFFF