Redefined GNSS attitude messages and deprecated old IDs.

- Removed redundant heading convenience fields (compute from the ENU vector or
yaw)
- Added YPR std dev
- Added baseline distance std dev
- Compute heading angle as needed in Analyzer

python/fusion_engine_client/analysis/analyzer.py

@@ -2012,10 +2012,11 @@ def plot_gnss_attitude_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"},
@@ -2030,6 +2031,9 @@ def plot_gnss_attitude_measurements(self):
         # 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, :]))
+
             # 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
@@ -2047,7 +2051,7 @@ def plot_gnss_attitude_measurements(self):
             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"},
@@ -2058,12 +2062,12 @@ def plot_gnss_attitude_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),
@@ -2077,7 +2081,7 @@ def plot_gnss_attitude_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),

python/fusion_engine_client/messages/defs.py

@@ -99,10 +99,12 @@ class MessageType(IntEnum):
 
     # Sensor measurement messages.
     IMU_OUTPUT = 11000
-    RAW_GNSS_ATTITUDE_OUTPUT = 11001
+    DEPRECATED_RAW_HEADING_OUTPUT = 11001
     RAW_IMU_OUTPUT = 11002
-    GNSS_ATTITUDE_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

python/fusion_engine_client/messages/measurements.py

@@ -1,3 +1,4 @@
+import math
 import struct
 from typing import Sequence
 
@@ -1176,14 +1177,15 @@ class GNSSAttitudeOutput(MessagePayload):
     MESSAGE_TYPE = MessageType.GNSS_ATTITUDE_OUTPUT
     MESSAGE_VERSION = 0
 
-    _STRUCT = struct.Struct('<B3xL3ff')
+    _STRUCT = struct.Struct('<B3xI3f3f')
 
     def __init__(self):
         ## Measurement timestamps, if available. See @ref measurement_messages.
         self.details = MeasurementDetails()
 
         ## Set to @ref SolutionType::RTKFixed when heading is available, or @ref SolutionType::Invalid otherwise.
         self.solution_type = SolutionType.Invalid
+
         ## A bitmask of flags associated with the solution
         self.flags = 0
 
@@ -1193,12 +1195,8 @@ def __init__(self):
         self.ypr_deg = np.full((3,), np.nan)
 
         ##
-        # The heading angle (in degrees) with respect to true north, pointing from the primary antenna to the secondary
-        # antenna, after applying offset corrections.
-        #
-        # @note
-        # Reported in the range [0, 360).
-        self.heading_true_north_deg = np.nan
+        # The standard deviation of the orientation measurement (in degrees).
+        self.ypr_std_deg = np.full((3,), np.nan)
 
     def pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = True) -> (bytes, int):
         if buffer is None:
@@ -1216,7 +1214,9 @@ def pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = True
             self.ypr_deg[0],
             self.ypr_deg[1],
             self.ypr_deg[2],
-            self.heading_true_north_deg)
+            self.ypr_std_deg[0],
+            self.ypr_std_deg[1],
+            self.ypr_std_deg[2])
         offset += self._STRUCT.size
 
         if return_buffer:
@@ -1234,7 +1234,9 @@ def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessageP
          self.ypr_deg[0],
          self.ypr_deg[1],
          self.ypr_deg[2],
-         self.heading_true_north_deg) = \
+         self.ypr_std_deg[0],
+         self.ypr_std_deg[1],
+         self.ypr_std_deg[2]) = \
             self._STRUCT.unpack_from(buffer, offset)
         offset += self._STRUCT.size
 
@@ -1244,15 +1246,16 @@ def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessageP
 
     def __repr__(self):
         result = super().__repr__()[:-1]
