Improve heuristics for orientation detection.

1. Except as otherwise indicated, orientation change happens once
   the predicted rotation has been stable for 40ms.  Noise is
   suppressed by a low-pass filter with a 200ms time constant which
   seems to be about as small as is practical given the quality
   of the sensor data.

2. If the magnitude exceeds a threshold (excessive noise or freefall),
   resets the predicted orientation.
   Doesn't happen very often even when shaking the device.
   This heuristic mainly protects the detector from spurious tilt due
   to inaccurate determination of the gravity vector.

3. If the device was previously in a flat posture (on a table for at
   least 1000ms), then it must move out of that posture for at least
   500ms before the next orientation change will happen.
   This heuristic suppresses most spurious rotations that happen while
   picking up the device.

4. If the device is tilted away from the user by 20 degrees within
   a span of 300ms, the device is said to be swinging and at least
   300ms must elapse after the device stops swinging before the
   next orientation change will happen.
   This heuristic suppresses some but not all spurious rotations that
   happen while putting down a device.  Unfortunately, this heuristic
   sometimes triggers a false positive when turning the device very
   rapidly due to accelerometer noise.  The 300ms pause is a compromise
   so that occasional mispredicted swings don't significantly delay
   the rotation.

Bug: 5796249
Change-Id: Id7b36c4c563e35b70d6a7ac36d04f3c3d6ea5811

1 files changed, 69 insertions, 47 deletions

diff --git a/tools/orientationplot/orientationplot.py b/tools/orientationplot/orientationplot.py
index 3a44cb2..f4e6b45 100755
--- a/tools/orientationplot/orientationplot.py
+++ b/tools/orientationplot/orientationplot.py
@@ -82,6 +82,7 @@ class Plotter:
     self.raw_acceleration_x = self._make_timeseries()
     self.raw_acceleration_y = self._make_timeseries()
     self.raw_acceleration_z = self._make_timeseries()
+    self.raw_acceleration_magnitude = self._make_timeseries()
     self.raw_acceleration_axes = self._add_timeseries_axes(
         1, 'Raw Acceleration', 'm/s^2', [-20, 20],
         yticks=range(-15, 16, 5))
@@ -91,6 +92,8 @@ class Plotter:
         self.raw_acceleration_axes, 'y', 'green')
     self.raw_acceleration_line_z = self._add_timeseries_line(
         self.raw_acceleration_axes, 'z', 'blue')
+    self.raw_acceleration_line_magnitude = self._add_timeseries_line(
+        self.raw_acceleration_axes, 'magnitude', 'orange', linewidth=2)
     self._add_timeseries_legend(self.raw_acceleration_axes)
 
     shared_axis = self.raw_acceleration_axes
@@ -98,7 +101,7 @@ class Plotter:
     self.filtered_acceleration_x = self._make_timeseries()
     self.filtered_acceleration_y = self._make_timeseries()
     self.filtered_acceleration_z = self._make_timeseries()
-    self.magnitude = self._make_timeseries()
+    self.filtered_acceleration_magnitude = self._make_timeseries()
     self.filtered_acceleration_axes = self._add_timeseries_axes(
         2, 'Filtered Acceleration', 'm/s^2', [-20, 20],
         sharex=shared_axis,
@@ -109,7 +112,7 @@ class Plotter:
         self.filtered_acceleration_axes, 'y', 'green')
     self.filtered_acceleration_line_z = self._add_timeseries_line(
         self.filtered_acceleration_axes, 'z', 'blue')
