From 7b0206663f1657a7eb20932d8e877af4710bbb58 Mon Sep 17 00:00:00 2001 From: William Brannon Date: Tue, 23 Jul 2024 12:30:57 -0600 Subject: [PATCH 1/6] Add bitmask to signal dynamic status of device --- src/point_one/fusion_engine/messages/solution.h | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/point_one/fusion_engine/messages/solution.h b/src/point_one/fusion_engine/messages/solution.h index c434d6ab..04d39d59 100644 --- a/src/point_one/fusion_engine/messages/solution.h +++ b/src/point_one/fusion_engine/messages/solution.h @@ -42,6 +42,9 @@ struct P1_ALIGNAS(4) PoseMessage : public MessagePayload { static constexpr uint8_t MESSAGE_VERSION = 1; static constexpr int16_t INVALID_UNDULATION = INT16_MIN; + static constexpr uint8_t NONSTATIONARY = 0x00000000; + static constexpr uint8_t STATIONARY = 0x00000001; + /** The time of the message, in P1 time (beginning at power-on). */ Timestamp p1_time; @@ -51,7 +54,8 @@ struct P1_ALIGNAS(4) PoseMessage : public MessagePayload { /** The type of this position solution. */ SolutionType solution_type; - uint8_t reserved = 0; + /** Signals device dynamic status (stationary or nonstationary). */ + uint8_t dynamic_status = NONSTATIONARY; /** * The geoid undulation at at the current location (i.e., the difference From 6127670017bc3c96bd401579663c84bc1fb70780 Mon Sep 17 00:00:00 2001 From: William Brannon Date: Tue, 23 Jul 2024 13:56:07 -0600 Subject: [PATCH 2/6] Add dynamic status to Python `PoseMessage` --- python/fusion_engine_client/messages/solution.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/python/fusion_engine_client/messages/solution.py b/python/fusion_engine_client/messages/solution.py index b5bd7a6b..e17cc28b 100644 --- a/python/fusion_engine_client/messages/solution.py +++ b/python/fusion_engine_client/messages/solution.py @@ -26,6 +26,8 @@ def __init__(self): self.solution_type = SolutionType.Invalid + self.dynamic_status = np.nan + # Added in version 1.1. self.undulation_m = np.nan @@ -60,6 +62,7 @@ def pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = True self._STRUCT.pack_into( buffer, offset, int(self.solution_type), + self.dynamic_status, undulation_cm, self.lla_deg[0], self.lla_deg[1], self.lla_deg[2], self.position_std_enu_m[0], self.position_std_enu_m[1], self.position_std_enu_m[2], @@ -84,6 +87,7 @@ def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessageP offset += self.gps_time.unpack(buffer, offset) (solution_type_int, + dynamic_status, undulation_cm, self.lla_deg[0], self.lla_deg[1], self.lla_deg[2], self.position_std_enu_m[0], self.position_std_enu_m[1], self.position_std_enu_m[2], @@ -104,6 +108,8 @@ def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessageP self.solution_type = SolutionType(solution_type_int) + self.dynamic_status = dynamic_status + return offset - initial_offset def __repr__(self): @@ -152,6 +158,7 @@ def to_numpy(cls, messages): 'p1_time': np.array([float(m.p1_time) for m in messages]), 'gps_time': np.array([float(m.gps_time) for m in messages]), 'solution_type': np.array([int(m.solution_type) for m in messages], dtype=int), + 'dynamic_status': np.array([m.dynamic_status for m in messages]), 'undulation': np.array([m.undulation_m for m in messages]), 'lla_deg': np.array([m.lla_deg for m in messages]).T, 'ypr_deg': np.array([m.ypr_deg for m in messages]).T, From d0189b8c2f0a8468563d845c4eda9bd33692e32f Mon Sep 17 00:00:00 2001 From: William Brannon Date: Wed, 24 Jul 2024 12:08:55 -0600 Subject: [PATCH 3/6] dynamic_status -> flags --- python/fusion_engine_client/messages/solution.py | 12 +++++++----- src/point_one/fusion_engine/messages/solution.h | 8 ++++---- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/python/fusion_engine_client/messages/solution.py b/python/fusion_engine_client/messages/solution.py index e17cc28b..456d895c 100644 --- a/python/fusion_engine_client/messages/solution.py +++ b/python/fusion_engine_client/messages/solution.py @@ -18,6 +18,8 @@ class PoseMessage(MessagePayload): INVALID_UNDULATION = -32768 + FLAG_STATIONARY = 0x1 + _STRUCT = struct.Struct(' Date: Wed, 24 Jul 2024 13:49:45 -0600 Subject: [PATCH 4/6] Update `PoseMessage` struct string to incorporate `flags` parameter --- python/fusion_engine_client/messages/solution.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/fusion_engine_client/messages/solution.py b/python/fusion_engine_client/messages/solution.py index 456d895c..0fba45f3 100644 --- a/python/fusion_engine_client/messages/solution.py +++ b/python/fusion_engine_client/messages/solution.py @@ -20,7 +20,7 @@ class PoseMessage(MessagePayload): FLAG_STATIONARY = 0x1 - _STRUCT = struct.Struct(' Date: Wed, 24 Jul 2024 13:50:14 -0600 Subject: [PATCH 5/6] Add plot for stationary status --- .../fusion_engine_client/analysis/analyzer.py | 34 +++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/python/fusion_engine_client/analysis/analyzer.py b/python/fusion_engine_client/analysis/analyzer.py index 4359c26f..a1e49996 100755 --- a/python/fusion_engine_client/analysis/analyzer.py +++ b/python/fusion_engine_client/analysis/analyzer.py @@ -724,6 +724,39 @@ def plot_solution_type(self): self._add_figure(name="solution_type", figure=figure, title="Solution Type") + def plot_stationary_status(self): + """! + @brief Plot the staionary status over time. + """ + if self.output_dir is None: + return + + # Read the pose data. + result = self.reader.read(message_types=[PoseMessage], source_ids=self.default_source_id, **self.params) + pose_data = result[PoseMessage.MESSAGE_TYPE] + + if len(pose_data.p1_time) == 0: + self.logger.info('No pose data available. Skipping solution type plot.') + return + + # Set up the figure. + figure = make_subplots(rows=1, cols=1, print_grid=False, shared_xaxes=True, subplot_titles=['Stationary Status']) + + figure['layout']['xaxis'].update(title=self.p1_time_label) + figure['layout']['yaxis1'].update(title="Stationary Status", + ticktext=['Nonstationary', 'Stationary'], + tickvals=[0, 1]) + + time = pose_data.p1_time - float(self.t0) + + # Extract the stationary status from the pose data flags. + stationary_status = pose_data.flags & PoseMessage.FLAG_STATIONARY + + text = ["Time: %.3f sec (%.3f sec)" % (t, t + float(self.t0)) for t in time] + figure.add_trace(go.Scattergl(x=time, y=stationary_status, text=text, mode='markers'), 1, 1) + + self._add_figure(name="stationary_status", figure=figure, title="Stationary Status") + def _plot_displacement(self, source, time, solution_type, displacement_enu_m, std_enu_m, title='Displacement'): """! @@ -2944,6 +2977,7 @@ def main(): analyzer.plot_time_scale() analyzer.plot_solution_type() + analyzer.plot_stationary_status() analyzer.plot_reset_timing() analyzer.plot_pose() analyzer.plot_pose_displacement() From 7ece89e407ba1e46c9996cb2a65d4524c8c38c67 Mon Sep 17 00:00:00 2001 From: William Brannon Date: Thu, 25 Jul 2024 12:19:21 -0600 Subject: [PATCH 6/6] Changes for review --- python/fusion_engine_client/analysis/analyzer.py | 4 ++-- python/fusion_engine_client/messages/solution.py | 2 +- src/point_one/fusion_engine/messages/solution.h | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/python/fusion_engine_client/analysis/analyzer.py b/python/fusion_engine_client/analysis/analyzer.py index a1e49996..c8717f30 100755 --- a/python/fusion_engine_client/analysis/analyzer.py +++ b/python/fusion_engine_client/analysis/analyzer.py @@ -744,8 +744,8 @@ def plot_stationary_status(self): figure['layout']['xaxis'].update(title=self.p1_time_label) figure['layout']['yaxis1'].update(title="Stationary Status", - ticktext=['Nonstationary', 'Stationary'], - tickvals=[0, 1]) + ticktext=['Moving', 'Stationary'], + tickvals=[0, PoseMessage.FLAG_STATIONARY]) time = pose_data.p1_time - float(self.t0) diff --git a/python/fusion_engine_client/messages/solution.py b/python/fusion_engine_client/messages/solution.py index 0fba45f3..b3eb8ae5 100644 --- a/python/fusion_engine_client/messages/solution.py +++ b/python/fusion_engine_client/messages/solution.py @@ -160,7 +160,7 @@ def to_numpy(cls, messages): 'p1_time': np.array([float(m.p1_time) for m in messages]), 'gps_time': np.array([float(m.gps_time) for m in messages]), 'solution_type': np.array([int(m.solution_type) for m in messages], dtype=int), - 'flags': np.array([m.flags for m in messages]), + 'flags': np.array([m.flags for m in messages], dtype=int), 'undulation': np.array([m.undulation_m for m in messages]), 'lla_deg': np.array([m.lla_deg for m in messages]).T, 'ypr_deg': np.array([m.ypr_deg for m in messages]).T, diff --git a/src/point_one/fusion_engine/messages/solution.h b/src/point_one/fusion_engine/messages/solution.h index d20018e8..d3b45cc7 100644 --- a/src/point_one/fusion_engine/messages/solution.h +++ b/src/point_one/fusion_engine/messages/solution.h @@ -42,7 +42,7 @@ struct P1_ALIGNAS(4) PoseMessage : public MessagePayload { static constexpr uint8_t MESSAGE_VERSION = 1; static constexpr int16_t INVALID_UNDULATION = INT16_MIN; - /** Set this flag if the device is stationary. */ + /** Set if the device is stationary. */ static constexpr uint8_t FLAG_STATIONARY = 0x01; /** The time of the message, in P1 time (beginning at power-on). */