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

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,8 +1267,9 @@ 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['heading_true_north_deg'] = yaw_to_heading(result['ypr_deg'])
         result.update(MeasurementDetails.to_numpy([m.details for m in messages]))
         return result
 
@@ -1292,23 +1296,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 +1333,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 +1355,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 +1365,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 +1377,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 +1392,9 @@ 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]),
+            'heading_true_north_deg': np.array([m.get_heading_deg() for m in messages]).T,
         }
         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 =

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;
 };
 
 ////////////////////////////////////////////////////////////////////////////////