Include baseline distance in corrected attitude message.

python/fusion_engine_client/analysis/analyzer.py

@@ -2028,6 +2028,19 @@ def plot_gnss_attitude_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)

python/fusion_engine_client/messages/measurements.py

@@ -1177,7 +1177,7 @@ class GNSSAttitudeOutput(MessagePayload):
     MESSAGE_TYPE = MessageType.GNSS_ATTITUDE_OUTPUT
     MESSAGE_VERSION = 0
 
-    _STRUCT = struct.Struct('<B3xI3f3f')
+    _STRUCT = struct.Struct('<B3xI3f3f2f')
 
     def __init__(self):
         ## Measurement timestamps, if available. See @ref measurement_messages.
@@ -1198,6 +1198,14 @@ def __init__(self):
         # The standard deviation of the orientation measurement (in degrees).
         self.ypr_std_deg = np.full((3,), 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 pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = True) -> (bytes, int):
         if buffer is None:
             buffer = bytearray(self.calcsize())
@@ -1216,7 +1224,9 @@ def pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = True
             self.ypr_deg[2],
             self.ypr_std_deg[0],
             self.ypr_std_deg[1],
-            self.ypr_std_deg[2])
+            self.ypr_std_deg[2],
+            self.baseline_distance_m,
+            self.baseline_distance_std_m)
         offset += self._STRUCT.size
 
         if return_buffer:
@@ -1236,7 +1246,9 @@ def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessageP
          self.ypr_deg[2],
          self.ypr_std_deg[0],
          self.ypr_std_deg[1],
-         self.ypr_std_deg[2]) = \
+         self.ypr_std_deg[2],
+         self.baseline_distance_m,
+         self.baseline_distance_std_m) = \
             self._STRUCT.unpack_from(buffer, offset)
         offset += self._STRUCT.size
 
@@ -1247,15 +1259,18 @@ def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessageP
     def __repr__(self):
         result = super().__repr__()[:-1]
         ypr_str = '(%.1f, %.1f, %.1f)' % tuple(self.ypr_deg)
-        result += f', solution_type={self.solution_type}, ypr={ypr_str} deg]'
+        result += f', solution_type={self.solution_type}, ypr={ypr_str} deg, ' \
+                  f'baseline={self.baseline_distance_m} m]'
         return result
 
     def __str__(self):
         return f"""\
 GNSS Attitude Output @ {str(self.details.p1_time)}
   Solution Type: {self.solution_type}
   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}"""
+  YPR std (deg): {self.ypr_std_deg[0]:.2f}, {self.ypr_std_deg[1]:.2f}, {self.ypr_std_deg[2]:.2f}
+  Baseline distance (m): {self.baseline_distance_m:.2f}
+  Baseline std (m): {self.baseline_distance_std_m:.2f}"""
 
     @classmethod
     def calcsize(cls) -> int:
@@ -1268,6 +1283,8 @@ def to_numpy(cls, messages: Sequence['GNSSAttitudeOutput']):
             '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,
             'ypr_std_deg': np.array([m.ypr_std_deg for m in messages]).T,
+            '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/measurements.h

@@ -1200,6 +1200,16 @@ struct P1_ALIGNAS(4) GNSSAttitudeOutput : public MessagePayload {
    * The standard deviation of the orientation measurement (in degrees).
    */
   float ypr_std_deg[3] = {NAN, NAN, NAN};
+
+  /**
+   * The estimated distance between primary and secondary antennas (in meters).
+   */
+  float baseline_distance_m = NAN;
+
+  /**
+   * The standard deviation of the baseline distance estimate (in meters).
+   */
+  float baseline_distance_std_m = NAN;
 };
 
 /**