diff options
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; } |