Add bitmask in PoseMessage to signal stationary status of device (#331

)

# New Features
- `PoseMessage` now has `flags member, which currently signals whether
the device is stationary or not
- Add `Stationary Status` plotting tool

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=['Moving', 'Stationary'],
+                                          tickvals=[0, PoseMessage.FLAG_STATIONARY])
+
+        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()

python/fusion_engine_client/messages/solution.py

@@ -18,14 +18,18 @@ class PoseMessage(MessagePayload):
 
     INVALID_UNDULATION = -32768
 
-    _STRUCT = struct.Struct('<Bx h ddd fff ddd fff ddd fff fff')
+    FLAG_STATIONARY = 0x1
+
+    _STRUCT = struct.Struct('<BB h ddd fff ddd fff ddd fff fff')
 
     def __init__(self):
         self.p1_time = Timestamp()
         self.gps_time = Timestamp()
 
         self.solution_type = SolutionType.Invalid
 
+        self.flags = 0x0
+
         # Added in version 1.1.
         self.undulation_m = np.nan
 
@@ -60,6 +64,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.flags,
             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 +89,7 @@ def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessageP
         offset += self.gps_time.unpack(buffer, offset)
 
         (solution_type_int,
+         flags,
          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 +110,8 @@ def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessageP
 
         self.solution_type = SolutionType(solution_type_int)
 
+        self.flags = flags
+
         return offset - initial_offset
 
     def __repr__(self):
@@ -152,6 +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], 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,

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;
 
+  /** 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). */
   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;
+  /** A bitmask of flags associated with the pose data. */
+  uint8_t flags = 0x0;
 
   /**
    * The geoid undulation at at the current location (i.e., the difference