Replaced deprecated HeadingOutput messages with new `GNSSAttitudeOu…

…tput`. (#345)

# New Features
- Added Python `MessagePayload.get_gps_time()` helper function
- Include `gps_time` in numpy output for measurements timestamped in GPS time

# Changes
- Removed deprecated `[Raw]HeadingOutput` messages
- Replaced with `[Raw]GNSSAttitudeOutput`
  - Distinguishes between multi-antenna GNSS attitude _measurements_ and the nav engine's filtered YPR estimate
  - Adds YPR and baseline distance standard deviation
  - Removes redundant heading angle fields

python/fusion_engine_client/analysis/analyzer.py

@@ -1943,24 +1943,24 @@ def _plot_imu_data(self, message_cls, filename, figure_title):
 
         self._add_figure(name=filename, figure=figure, title=figure_title)
 
-    def plot_heading_measurements(self):
+    def plot_gnss_attitude_measurements(self):
         """!
-        @brief Generate time series plots for heading (degrees) and baseline distance (meters) data.
+        @brief Generate time series plots for GNSS attitude (degrees) and baseline distance (meters) data.
         """
         if self.output_dir is None:
             return
 
-        # Read the heading measurement data.
-        result = self.reader.read(message_types=[RawHeadingOutput, HeadingOutput], **self.params)
-        raw_heading_data = result[RawHeadingOutput.MESSAGE_TYPE]
-        heading_data = result[HeadingOutput.MESSAGE_TYPE]
+        # Read the attitude measurement data.
+        result = self.reader.read(message_types=[RawGNSSAttitudeOutput, GNSSAttitudeOutput], **self.params)
+        raw_heading_data = result[RawGNSSAttitudeOutput.MESSAGE_TYPE]
+        heading_data = result[GNSSAttitudeOutput.MESSAGE_TYPE]
 
         if (len(heading_data.p1_time) == 0) and (len(raw_heading_data.p1_time) == 0):
-            self.logger.info('No heading measurement data available. Skipping plot.')
+            self.logger.info('No GNSS attitude measurement data available. Skipping plot.')
             return
 
-        # Note that we read the pose data after heading, that way we don't bother reading pose data from disk if there's
-        # no heading data in the log.
+        # Note that we read the pose data after attitude, that way we don't bother reading pose data from disk if
+        # there's no heading data in the log.
         result = self.reader.read(message_types=[PoseMessage], source_ids=self.default_source_id, **self.params)
         primary_pose_data = result[PoseMessage.MESSAGE_TYPE]
 
@@ -1987,7 +1987,6 @@ def plot_heading_measurements(self):
 
         fig.update_layout(title='Heading Plots', legend_traceorder='normal', modebar_add=['v1hovermode'])
 
-
         # Display the navigation engine's heading estimate, if available, for comparison with the heading sensor
         # measurement.
         if primary_pose_data is not None:
@@ -2003,7 +2002,7 @@ def plot_heading_measurements(self):
                         customdata=primary_pose_data.p1_time,
                         mode='lines',
                         line={'color': 'yellow'},
-                        name='Primary Device Heading Estimate',
+                        name='Navigation Engine Heading Estimate',
                         hovertemplate='<b>Time</b>: %{x:.3f} sec (%{customdata:.3f} sec)'
                                       '<br><b>Heading</b>: %{y:.2f} deg'
                     ),
@@ -2013,10 +2012,11 @@ def plot_heading_measurements(self):
         # Corrected heading plot
         if len(heading_data.p1_time) > 0:
             heading_time = heading_data.p1_time - float(self.t0)
+            heading_deg = 90.0 - heading_data.ypr_deg[0, :]
             fig.add_trace(
                 go.Scatter(
                     x=heading_time,
-                    y=heading_data.heading_true_north_deg,
+                    y=heading_deg,
                     customdata=heading_data.p1_time,
                     mode='markers',
                     marker={'size': 2, "color": "green"},
@@ -2028,9 +2028,26 @@ def plot_heading_measurements(self):
                 row=1, col=1
             )
 
+            fig.add_trace(
+                go.Scatter(
+                    x=heading_time,
+                    y=heading_data.baseline_distance_m,
+                    customdata=heading_data.p1_time,
+                    marker={'size': 2, "color": "green"},
+                    hovertemplate='<b>Time</b>: %{x:.3f} sec (%{customdata:.3f} sec)'
+                                  '<br><b>Baseline</b>: %{y:.2f} m',
+                    name='Baseline'
+                ),
+                row=2, col=1
+            )
+
         # Uncorrected heading plot
         if len(raw_heading_data.p1_time) > 0:
             raw_heading_time = raw_heading_data.p1_time - float(self.t0)
+            raw_heading_deg = np.degrees(np.arctan2(raw_heading_data.relative_position_enu_m[1, :],
+                                                    raw_heading_data.relative_position_enu_m[0, :]))
+            raw_baseline_distance_m = np.linalg.norm(raw_heading_data.relative_position_enu_m, axis=0)
+
             # Compute heading uncertainty envelop.
             denom = raw_heading_data.relative_position_enu_m[0]**2 + raw_heading_data.relative_position_enu_m[1]**2
             dh_e = raw_heading_data.relative_position_enu_m[0] / denom
@@ -2042,13 +2059,13 @@ def plot_heading_measurements(self):
             )
 
             envelope = np.arctan(
-                (2 * heading_std / raw_heading_data.baseline_distance_m)
+                (2 * heading_std / raw_baseline_distance_m)
             )
             envelope *= 180. / np.pi
             fig.add_trace(
                 go.Scatter(
                     x=raw_heading_time,
-                    y=raw_heading_data.heading_true_north_deg,
+                    y=raw_heading_deg,
                     customdata=raw_heading_data.p1_time,
                     mode='markers',
                     marker={'size': 2, "color": "red"},
@@ -2059,12 +2076,12 @@ def plot_heading_measurements(self):
                 ),
                 row=1, col=1
             )