-        result += f', solution_type={self.solution_type}, heading={self.heading_true_north_deg:.1f} deg]'
+        ypr_str = '(%.1f, %.1f, %.1f)' % tuple(self.ypr_deg)
+        result += f', solution_type={self.solution_type}, ypr={ypr_str} deg]'
         return result
 
     def __str__(self):
         return f"""\
 GNSS Attitude Output @ {str(self.details.p1_time)}
   Solution Type: {self.solution_type}
-  YPR (ENU) (deg): {self.ypr_deg[0]:.2f}, {self.ypr_deg[1]:.2f}, {self.ypr_deg[2]:.2f}
-  Heading (deg): {self.heading_true_north_deg:.2f}"""
+  YPR (deg): {self.ypr_deg[0]:.2f}, {self.ypr_deg[1]:.2f}, {self.ypr_deg[2]:.2f}
+  YPR std (deg): {self.ypr_std_deg[0]:.2f}, {self.ypr_std_deg[1]:.2f}, {self.ypr_std_deg[2]:.2f}"""
 
     @classmethod
     def calcsize(cls) -> int:
@@ -1264,7 +1267,7 @@ def to_numpy(cls, messages: Sequence['GNSSAttitudeOutput']):
             'solution_type': np.array([int(m.solution_type) for m in messages], dtype=int),
             'flags': np.array([int(m.flags) for m in messages], dtype=np.uint32),
             'ypr_deg': np.array([m.ypr_deg for m in messages]).T,
-            'heading_true_north_deg': np.array([m.heading_true_north_deg for m in messages], dtype=float),
+            'ypr_std_deg': np.array([m.ypr_std_deg for m in messages]).T,
         }
         result.update(MeasurementDetails.to_numpy([m.details for m in messages]))
         return result
@@ -1292,23 +1295,23 @@ def __init__(self):
         # The position of the secondary GNSS antenna relative to the primary antenna (in meters), resolved with respect
         # to the local ENU tangent plane: east, north, up.
         self.relative_position_enu_m = np.full((3,), np.nan)
+
         ##
         # The position standard deviation (in meters), resolved with respect to the
         # local ENU tangent plane: east, north, up.
         self.position_std_enu_m = np.full((3,), np.nan)
 
-        ##
-        # The heading between the primary device antenna and the secondary (in degrees) with
-        # respect to true north.
-        #
-        # @note
-        # Reported in the range [0, 360).
-        self.heading_true_north_deg = np.nan
-
         ##
         # The estimated distance between primary and secondary antennas (in meters).
         self.baseline_distance_m = np.nan
 
+        ##
+        # The standard deviation of the baseline distance estimate (in meters).
+        self.baseline_distance_std_m = np.nan
+
+    def get_heading_deg(self):
+        return math.degrees(math.atan2(self.relative_position_enu_m[1], self.relative_position_enu_m[0]))
+
     def pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = True) -> (bytes, int):
         if buffer is None:
             buffer = bytearray(self.calcsize())
@@ -1329,8 +1332,8 @@ def pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = True
             self.position_std_enu_m[0],
             self.position_std_enu_m[1],
             self.position_std_enu_m[2],
-            self.heading_true_north_deg,
-            self.baseline_distance_m)
+            self.baseline_distance_m,
+            self.baseline_distance_std_m)
         offset += self._STRUCT.size
 
         if return_buffer:
@@ -1351,8 +1354,8 @@ def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessageP
          self.position_std_enu_m[0],
          self.position_std_enu_m[1],
          self.position_std_enu_m[2],
-         self.heading_true_north_deg,
-         self.baseline_distance_m) = self._STRUCT.unpack_from(buffer, offset)
+         self.baseline_distance_m,
+         self.baseline_distance_std_m) = self._STRUCT.unpack_from(buffer, offset)
         offset += self._STRUCT.size
 
         self.solution_type = SolutionType(solution_type_int)
@@ -1361,7 +1364,9 @@ def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessageP
 
     def __repr__(self):
         result = super().__repr__()[:-1]
-        result += f', solution_type={self.solution_type}, heading={self.heading_true_north_deg:.1f} deg, ' \
+        enu_str = '(%.2f, %.2f, %.3f)' % tuple(self.relative_position_enu_m)
+        heading_deg = self.get_heading_deg()
+        result += f', solution_type={self.solution_type}, enu={enu_str} m, heading={heading_deg:.1f} deg, ' \
                   f'baseline={self.baseline_distance_m} m]'
         return result
 
@@ -1371,8 +1376,9 @@ def __str__(self):
   Solution Type: {self.solution_type}
   Relative position (ENU) (m): {self.relative_position_enu_m[0]:.2f}, {self.relative_position_enu_m[1]:.2f}, {self.relative_position_enu_m[2]:.2f}
   Position std (ENU) (m): {self.position_std_enu_m[0]:.2f}, {self.position_std_enu_m[1]:.2f}, {self.position_std_enu_m[2]:.2f}
-  Heading (deg): {self.heading_true_north_deg:.2f}
-  Baseline distance (m): {self.baseline_distance_m:.2f}"""
+  Heading (deg): {self.get_heading_deg():.2f}
+  Baseline distance (m): {self.baseline_distance_m:.2f}
+  Baseline std (m): {self.baseline_distance_std_m:.2f}"""
 
     @classmethod
     def calcsize(cls) -> int:
@@ -1385,8 +1391,8 @@ def to_numpy(cls, messages: Sequence['RawGNSSAttitudeOutput']):
             'flags': np.array([int(m.flags) for m in messages], dtype=np.uint32),
             'relative_position_enu_m': np.array([m.relative_position_enu_m for m in messages]).T,
             'position_std_enu_m': np.array([m.position_std_enu_m for m in messages]).T,
-            'heading_true_north_deg': np.array([float(m.heading_true_north_deg) for m in messages]),
             'baseline_distance_m': np.array([float(m.baseline_distance_m) for m in messages]),
+            'baseline_distance_std_m': np.array([float(m.baseline_distance_std_m) for m in messages]),
         }
         result.update(MeasurementDetails.to_numpy([m.details for m in messages]))
         return result

src/point_one/fusion_engine/messages/defs.h

@@ -47,11 +47,12 @@ enum class MessageType : uint16_t {
 
   // Sensor measurement messages.
   IMU_OUTPUT = 11000, ///< @ref IMUOutput
-  RAW_GNSS_ATTITUDE_OUTPUT = 11001, ///< @ref RawGNSSAttitudeOutput
+  DEPRECATED_RAW_HEADING_OUTPUT = 11001,
   RAW_IMU_OUTPUT = 11002, ///< @ref RawIMUOutput
-  // TODO Change the numbers
-  GNSS_ATTITUDE_OUTPUT = 11003, ///< @ref GNSSAttitudeOutput
+  DEPRECATED_HEADING_OUTPUT = 11003,
   IMU_INPUT = 11004, ///< @ref IMUInput
+  GNSS_ATTITUDE_OUTPUT = 11005, ///< @ref GNSSAttitudeOutput
+  RAW_GNSS_ATTITUDE_OUTPUT = 11006, ///< @ref RawGNSSAttitudeOutput
 
   // Vehicle measurement messages.
   DEPRECATED_WHEEL_SPEED_MEASUREMENT =
@@ -152,18 +153,24 @@ P1_CONSTEXPR_FUNC const char* to_string(MessageType type) {
     case MessageType::IMU_OUTPUT:
       return "IMU Output";
 
-    case MessageType::RAW_GNSS_ATTITUDE_OUTPUT:
-      return "Raw GNSS Attitude Output";
+    case MessageType::DEPRECATED_RAW_HEADING_OUTPUT:
+      return "Raw GNSS Heading Output";
 
     case MessageType::RAW_IMU_OUTPUT:
       return "Raw IMU Output";
 
-    case MessageType::GNSS_ATTITUDE_OUTPUT:
-      return "GNSS Attitude Output";
+    case MessageType::DEPRECATED_HEADING_OUTPUT:
+      return "GNSS Heading Output";
 
     case MessageType::IMU_INPUT:
       return "IMU Input";
 
+    case MessageType::GNSS_ATTITUDE_OUTPUT:
+      return "GNSS Attitude Output";
+
+    case MessageType::RAW_GNSS_ATTITUDE_OUTPUT:
+      return "Raw GNSS Attitude Output";
+
     case MessageType::DEPRECATED_WHEEL_SPEED_MEASUREMENT:
       return "Wheel Speed Measurement";
 
@@ -331,10 +338,12 @@ P1_CONSTEXPR_FUNC bool IsCommand(MessageType message_type) {
     case MessageType::RELATIVE_ENU_POSITION:
     case MessageType::SYSTEM_STATUS:
     case MessageType::IMU_OUTPUT:
-    case MessageType::RAW_GNSS_ATTITUDE_OUTPUT:
+    case MessageType::DEPRECATED_RAW_HEADING_OUTPUT:
     case MessageType::RAW_IMU_OUTPUT:
-    case MessageType::GNSS_ATTITUDE_OUTPUT:
+    case MessageType::DEPRECATED_HEADING_OUTPUT:
     case MessageType::IMU_INPUT:
+    case MessageType::GNSS_ATTITUDE_OUTPUT:
+    case MessageType::RAW_GNSS_ATTITUDE_OUTPUT:
     case MessageType::DEPRECATED_WHEEL_SPEED_MEASUREMENT:
     case MessageType::DEPRECATED_VEHICLE_SPEED_MEASUREMENT:
     case MessageType::WHEEL_TICK_INPUT:

src/point_one/fusion_engine/messages/measurements.h

@@ -1189,19 +1189,17 @@ struct P1_ALIGNAS(4) GNSSAttitudeOutput : public MessagePayload {
    *
    * If any angles are not available, they will be set to `NAN`. For
    * dual-antenna systems, the device will measure yaw and pitch, but not roll.
+   *
+   * Note that yaw is measured from east in a counter-clockwise direction. For
+   * example, north is +90 degrees. Heading with respect to true north can be
+   * computed as `heading = 90.0 - ypr_deg[0]`.
    */
   float ypr_deg[3] = {NAN, NAN, NAN};
 
   /**
-   * The heading angle (in degrees) with respect to true north, pointing from
-   * the primary antenna to the secondary antenna.
-   *
-   * The reported angle includes the user-specified heading offset correction.
-   *
-   * @note
-   * Reported in the range [0, 360).
+   * The standard deviation of the orientation measurement (in degrees).
    */
-  float heading_true_north_deg = NAN;
+  float ypr_std_deg[3] = {NAN, NAN, NAN};
 };
 
 /**
@@ -1253,24 +1251,20 @@ struct P1_ALIGNAS(4) RawGNSSAttitudeOutput : public MessagePayload {
   float relative_position_enu_m[3] = {NAN, NAN, NAN};
 
   /**
-   * The position standard deviation (in meters), resolved with respect to the
-   * local ENU tangent plane: east, north, up.
+   * The standard deviation of the relative position vector (in meters),
+   * resolved with respect to the local ENU tangent plane: east, north, up.
    */
   float position_std_enu_m[3] = {NAN, NAN, NAN};
 
   /**
-   * The measured heading angle (in degrees) with respect to true north,
-   * pointing from the primary antenna to the secondary antenna.
-   *
-   * @note
-   * Reported in the range [0, 360).
+   * The estimated distance between primary and secondary antennas (in meters).
    */
-  float heading_true_north_deg = NAN;
+  float baseline_distance_m = NAN;
 
   /**
-   * The estimated distance between primary and secondary antennas (in meters).
+   * The standard deviation of the baseline distance estimate (in meters).
    */
-  float baseline_distance_m = NAN;
+  float baseline_distance_std_m = NAN;
 };
 
 ////////////////////////////////////////////////////////////////////////////////