-    self.magnitude_line = self._add_timeseries_line(
+    self.filtered_acceleration_line_magnitude = self._add_timeseries_line(
         self.filtered_acceleration_axes, 'magnitude', 'orange', linewidth=2)
     self._add_timeseries_legend(self.filtered_acceleration_axes)
 
@@ -133,32 +136,46 @@ class Plotter:
 
     self.current_rotation = self._make_timeseries()
     self.proposed_rotation = self._make_timeseries()
-    self.proposal_rotation = self._make_timeseries()
+    self.predicted_rotation = self._make_timeseries()
     self.orientation_axes = self._add_timeseries_axes(
-        5, 'Current / Proposed Orientation and Confidence', 'rotation', [-1, 4],
+        5, 'Current / Proposed Orientation', 'rotation', [-1, 4],
         sharex=shared_axis,
         yticks=range(0, 4))
     self.current_rotation_line = self._add_timeseries_line(
         self.orientation_axes, 'current', 'black', linewidth=2)
-    self.proposal_rotation_line = self._add_timeseries_line(
-        self.orientation_axes, 'proposal', 'purple', linewidth=3)
+    self.predicted_rotation_line = self._add_timeseries_line(
+        self.orientation_axes, 'predicted', 'purple', linewidth=3)
     self.proposed_rotation_line = self._add_timeseries_line(
         self.orientation_axes, 'proposed', 'green', linewidth=3)
     self._add_timeseries_legend(self.orientation_axes)
 
-    self.proposal_confidence = [[self._make_timeseries(), self._make_timeseries()]
-      for i in range(0, 4)]
-    self.proposal_confidence_polys = []
+    self.time_until_settled = self._make_timeseries()
+    self.time_until_flat_delay_expired = self._make_timeseries()
+    self.time_until_swing_delay_expired = self._make_timeseries()
+    self.stability_axes = self._add_timeseries_axes(
+        6, 'Proposal Stability', 'ms', [-10, 600],
+        sharex=shared_axis,
+        yticks=range(0, 600, 100))
+    self.time_until_settled_line = self._add_timeseries_line(
+        self.stability_axes, 'time until settled', 'black', linewidth=2)
+    self.time_until_flat_delay_expired_line = self._add_timeseries_line(
+        self.stability_axes, 'time until flat delay expired', 'green')
+    self.time_until_swing_delay_expired_line = self._add_timeseries_line(
+        self.stability_axes, 'time until swing delay expired', 'blue')
+    self._add_timeseries_legend(self.stability_axes)
 
     self.sample_latency = self._make_timeseries()
     self.sample_latency_axes = self._add_timeseries_axes(
-        6, 'Accelerometer Sampling Latency', 'ms', [-10, 500],
+        7, 'Accelerometer Sampling Latency', 'ms', [-10, 500],
         sharex=shared_axis,
         yticks=range(0, 500, 100))
     self.sample_latency_line = self._add_timeseries_line(
         self.sample_latency_axes, 'latency', 'black')
     self._add_timeseries_legend(self.sample_latency_axes)
 
+    self.fig.canvas.mpl_connect('button_press_event', self._on_click)
+    self.paused = False
+
     self.timer = self.fig.canvas.new_timer(interval=100)
     self.timer.add_callback(lambda: self.update())
     self.timer.start()
@@ -166,13 +183,22 @@ class Plotter:
     self.timebase = None
     self._reset_parse_state()
 
+  # Handle a click event to pause or restart the timer.
+  def _on_click(self, ev):
+    if not self.paused:
+      self.paused = True
+      self.timer.stop()
+    else:
+      self.paused = False
+      self.timer.start()
+
   # Initialize a time series.
   def _make_timeseries(self):
     return [[], []]
 
   # Add a subplot to the figure for a time series.
   def _add_timeseries_axes(self, index, title, ylabel, ylim, yticks, sharex=None):
-    num_graphs = 6
+    num_graphs = 7
     height = 0.9 / num_graphs
     top = 0.95 - height * index
     axes = self.fig.add_axes([0.1, top, 0.8, height],
@@ -214,16 +240,19 @@ class Plotter:
     self.parse_raw_acceleration_x = None
     self.parse_raw_acceleration_y = None
     self.parse_raw_acceleration_z = None
+    self.parse_raw_acceleration_magnitude = None
     self.parse_filtered_acceleration_x = None
     self.parse_filtered_acceleration_y = None
     self.parse_filtered_acceleration_z = None
-    self.parse_magnitude = None
+    self.parse_filtered_acceleration_magnitude = None
     self.parse_tilt_angle = None
     self.parse_orientation_angle = None
     self.parse_current_rotation = None
     self.parse_proposed_rotation = None
-    self.parse_proposal_rotation = None
-    self.parse_proposal_confidence = None
+    self.parse_predicted_rotation = None
+    self.parse_time_until_settled = None
+    self.parse_time_until_flat_delay_expired = None
+    self.parse_time_until_swing_delay_expired = None
     self.parse_sample_latency = None
 
   # Update samples.
@@ -252,14 +281,13 @@ class Plotter:
         self.parse_raw_acceleration_x = self._get_following_number(line, 'x=')
         self.parse_raw_acceleration_y = self._get_following_number(line, 'y=')
         self.parse_raw_acceleration_z = self._get_following_number(line, 'z=')
+        self.parse_raw_acceleration_magnitude = self._get_following_number(line, 'magnitude=')
 
       if line.find('Filtered acceleration vector:') != -1:
         self.parse_filtered_acceleration_x = self._get_following_number(line, 'x=')
         self.parse_filtered_acceleration_y = self._get_following_number(line, 'y=')
         self.parse_filtered_acceleration_z = self._get_following_number(line, 'z=')
-
-      if line.find('magnitude=') != -1:
-        self.parse_magnitude = self._get_following_number(line, 'magnitude=')
+        self.parse_filtered_acceleration_magnitude = self._get_following_number(line, 'magnitude=')
 
       if line.find('tiltAngle=') != -1:
         self.parse_tilt_angle = self._get_following_number(line, 'tiltAngle=')
@@ -270,17 +298,20 @@ class Plotter:
       if line.find('Result:') != -1:
         self.parse_current_rotation = self._get_following_number(line, 'currentRotation=')
         self.parse_proposed_rotation = self._get_following_number(line, 'proposedRotation=')
-        self.parse_proposal_rotation = self._get_following_number(line, 'proposalRotation=')
-        self.parse_proposal_confidence = self._get_following_number(line, 'proposalConfidence=')
+        self.parse_predicted_rotation = self._get_following_number(line, 'predictedRotation=')
         self.parse_sample_latency = self._get_following_number(line, 'timeDeltaMS=')
+        self.parse_time_until_settled = self._get_following_number(line, 'timeUntilSettledMS=')
+        self.parse_time_until_flat_delay_expired = self._get_following_number(line, 'timeUntilFlatDelayExpiredMS=')
+        self.parse_time_until_swing_delay_expired = self._get_following_number(line, 'timeUntilSwingDelayExpiredMS=')
 
         self._append(self.raw_acceleration_x, timeindex, self.parse_raw_acceleration_x)
         self._append(self.raw_acceleration_y, timeindex, self.parse_raw_acceleration_y)
         self._append(self.raw_acceleration_z, timeindex, self.parse_raw_acceleration_z)
+        self._append(self.raw_acceleration_magnitude, timeindex, self.parse_raw_acceleration_magnitude)
         self._append(self.filtered_acceleration_x, timeindex, self.parse_filtered_acceleration_x)
         self._append(self.filtered_acceleration_y, timeindex, self.parse_filtered_acceleration_y)
         self._append(self.filtered_acceleration_z, timeindex, self.parse_filtered_acceleration_z)
-        self._append(self.magnitude, timeindex, self.parse_magnitude)
+        self._append(self.filtered_acceleration_magnitude, timeindex, self.parse_filtered_acceleration_magnitude)
         self._append(self.tilt_angle, timeindex, self.parse_tilt_angle)
         self._append(self.orientation_angle, timeindex, self.parse_orientation_angle)
         self._append(self.current_rotation, timeindex, self.parse_current_rotation)
@@ -288,17 +319,13 @@ class Plotter:
           self._append(self.proposed_rotation, timeindex, self.parse_proposed_rotation)
         else:
           self._append(self.proposed_rotation, timeindex, None)
-        if self.parse_proposal_rotation >= 0:
-          self._append(self.proposal_rotation, timeindex, self.parse_proposal_rotation)
+        if self.parse_predicted_rotation >= 0:
+          self._append(self.predicted_rotation, timeindex, self.parse_predicted_rotation)
         else:
-          self._append(self.proposal_rotation, timeindex, None)
-        for i in range(0, 4):
-          self._append(self.proposal_confidence[i][0], timeindex, i)
-          if i == self.parse_proposal_rotation:
-            self._append(self.proposal_confidence[i][1], timeindex,
-              i + self.parse_proposal_confidence)
-          else:
-            self._append(self.proposal_confidence[i][1], timeindex, i)
+          self._append(self.predicted_rotation, timeindex, None)
+        self._append(self.time_until_settled, timeindex, self.parse_time_until_settled)
+        self._append(self.time_until_flat_delay_expired, timeindex, self.parse_time_until_flat_delay_expired)
+        self._append(self.time_until_swing_delay_expired, timeindex, self.parse_time_until_swing_delay_expired)
         self._append(self.sample_latency, timeindex, self.parse_sample_latency)
         self._reset_parse_state()
 
@@ -309,45 +336,40 @@ class Plotter:
       self._scroll(self.raw_acceleration_x, bottom)
       self._scroll(self.raw_acceleration_y, bottom)
       self._scroll(self.raw_acceleration_z, bottom)
+      self._scroll(self.raw_acceleration_magnitude, bottom)
       self._scroll(self.filtered_acceleration_x, bottom)
       self._scroll(self.filtered_acceleration_y, bottom)
       self._scroll(self.filtered_acceleration_z, bottom)
-      self._scroll(self.magnitude, bottom)
+      self._scroll(self.filtered_acceleration_magnitude, bottom)
       self._scroll(self.tilt_angle, bottom)
       self._scroll(self.orientation_angle, bottom)
       self._scroll(self.current_rotation, bottom)
       self._scroll(self.proposed_rotation, bottom)
-      self._scroll(self.proposal_rotation, bottom)
-      for i in range(0, 4):
-        self._scroll(self.proposal_confidence[i][0], bottom)
-        self._scroll(self.proposal_confidence[i][1], bottom)
+      self._scroll(self.predicted_rotation, bottom)
+      self._scroll(self.time_until_settled, bottom)
+      self._scroll(self.time_until_flat_delay_expired, bottom)
+      self._scroll(self.time_until_swing_delay_expired, bottom)
       self._scroll(self.sample_latency, bottom)
 
     # Redraw the plots.
     self.raw_acceleration_line_x.set_data(self.raw_acceleration_x)
     self.raw_acceleration_line_y.set_data(self.raw_acceleration_y)
     self.raw_acceleration_line_z.set_data(self.raw_acceleration_z)
+    self.raw_acceleration_line_magnitude.set_data(self.raw_acceleration_magnitude)
     self.filtered_acceleration_line_x.set_data(self.filtered_acceleration_x)
     self.filtered_acceleration_line_y.set_data(self.filtered_acceleration_y)
     self.filtered_acceleration_line_z.set_data(self.filtered_acceleration_z)
-    self.magnitude_line.set_data(self.magnitude)
+    self.filtered_acceleration_line_magnitude.set_data(self.filtered_acceleration_magnitude)
     self.tilt_angle_line.set_data(self.tilt_angle)
     self.orientation_angle_line.set_data(self.orientation_angle)
     self.current_rotation_line.set_data(self.current_rotation)
     self.proposed_rotation_line.set_data(self.proposed_rotation)
-    self.proposal_rotation_line.set_data(self.proposal_rotation)
+    self.predicted_rotation_line.set_data(self.predicted_rotation)
+    self.time_until_settled_line.set_data(self.time_until_settled)
+    self.time_until_flat_delay_expired_line.set_data(self.time_until_flat_delay_expired)
+    self.time_until_swing_delay_expired_line.set_data(self.time_until_swing_delay_expired)
     self.sample_latency_line.set_data(self.sample_latency)
 
-    for poly in self.proposal_confidence_polys:
-      poly.remove()
-    self.proposal_confidence_polys = []
-    for i in range(0, 4):
-      self.proposal_confidence_polys.append(self.orientation_axes.fill_between(
-        self.proposal_confidence[i][0][0],
-        self.proposal_confidence[i][0][1],
-        self.proposal_confidence[i][1][1],
-        facecolor='goldenrod', edgecolor='goldenrod'))
-
     self.fig.canvas.draw_idle()
 
   # Scroll a time series.