-            idx = ~np.isnan(raw_heading_data.heading_true_north_deg)
+            idx = ~np.isnan(raw_heading_deg)
 
             fig.add_trace(
                 go.Scatter(
                     x=raw_heading_time[idx],
-                    y=raw_heading_data.heading_true_north_deg[idx] + envelope[idx],
+                    y=raw_heading_deg[idx] + envelope[idx],
                     mode='lines',
                     marker={'size': 2, "color": "red"},
                     line=dict(width=0),
@@ -2078,7 +2095,7 @@ def plot_heading_measurements(self):
             fig.add_trace(
                 go.Scatter(
                     x=raw_heading_time[idx],
-                    y=raw_heading_data.heading_true_north_deg[idx] - envelope[idx],
+                    y=raw_heading_deg[idx] - envelope[idx],
                     mode='lines',
                     marker={'size': 2, "color": "red"},
                     line=dict(width=0),
@@ -2131,7 +2148,7 @@ def plot_heading_measurements(self):
             fig.add_trace(
                 go.Scatter(
                     x=raw_heading_time,
-                    y=raw_heading_data.baseline_distance_m,
+                    y=raw_baseline_distance_m,
                     customdata=raw_heading_data.p1_time,
                     marker={'size': 2, "color": "red"},
                     hovertemplate='<b>Time</b>: %{x:.3f} sec (%{customdata:.3f} sec)'
@@ -2188,7 +2205,7 @@ def plot_heading_measurements(self):
                 row=3, col=1
             )
 
-        self._add_figure(name='heading_measurement', figure=fig, title='Measurements: Heading')
+        self._add_figure(name='gnss_attitude_measurement', figure=fig, title='Measurements: GNSS Attitude')
 
     def plot_system_status_profiling(self):
         """!
@@ -3002,9 +3019,9 @@ def main(args=None):
         analyzer.plot_gnss_corrections_status()
         analyzer.plot_dop()
 
-        # By default, we always plot heading measurements (i.e., output from a secondary heading device like an
+        # By default, we always plot attitude measurements (i.e., output from a secondary GNSS attitude sensor like an
         # LG69T-AH), separate from other sensor measurements controlled by --measurements.
-        analyzer.plot_heading_measurements()
+        analyzer.plot_gnss_attitude_measurements()
 
         if truth_lla_deg is not None:
             analyzer.plot_stationary_position_error(truth_lla_deg)

python/fusion_engine_client/messages/defs.py

@@ -99,10 +99,12 @@ class MessageType(IntEnum):
 
     # Sensor measurement messages.
     IMU_OUTPUT = 11000
-    RAW_HEADING_OUTPUT = 11001
+    DEPRECATED_RAW_HEADING_OUTPUT = 11001
     RAW_IMU_OUTPUT = 11002
-    HEADING_OUTPUT = 11003
+    DEPRECATED_HEADING_OUTPUT = 11003
     IMU_INPUT = 11004
+    GNSS_ATTITUDE_OUTPUT = 11005
+    RAW_GNSS_ATTITUDE_OUTPUT = 11006
 
     # Vehicle measurement messages.
     DEPRECATED_WHEEL_SPEED_MEASUREMENT = 11101
@@ -587,6 +589,13 @@ def get_p1_time(self) -> Timestamp:
         else:
             return getattr(self, 'p1_time', None)
 
+    def get_gps_time(self) -> Timestamp:
+        measurement_details = getattr(self, 'details', None)
+        if isinstance(measurement_details, MeasurementDetails):
+            if measurement_details.measurement_time_source == SystemTimeSource.GPS_TIME:
+                return measurement_details.measurement_time
+        return getattr(self, 'gps_time', None)
+
     def get_system_time_ns(self) -> float:
         measurement_details = getattr(self, 'details', None)
         if isinstance(measurement_details, MeasurementDetails):
@@ -785,3 +794,11 @@ def PackedDataToBuffer(packed_data: bytes, buffer: Optional[bytes] = None, offse
         return buffer
     else:
         return len(packed_data)
+
+
+def yaw_to_heading(yaw_deg: Union[float, np.ndarray]):
+    return 90.0 - yaw_deg
+
+
+def heading_to_yaw(heading_deg: Union[float, np.ndarray]):
+    return 90.0 - heading_deg

python/fusion_engine_client/messages/measurement_details.py

@@ -131,6 +131,12 @@ def to_numpy(cls, messages):
             'p1_time': p1_time,
         }
 
+        idx = time_source == SystemTimeSource.GPS_TIME
+        if np.any(idx):
+            gps_time = np.full_like(time_source, np.nan)
+            gps_time[idx] = measurement_time[idx]
+            result['gps_time'] = gps_time
+
         idx = time_source == SystemTimeSource.TIMESTAMPED_ON_RECEPTION
         if np.any(idx):
             system_time = np.full_like(time_source, np.nan)