diff --git a/python/fusion_engine_client/analysis/analyzer.py b/python/fusion_engine_client/analysis/analyzer.py index c9e68878..ac0cbc7c 100755 --- a/python/fusion_engine_client/analysis/analyzer.py +++ b/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='Time: %{x:.3f} sec (%{customdata:.3f} sec)' + '
Baseline: %{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) diff --git a/python/fusion_engine_client/messages/measurements.py b/python/fusion_engine_client/messages/measurements.py index e45ce835..f95c9dae 100644 --- a/python/fusion_engine_client/messages/measurements.py +++ b/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(' (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,7 +1259,8 @@ 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): @@ -1255,7 +1268,9 @@ def __str__(self): 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 diff --git a/src/point_one/fusion_engine/messages/measurements.h b/src/point_one/fusion_engine/messages/measurements.h index a1e766e7..5076cfef 100644 --- a/src/point_one/fusion_engine/messages/measurements.h +++ b/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; }; /**