motion_lid: prevent angle 0 <-> 360 transition.

When lid is closed, the lid angle can move to 358, 360, 0, 359 ...
Prevent transition 0 from/to 360 by keeping the last calculated value.

BRANCH=kevin
BUG=chrome-os-partner:55702
TEST=Check transition does not happen anymore.

Change-Id: Ifa8415470f425c893e2c3662c84c8fd0156e0524
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/373040
Reviewed-by: Aseda Aboagye <aaboagye@chromium.org>

4 files changed, 69 insertions, 7 deletions

diff --git a/common/motion_lid.c b/common/motion_lid.c
index d4cae6be08..d46173918c 100644
--- a/common/motion_lid.c
+++ b/common/motion_lid.c
@@ -26,8 +26,14 @@
 #define CPRINTS(format, args...) cprints(CC_MOTION_LID, format, ## args)
 #define CPRINTF(format, args...) cprintf(CC_MOTION_LID, format, ## args)
 
+#ifdef CONFIG_LID_ANGLE_INVALID_CHECK
+/* Previous lid_angle. */
+static fp_t last_lid_angle_fp = FLOAT_TO_FP(-1);
+#endif
+
 /* Current acceleration vectors and current lid angle. */
 static int lid_angle_deg;
+
 static int lid_angle_is_reliable;
 
 /*
@@ -39,6 +45,12 @@ static int lid_angle_is_reliable;
 #define HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD FLOAT_TO_FP(0.96593)
 
 /*
+ * Constant to debounce lid angle changes around 360 - 0:
+ * If we have a rotation  through the angle 0, ignore.
+ */
+#define DEBOUNCE_ANGLE_DELTA FLOAT_TO_FP(20)
+
+/*
  * Define the accelerometer orientation matrices based on the standard
  * reference frame in use (note: accel data is converted to standard ref
  * frame before calculating lid angle).
@@ -97,7 +109,7 @@ static int calculate_lid_angle(const vector_3_t base, const vector_3_t lid,
 			       int *lid_angle)
 {
 	vector_3_t v;
-	fp_t ang_lid_to_base, cos_lid_90, cos_lid_270;
+	fp_t lid_to_base_fp, cos_lid_90, cos_lid_270;
 	fp_t lid_to_base, base_to_hinge;
 	fp_t denominator;
 	int reliable = 1;
@@ -128,8 +140,8 @@ static int calculate_lid_angle(const vector_3_t base, const vector_3_t lid,
 		return 0;
 	}
 
-	ang_lid_to_base = arc_cos(fp_div(lid_to_base - base_to_hinge,
-					 denominator));
+	lid_to_base_fp = arc_cos(fp_div(lid_to_base - base_to_hinge,
+					denominator));
 
 	/*
 	 * The previous calculation actually has two solutions, a positive and
@@ -155,18 +167,34 @@ static int calculate_lid_angle(const vector_3_t base, const vector_3_t lid,
 	 * decreasing, the logic of this comparison is reversed.
 	 */
 	if (cos_lid_270 > cos_lid_90)
-		ang_lid_to_base = -ang_lid_to_base;
+		lid_to_base_fp = -lid_to_base_fp;
 
 	/* Place lid angle between 0 and 360 degrees. */
-	if (ang_lid_to_base < 0)
-		ang_lid_to_base += FLOAT_TO_FP(360);
+	if (lid_to_base_fp < 0)
+		lid_to_base_fp += FLOAT_TO_FP(360);
+
+#ifdef CONFIG_LID_ANGLE_INVALID_CHECK
+	/* Check if we have a sudden rotation from 360 <-> 0 */
+	if (last_lid_angle_fp >= 0 &&
+	    ((FLOAT_TO_FP(360) - last_lid_angle_fp < DEBOUNCE_ANGLE_DELTA &&
+	      lid_to_base_fp < DEBOUNCE_ANGLE_DELTA) ||
+	     (FLOAT_TO_FP(360) - lid_to_base_fp < DEBOUNCE_ANGLE_DELTA &&
+	      last_lid_angle_fp < DEBOUNCE_ANGLE_DELTA)))
+		CPRINTS("ignore transition: %d to %d",
+			FP_TO_INT(last_lid_angle_fp),
+			FP_TO_INT(lid_to_base_fp));
+	else
+		last_lid_angle_fp = lid_to_base_fp;
 
 	/*
 	 * Round to nearest int by adding 0.5. Note, only works because lid
 	 * angle is known to be positive.
 	 */
