diff options
author | Gwendal Grignou <gwendal@chromium.org> | 2018-12-17 15:14:00 -0800 |
---|---|---|
committer | chrome-bot <chrome-bot@chromium.org> | 2019-03-26 17:27:00 -0700 |
commit | 40f9e2fc0f2354e033553131fd63cfb9f755a2e2 (patch) | |
tree | 1498d7190aece44697f3bee0e797565391f99750 /common | |
parent | 4a48404aeea58bfe0c31d5fea554b5329a223131 (diff) | |
download | chrome-ec-40f9e2fc0f2354e033553131fd63cfb9f755a2e2.tar.gz |
motion_lid: Rewrite lid angle calculation based on chromium code
Use code from ash/wm/tablet_mode/tablet_mode_controller.cc, in
particular TabletModeController::HandleHingeRotation()
to calculate lid angle.
Add unit tests based on
ash/wm/tablet_mode/tablet_mode_controller_unittest.cc and the data file
accelerometer_test_data_literals.cc.
BUG=b:120346412
BRANCH=none
TEST=Check unit tests pass, check it compile on FPU based EC, EC without
FPU and no 64 bit support (ampton).
Check lid calculation is correct on eve:
- with "while true ; do ectool motionsense lid_angle ; sleep 1 ; done"
Check when hinge is almost vertical lid angle is close to constant or
marked are unrieliable.
Check when shaking device, lid angle is also unreliable
Check with evtest SW_TABLET_MODE event is trigger when lid angle is
available and cross 180 region.
Change-Id: I545f7333ed9b53accedb75f238f747f66bae1f5d
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/1388844
Commit-Ready: ChromeOS CL Exonerator Bot <chromiumos-cl-exonerator@appspot.gserviceaccount.com>
Reviewed-by: Jett Rink <jettrink@chromium.org>
Diffstat (limited to 'common')
-rw-r--r-- | common/motion_lid.c | 357 |
1 files changed, 157 insertions, 200 deletions
diff --git a/common/motion_lid.c b/common/motion_lid.c index 4513df1e48..dd3506a3b8 100644 --- a/common/motion_lid.c +++ b/common/motion_lid.c @@ -38,7 +38,7 @@ static fp_t last_lid_angle_fp = FLOAT_TO_FP(-1); * measurements when the lid is physically closed. This will be used in * reliability calculations. */ -#define SMALL_LID_ANGLE_RANGE 15 +#define SMALL_LID_ANGLE_RANGE (FLOAT_TO_FP(15)) #endif /* Current acceleration vectors and current lid angle. */ @@ -46,15 +46,15 @@ static int lid_angle_deg; static int lid_angle_is_reliable; -/* - * Angle threshold for how close the hinge aligns with gravity before - * considering the lid angle calculation unreliable. For computational - * efficiency, value is given unit-less, so if you want the threshold to be - * at 15 degrees, the value would be cos(15 deg) = 0.96593. - * - * Here we're using cos(27.5 deg) = 0.88701. - */ -#define HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD FLOAT_TO_FP(0.88701) +/* Smoothed vectors to increase accurency. */ +static intv3_t smoothed_base, smoothed_lid; + +/* 8.7 m/s^2 is the the maximum acceleration parallel to the hinge */ +#define SCALED_HINGE_VERTICAL_MAXIMUM \ + ((int)((8.7f * MOTION_SCALING_FACTOR) / MOTION_ONE_G)) + +#define SCALED_HINGE_VERTICAL_SMOOTHING_START \ + ((int)((7.0f * MOTION_SCALING_FACTOR) / MOTION_ONE_G)) /* * Constant to debounce lid angle changes around 360 - 0: @@ -67,9 +67,20 @@ static int lid_angle_is_reliable; * under the same acceleration. This constant, which mirrors * kNoisyMagnitudeDeviation used in Chromium, is an integer which defines the * maximum deviation in magnitude between the base and lid vectors. The units - * are in m/s^2. + * are in g. Currently set at 1m/s^2. */ -#define NOISY_MAGNITUDE_DEVIATION 1 +#define NOISY_MAGNITUDE_DEVIATION ((int)(MOTION_SCALING_FACTOR / MOTION_ONE_G)) + +/* + * Even with noise, any measurement greater than 1g on any axis is not suitable + * for lid calculation. It means the device is moving. + * To avoid using 64bits arithmetic, we need to be sure that square of magnitude + * is less than 1<<31, so magnitude is less sqrt(2)*(1<<15), less than ~40% over + * 1g. This is way above any usable noise. Assume noise is less than 10%. + */ +#define MOTION_SCALING_AXIS_MAX (MOTION_SCALING_FACTOR * 110) + +#define MOTION_SCALING_FACTOR2 (MOTION_SCALING_FACTOR * MOTION_SCALING_FACTOR) /* * Define the accelerometer orientation matrices based on the standard @@ -77,43 +88,16 @@ static int lid_angle_is_reliable; * frame before calculating lid angle). */ #ifdef CONFIG_ACCEL_STD_REF_FRAME_OLD -const struct accel_orientation acc_orient = { - /* Hinge aligns with y axis. */ - .rot_hinge_90 = { - { 0, 0, FLOAT_TO_FP(1)}, - { 0, FLOAT_TO_FP(1), 0}, - { FLOAT_TO_FP(-1), 0, 0} - }, - .rot_hinge_180 = { - { FLOAT_TO_FP(-1), 0, 0}, - { 0, FLOAT_TO_FP(1), 0}, - { 0, 0, FLOAT_TO_FP(-1)} - }, - .hinge_axis = {0, 1, 0}, -}; +static const intv3_t hinge_axis = { 0, 1, 0}; +#define HINGE_AXIS Y #else -const struct accel_orientation acc_orient = { - /* Hinge aligns with x axis. */ - .rot_hinge_90 = { - { FLOAT_TO_FP(1), 0, 0}, - { 0, 0, FLOAT_TO_FP(1)}, - { 0, FLOAT_TO_FP(-1), 0} - }, - .rot_hinge_180 = { - { FLOAT_TO_FP(1), 0, 0}, - { 0, FLOAT_TO_FP(-1), 0}, - { 0, 0, FLOAT_TO_FP(-1)} - }, - .hinge_axis = {1, 0, 0}, -}; +static const intv3_t hinge_axis = { 1, 0, 0}; +#define HINGE_AXIS X #endif -/* Pointer to constant acceleration orientation data. */ -const struct accel_orientation * const p_acc_orient = &acc_orient; - -const struct motion_sensor_t * const accel_base = +static const struct motion_sensor_t * const accel_base = &motion_sensors[CONFIG_LID_ANGLE_SENSOR_BASE]; -const struct motion_sensor_t * const accel_lid = +static const struct motion_sensor_t * const accel_lid = &motion_sensors[CONFIG_LID_ANGLE_SENSOR_LID]; #ifdef CONFIG_TABLET_MODE @@ -165,16 +149,6 @@ static fp_t laptop_zone_lid_angle = static int tablet_mode_lid_angle = DEFAULT_TABLET_MODE_ANGLE; static int tablet_mode_hys_degree = DEFAULT_TABLET_MODE_HYS; -/* - * We will change our tablet mode status when we are "convinced" that it has - * changed. This means we will have to consecutively calculate our new tablet - * mode while the angle is stable and come to the same conclusion. The number - * of consecutive calculations is the debounce count with an interval between - * readings set by the motion_sense task. This should avoid spurious forces - * that may trigger false transitions of the tablet mode switch. - */ -#define TABLET_MODE_DEBOUNCE_COUNT 3 - static void motion_lid_set_tablet_mode(int reliable) { static int tablet_mode_debounce_cnt = TABLET_MODE_DEBOUNCE_COUNT; @@ -305,109 +279,41 @@ static void motion_lid_set_dptf_profile(int reliable) static int calculate_lid_angle(const intv3_t base, const intv3_t lid, int *lid_angle) { - intv3_t v; - 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; - int base_magnitude2, lid_magnitude2; - int base_range, lid_range, i; - intv3_t scaled_base, scaled_lid; - - /* - * The angle between lid and base is: - * acos((cad(base, lid) - cad(base, hinge)^2) /(1 - cad(base, hinge)^2)) - * where cad() is the cosine_of_angle_diff() function. - * - * Make sure to check for divide by 0. - */ - lid_to_base = cosine_of_angle_diff(base, lid); - base_to_hinge = cosine_of_angle_diff(base, p_acc_orient->hinge_axis); + intv3_t cross, proj_lid, proj_base, scaled_base, scaled_lid; + fp_t lid_to_base_fp, smoothed_ratio; + int base_magnitude2, lid_magnitude2, largest_hinge_accel; + int reliable = 1, i; /* - * If hinge aligns too closely with gravity, then result may be - * unreliable. + * Scale the vectors by their range, to be able to compare them. + * If a single measurement is greated than 1g, we may overflow fixed + * point calculation. However, we can exclude such a measurement, it + * means the device is in movement and lid angle calculation is not + * possible. */ - if (fp_abs(base_to_hinge) > HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD) - reliable = 0; - - base_to_hinge = fp_sq(base_to_hinge); - - /* Check divide by 0. */ - denominator = FLOAT_TO_FP(1.0) - base_to_hinge; - if (fp_abs(denominator) < FLOAT_TO_FP(0.01)) { - *lid_angle = 0; - return 0; + for (i = X; i <= Z; i++) { + scaled_base[i] = base[i] * + accel_base->drv->get_range(accel_base); + scaled_lid[i] = lid[i] * + accel_lid->drv->get_range(accel_lid); + if (ABS(scaled_base[i]) > MOTION_SCALING_AXIS_MAX || + ABS(scaled_lid[i]) > MOTION_SCALING_AXIS_MAX) { + reliable = 0; + goto end_calculate_lid_angle; + } } - 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 - * a negative solution. To figure out the sign of the answer, calculate - * the cosine of the angle between the actual lid angle and the - * estimated vector if the lid were open to 90 deg, cos_lid_90. Also - * calculate the cosine of the angle between the actual lid angle and - * the estimated vector if the lid were open to 270 deg, - * cos_lid_270. The smaller of the two angles represents which one is - * closer. If the lid is closer to the estimated 270 degree vector then - * the result is negative, otherwise it is positive. - */ - rotate(base, p_acc_orient->rot_hinge_90, v); - cos_lid_90 = cosine_of_angle_diff(v, lid); - rotate(v, p_acc_orient->rot_hinge_180, v); - cos_lid_270 = cosine_of_angle_diff(v, lid); - - /* - * Note that cos_lid_90 and cos_lid_270 are not in degrees, because - * the arc_cos() was never performed. But, since arc_cos() is - * monotonically decreasing, we can do this comparison without ever - * taking arc_cos(). But, since the function is monotonically - * decreasing, the logic of this comparison is reversed. - */ - if (cos_lid_270 > cos_lid_90) - lid_to_base_fp = -lid_to_base_fp; - - /* Place lid angle between 0 and 360 degrees. */ - if (lid_to_base_fp < 0) - lid_to_base_fp += FLOAT_TO_FP(360); - /* - * Perform some additional reliability checks. - * - * If the magnitude of the two vectors differ too greatly, then the - * readings are unreliable and we can't use them to calculate the lid - * angle. + * Calculate square of vector magnitude in g. + * Each entry is guaranteed to be up to +/- 1<<15, so the square will be + * less than 1<<30. */ - - /* Scale the vectors by their range. */ - base_range = accel_base->drv->get_range(accel_base); - lid_range = accel_lid->drv->get_range(accel_lid); - - for (i = X; i <= Z; i++) { - /* - * To increase precision, we'll use 8x the sensor data in the - * intermediate calculation. We would normally divide by 2^15. - * - * This is safe because even at a range of 8g, calculating the - * magnitude squared should still be less than the max of a - * 32-bit signed integer. - * - * The max that base[i] could be is 32768, resulting in a max - * value for scaled_base[i] of 640 @ 8g range and force. - * Typically our range is set to 2g. - */ - scaled_base[i] = base[i] * base_range * 10 >> 12; - scaled_lid[i] = lid[i] * lid_range * 10 >> 12; - } - - base_magnitude2 = (scaled_base[X] * scaled_base[X] + - scaled_base[Y] * scaled_base[Y] + - scaled_base[Z] * scaled_base[Z]) >> 6; - lid_magnitude2 = (scaled_lid[X] * scaled_lid[X] + - scaled_lid[Y] * scaled_lid[Y] + - scaled_lid[Z] * scaled_lid[Z]) >> 6; + base_magnitude2 = scaled_base[X] * scaled_base[X] + + scaled_base[Y] * scaled_base[Y] + + scaled_base[Z] * scaled_base[Z]; + lid_magnitude2 = scaled_lid[X] * scaled_lid[X] + + scaled_lid[Y] * scaled_lid[Y] + + scaled_lid[Z] * scaled_lid[Z]; /* * Check to see if they differ than more than NOISY_MAGNITUDE_DEVIATION. @@ -417,27 +323,93 @@ static int calculate_lid_angle(const intv3_t base, const intv3_t lid, * magnitude, but we can work with the magnitudes squared directly as * shown below: * - * If A and B are the base and lid magnitudes, and x is the noisy - * magnitude deviation: + * If A is a magnitudes, and x is the noisy magnitude deviation: * - * A - B < x - * A^2 - B^2 < x * (A + B) - * A^2 - B^2 < 2 * x * avg(A, B) + * 0 < 1g - A < x + * 0 < 1g^2 - A^2 < x * (A + B) + * 0 < 1g^2 - A^2 < 2 * x * avg(A, B) * * If we assume that the average acceleration should be about 1g, then * we have: * - * (A^2 - B^2) < 2 * 1g * NOISY_MAGNITUDE_DEVIATION + * 0 < 1g^2 - A^2 < 2 * 1g * NOISY_MAGNITUDE_DEVIATION */ - if (ABS(base_magnitude2 - lid_magnitude2) > - (2 * 10 * NOISY_MAGNITUDE_DEVIATION)) + if (MOTION_SCALING_FACTOR2 - base_magnitude2 > + 2 * MOTION_SCALING_FACTOR * NOISY_MAGNITUDE_DEVIATION) { + reliable = 0; + goto end_calculate_lid_angle; + } + if (MOTION_SCALING_FACTOR2 - lid_magnitude2 > + 2 * MOTION_SCALING_FACTOR * NOISY_MAGNITUDE_DEVIATION) { + reliable = 0; + goto end_calculate_lid_angle; + } + + largest_hinge_accel = MAX(ABS(scaled_base[HINGE_AXIS]), + ABS(scaled_lid[HINGE_AXIS])); + + smoothed_ratio = MAX(INT_TO_FP(0), MIN(INT_TO_FP(1), + fp_div(INT_TO_FP(largest_hinge_accel - + SCALED_HINGE_VERTICAL_SMOOTHING_START), + INT_TO_FP(SCALED_HINGE_VERTICAL_MAXIMUM - + SCALED_HINGE_VERTICAL_SMOOTHING_START)))); + + /* Check hinge is not too vertical */ + if (largest_hinge_accel > SCALED_HINGE_VERTICAL_MAXIMUM) { reliable = 0; + goto end_calculate_lid_angle; + } + + /* Smooth input to reduce calculation error due to noise. */ + vector_scale(smoothed_base, smoothed_ratio); + vector_scale(smoothed_lid, smoothed_ratio); + vector_scale(scaled_base, INT_TO_FP(1) - smoothed_ratio); + vector_scale(scaled_lid, INT_TO_FP(1) - smoothed_ratio); + for (i = X; i <= Z; i++) { + smoothed_base[i] += scaled_base[i]; + smoothed_lid[i] += scaled_lid[i]; + } + + /* Project vectors on the hinge hyperplan, putting smooth ones aside. */ + memcpy(proj_base, smoothed_base, sizeof(intv3_t)); + memcpy(proj_lid, smoothed_lid, sizeof(intv3_t)); + proj_base[HINGE_AXIS] = 0; + proj_lid[HINGE_AXIS] = 0; + + /* Calculate the clockwise angle */ + lid_to_base_fp = arc_cos(cosine_of_angle_diff(proj_base, proj_lid)); + cross_product(proj_base, proj_lid, cross); + + /* + * If the dot product of this cross product is normal, it means that + * the shortest angle between |base| and |lid| was counterclockwise + * with respect to the surface represented by |hinge_axis| and this + * angle must be reversed. + */ + if (dot_product(cross, hinge_axis) > 0) + lid_to_base_fp = FLOAT_TO_FP(360) - lid_to_base_fp; + +#ifndef CONFIG_ACCEL_STD_REF_FRAME_OLD + /* + * Angle is between the keyboard and the front of screen: we need to + * anlge between keyboard and back of screen: + * 180 instead of 0 when lid and base are flat on surface. + * 0 instead of 180 when lid is closed on keyboard. + */ + lid_to_base_fp = FLOAT_TO_FP(180) - lid_to_base_fp; +#endif + + /* Place lid angle between 0 and 360 degrees. */ + if (lid_to_base_fp < 0) + lid_to_base_fp += FLOAT_TO_FP(360); #ifdef CONFIG_TABLET_MODE /* Ignore large angles when the lid is closed. */ if (!lid_is_open() && - (lid_to_base_fp > FLOAT_TO_FP(SMALL_LID_ANGLE_RANGE))) + (lid_to_base_fp > SMALL_LID_ANGLE_RANGE)) { reliable = 0; + goto end_calculate_lid_angle; + } /* * Ignore small angles when the lid is open. @@ -451,33 +423,31 @@ static int calculate_lid_angle(const intv3_t base, const intv3_t lid, * reliable readings over a threshold to disable key scanning. */ if (lid_is_open() && - (lid_to_base_fp <= FLOAT_TO_FP(SMALL_LID_ANGLE_RANGE))) + (lid_to_base_fp <= SMALL_LID_ANGLE_RANGE)) { reliable = 0; - - if (reliable) { - /* - * Seed the lid angle now that we have a reliable - * measurement. - */ - if (last_lid_angle_fp == FLOAT_TO_FP(-1)) - last_lid_angle_fp = lid_to_base_fp; - - /* - * If the angle was last seen as really large and now it's quite - * small, we may be rotating around from 360->0 so correct it to - * be large. But in case that the lid switch is closed, we can - * prove the small angle we see is correct so we take the angle - * as is. - */ - if ((last_lid_angle_fp >= - FLOAT_TO_FP(360) - DEBOUNCE_ANGLE_DELTA) && - (lid_to_base_fp <= DEBOUNCE_ANGLE_DELTA) && - (lid_is_open())) - last_lid_angle_fp = FLOAT_TO_FP(360) - lid_to_base_fp; - else - last_lid_angle_fp = lid_to_base_fp; + goto end_calculate_lid_angle; } + /* Seed the lid angle now that we have a reliable measurement. */ + if (last_lid_angle_fp == FLOAT_TO_FP(-1)) + last_lid_angle_fp = lid_to_base_fp; + + /* + * If the angle was last seen as really large and now it's quite + * small, we may be rotating around from 360->0 so correct it to + * be large. But in case that the lid switch is closed, we can + * prove the small angle we see is correct so we take the angle + * as is. + */ + if ((last_lid_angle_fp >= + FLOAT_TO_FP(360) - DEBOUNCE_ANGLE_DELTA) && + (lid_to_base_fp <= DEBOUNCE_ANGLE_DELTA) && + (lid_is_open())) + last_lid_angle_fp = FLOAT_TO_FP(360) - lid_to_base_fp; + else + last_lid_angle_fp = lid_to_base_fp; + +end_calculate_lid_angle: /* * Round to nearest int by adding 0.5. Note, only works because lid * angle is known to be positive. @@ -493,7 +463,9 @@ static int calculate_lid_angle(const intv3_t base, const intv3_t lid, #endif /* CONFIG_DPTF_MULTI_PROFILE && CONFIG_DPTF_MOTION_LID_NO_HALL_SENSOR */ #else /* CONFIG_TABLET_MODE */ - *lid_angle = FP_TO_INT(lid_to_base_fp + FLOAT_TO_FP(0.5)); +end_calculate_lid_angle: + if (reliable) + *lid_angle = FP_TO_INT(lid_to_base_fp + FLOAT_TO_FP(0.5)); #endif return reliable; } @@ -511,25 +483,10 @@ int motion_lid_get_angle(void) */ void motion_lid_calc(void) { -#ifndef CONFIG_ACCEL_STD_REF_FRAME_OLD - /* - * rotate lid vector by 180 deg to be in the right coordinate frame - * because calculate_lid_angle assumes when the lid is closed, that - * the lid and base accelerometer data matches - */ - intv3_t lid = { accel_lid->xyz[X], - accel_lid->xyz[Y] * -1, - accel_lid->xyz[Z] * -1}; - /* Calculate angle of lid accel. */ - lid_angle_is_reliable = calculate_lid_angle( - accel_base->xyz, lid, - &lid_angle_deg); -#else /* Calculate angle of lid accel. */ lid_angle_is_reliable = calculate_lid_angle( accel_base->xyz, accel_lid->xyz, &lid_angle_deg); -#endif #ifdef CONFIG_LID_ANGLE_UPDATE lid_angle_update(motion_lid_get_angle()); |