From 57b4c76c13ad82c72aeae01ce67d8658fd81f214 Mon Sep 17 00:00:00 2001 From: Ching-Kang Yen Date: Thu, 16 Jan 2020 16:11:40 +0800 Subject: driver: bmi160: Fix rounding error in set_offset() and get_offset() The original set_offset() and get_offset() codes in the driver/accelgyro_bmi160 use simple divisions to write the data. The more times the set_offset() and get_offset() is used, the data will get closer to 0. Fixing it by replacing simple division to round_divide(), division that round to nearest, in the common/math_util.c. BRANCH=octopus BUG=b:146823505 TEST=Testing on octopus:ampton on branch [firmware-octopus-11297.B]. Checking the data did not rounding to 0. Change-Id: Ide9df9e32fc501e63d6f952cb8254df7662afd23 Signed-off-by: Ching-Kang Yen Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2002998 Reviewed-by: Gwendal Grignou Commit-Queue: Gwendal Grignou Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2600801 Tested-by: David Huang Auto-Submit: David Huang --- common/math_util.c | 8 ++++++++ driver/accelgyro_bmi160.c | 21 +++++++++++++-------- include/math_util.h | 5 +++++ 3 files changed, 26 insertions(+), 8 deletions(-) diff --git a/common/math_util.c b/common/math_util.c index 22f5872267..e80d6a9888 100644 --- a/common/math_util.c +++ b/common/math_util.c @@ -272,3 +272,11 @@ void rotate_inv(const intv3_t v, const mat33_fp_t R, intv3_t res) res[1] = FP_TO_INT(fp_div(t[1], deter)); res[2] = FP_TO_INT(fp_div(t[2], deter)); } + +/* division that round to the nearest integer */ +int round_divide(int64_t dividend, int divisor) +{ + return (dividend > 0) ^ (divisor > 0) ? + (dividend - divisor / 2) / divisor : + (dividend + divisor / 2) / divisor; +} diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c index 10b70b2ebf..b97008768b 100644 --- a/driver/accelgyro_bmi160.c +++ b/driver/accelgyro_bmi160.c @@ -510,8 +510,10 @@ static int get_offset(const struct motion_sensor_t *s, BMI160_OFFSET_ACC70 + i, &val); if (val > 0x7f) val = -256 + val; - v[i] = val * BMI160_OFFSET_ACC_MULTI_MG / - BMI160_OFFSET_ACC_DIV_MG; + v[i] = round_divide( + (int64_t)val * BMI160_OFFSET_ACC_MULTI_MG, + BMI160_OFFSET_ACC_DIV_MG); + } break; case MOTIONSENSE_TYPE_GYRO: @@ -530,8 +532,9 @@ static int get_offset(const struct motion_sensor_t *s, val |= ((val98 >> (2 * i)) & 0x3) << 8; if (val > 0x1ff) val = -1024 + val; - v[i] = val * BMI160_OFFSET_GYRO_MULTI_MDS / - BMI160_OFFSET_GYRO_DIV_MDS; + v[i] = round_divide( + (int64_t)val * BMI160_OFFSET_GYRO_MULTI_MDS, + BMI160_OFFSET_GYRO_DIV_MDS); } break; #ifdef CONFIG_MAG_BMI160_BMM150 @@ -569,8 +572,9 @@ static int set_offset(const struct motion_sensor_t *s, switch (s->type) { case MOTIONSENSE_TYPE_ACCEL: for (i = X; i <= Z; i++) { - val = v[i] * BMI160_OFFSET_ACC_DIV_MG / - BMI160_OFFSET_ACC_MULTI_MG; + val = round_divide( + (int64_t)v[i] * BMI160_OFFSET_ACC_DIV_MG, + BMI160_OFFSET_ACC_MULTI_MG); if (val > 127) val = 127; if (val < -128) @@ -586,8 +590,9 @@ static int set_offset(const struct motion_sensor_t *s, break; case MOTIONSENSE_TYPE_GYRO: for (i = X; i <= Z; i++) { - val = v[i] * BMI160_OFFSET_GYRO_DIV_MDS / - BMI160_OFFSET_GYRO_MULTI_MDS; + val = round_divide( + (int64_t)v[i] * BMI160_OFFSET_GYRO_DIV_MDS, + BMI160_OFFSET_GYRO_MULTI_MDS); if (val > 511) val = 511; if (val < -512) diff --git a/include/math_util.h b/include/math_util.h index a536a0235a..4349306b34 100644 --- a/include/math_util.h +++ b/include/math_util.h @@ -170,4 +170,9 @@ void rotate(const intv3_t v, const mat33_fp_t R, intv3_t res); */ void rotate_inv(const intv3_t v, const mat33_fp_t R, intv3_t res); +/** + * Divide dividend by divisor and round it to the nearest integer. + */ +int round_divide(int64_t dividend, int divisor); + #endif /* __CROS_EC_MATH_UTIL_H */ -- cgit v1.2.1