-	*lid_angle = FP_TO_INT(ang_lid_to_base + FLOAT_TO_FP(0.5));
+	*lid_angle = FP_TO_INT(last_lid_angle_fp + FLOAT_TO_FP(0.5));
 
+#else    /* CONFIG_LID_ANGLE_INVALID_CHECK */
+	*lid_angle = FP_TO_INT(lid_to_base_fp + FLOAT_TO_FP(0.5));
+#endif
 	return reliable;
 }
 
diff --git a/include/config.h b/include/config.h
index 650a38812d..31912eee94 100644
--- a/include/config.h
+++ b/include/config.h
@@ -1059,6 +1059,12 @@
 /* Do we want to detect the lid angle? */
 #undef CONFIG_LID_ANGLE
 
+/*
+ * Add code for preventing 0 and 360 degree transition. Needed when
+ * Device supports tablet mode.
+ */
+#undef CONFIG_LID_ANGLE_INVALID_CHECK
+
 /* Which sensor is located on the base? */
 #undef CONFIG_LID_ANGLE_SENSOR_BASE
 /* Which sensor is located on the lid? */
diff --git a/test/motion_lid.c b/test/motion_lid.c
index 26eff7f1a4..f610250e9c 100644
--- a/test/motion_lid.c
+++ b/test/motion_lid.c
@@ -230,6 +230,33 @@ static int test_lid_angle(void)
 	wait_for_valid_sample();
 	TEST_ASSERT(motion_lid_get_angle() == 225);
 
+	/* Set lid open to 350 */
+	lid->xyz[X] = 0;
+	lid->xyz[Y] = -173;
+	lid->xyz[Z] = -984;
+	wait_for_valid_sample();
+	TEST_ASSERT(motion_lid_get_angle() == 350);
+
+	/* Set lid open to 10, check rotation did not change. */
+	lid->xyz[X] = 0;
+	lid->xyz[Y] = 173;
+	lid->xyz[Z] = -984;
+	wait_for_valid_sample();
+	TEST_ASSERT(motion_lid_get_angle() == 350);
+
+	/* Rotate back to 180 and then 10 */
+	lid->xyz[X] = 0;
+	lid->xyz[Y] = 0;
+	lid->xyz[Z] = 1000;
+	wait_for_valid_sample();
+	TEST_ASSERT(motion_lid_get_angle() == 180);
+
+	lid->xyz[X] = 0;
+	lid->xyz[Y] = 173;
+	lid->xyz[Z] = -984;
+	wait_for_valid_sample();
+	TEST_ASSERT(motion_lid_get_angle() == 10);
+
 	/*
 	 * Align base with hinge and make sure it returns unreliable for angle.
 	 * In this test it doesn't matter what the lid acceleration vector is.
diff --git a/test/test_config.h b/test/test_config.h
index dacfe04868..ffb8e877f4 100644
--- a/test/test_config.h
+++ b/test/test_config.h
@@ -44,6 +44,7 @@
 
 #ifdef TEST_MOTION_LID
 #define CONFIG_LID_ANGLE
+#define CONFIG_LID_ANGLE_INVALID_CHECK
 #define CONFIG_LID_ANGLE_SENSOR_BASE 0
 #define CONFIG_LID_ANGLE_SENSOR_LID 1
 #endif