diff options
author | Gwendal Grignou <gwendal@chromium.org> | 2016-08-18 15:34:07 -0700 |
---|---|---|
committer | chrome-bot <chrome-bot@chromium.org> | 2016-09-09 12:33:18 -0700 |
commit | 2b82ad78004e8ce37272743792acdb347a5a1001 (patch) | |
tree | f999255f32ccdff5dcfed9ae2f5e3115e98aed49 /common/motion_lid.c | |
parent | 00a0353a8860ec362d94c2ac3924142426fe06ac (diff) | |
download | chrome-ec-2b82ad78004e8ce37272743792acdb347a5a1001.tar.gz |
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>
Diffstat (limited to 'common/motion_lid.c')
-rw-r--r-- | common/motion_lid.c | 42 |
1 files changed, 35 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; } |