driver: bmi160: use rotation matrix to handle offsets.

Store offsets using sensors axis, not the device axis.
Therefore apply rot_standard_ref to the offset vector before
get and rot_standard_ref^-1 before set.

BRANCH=smaug
TEST=using mag sensor, check the offset are applied to the right axis
and store correctly.
BUG=chromium:517675

Change-Id: I95c8ef2a62603890184412674e7bde91ebecd288
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/294596
Reviewed-by: Sheng-liang Song <ssl@chromium.org>

3 files changed, 23 insertions, 18 deletions

diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c
index e2b6daddeb..6bddcdfb1d 100644
--- a/driver/accelgyro_bmi160.c
+++ b/driver/accelgyro_bmi160.c
@@ -455,6 +455,8 @@ static int get_offset(const struct motion_sensor_t *s,
 			int16_t    *temp)
 {
 	int i, val, val98;
+	vector_3_t v;
+
 	switch (s->type) {
 	case MOTIONSENSE_TYPE_ACCEL:
 		/*
@@ -466,7 +468,7 @@ static int get_offset(const struct motion_sensor_t *s,
 			raw_read8(s->addr, BMI160_OFFSET_ACC70 + i, &val);
 			if (val > 0x7f)
 				val = -256 + val;
-			offset[i] = val * BMI160_OFFSET_ACC_MULTI_MG /
+			v[i] = val * BMI160_OFFSET_ACC_MULTI_MG /
 				BMI160_OFFSET_ACC_DIV_MG;
 		}
 		break;
@@ -484,16 +486,21 @@ static int get_offset(const struct motion_sensor_t *s,
 			val |= ((val98 >> (2 * i)) & 0x3) << 8;
 			if (val > 0x1ff)
 				val = -1024 + val;
-			offset[i] = val * BMI160_OFFSET_GYRO_MULTI_MDS /
+			v[i] = val * BMI160_OFFSET_GYRO_MULTI_MDS /
 				BMI160_OFFSET_GYRO_DIV_MDS;
 		}
 		break;
 	case MOTIONSENSE_TYPE_MAG:
-		return bmm150_get_offset(s, offset, temp);
+		bmm150_get_offset(s, v);
+		break;
 	default:
 		for (i = X; i <= Z; i++)
-			offset[i] = 0;
+			v[i] = 0;
 	}
+	rotate(v, *s->rot_standard_ref, v);
+	offset[X] = v[X];
+	offset[Y] = v[Y];
+	offset[Z] = v[Z];
 	/* Saving temperature at calibration not supported yet */
 	*temp = EC_MOTION_SENSE_INVALID_CALIB_TEMP;
 	return EC_SUCCESS;
@@ -504,6 +511,10 @@ static int set_offset(const struct motion_sensor_t *s,
 			int16_t    temp)
 {
 	int ret, i, val, val98;
+	vector_3_t v = { offset[X], offset[Y], offset[Z] };
+
+	rotate_inv(v, *s->rot_standard_ref, v);
+
 	ret = raw_read8(s->addr, BMI160_OFFSET_EN_GYR98, &val98);
 	if (ret != 0)
 		return ret;
@@ -511,7 +522,7 @@ static int set_offset(const struct motion_sensor_t *s,
 	switch (s->type) {
 	case MOTIONSENSE_TYPE_ACCEL:
 		for (i = X; i <= Z; i++) {
-			val = offset[i] * BMI160_OFFSET_ACC_DIV_MG /
+			val = v[i] * BMI160_OFFSET_ACC_DIV_MG /
 				BMI160_OFFSET_ACC_MULTI_MG;
 			if (val > 127)
 				val = 127;
@@ -526,7 +537,7 @@ static int set_offset(const struct motion_sensor_t *s,
 		break;
 	case MOTIONSENSE_TYPE_GYRO:
 		for (i = X; i <= Z; i++) {
-			val = offset[i] * BMI160_OFFSET_GYRO_DIV_MDS /
+			val = v[i] * BMI160_OFFSET_GYRO_DIV_MDS /
 				BMI160_OFFSET_GYRO_MULTI_MDS;
 			if (val > 511)
 				val = 511;
@@ -543,7 +554,7 @@ static int set_offset(const struct motion_sensor_t *s,
 				 val98 | BMI160_OFFSET_GYRO_EN);
 		break;
 	case MOTIONSENSE_TYPE_MAG:
-		ret = bmm150_set_offset(s, offset, temp);
+		ret = bmm150_set_offset(s, v);
 		break;
 	default:
 		ret = EC_RES_INVALID_PARAM;
@@ -613,8 +624,7 @@ void normalize(const struct motion_sensor_t *s, vector_3_t v, uint8_t *data)
 		v[1] = ((int16_t)((data[3] << 8) | data[2]));
 		v[2] = ((int16_t)((data[5] << 8) | data[4]));
 	}
-	if (*s->rot_standard_ref != NULL)
-		rotate(v, *s->rot_standard_ref, v);
+	rotate(v, *s->rot_standard_ref, v);
 }
 
 #ifdef CONFIG_ACCEL_INTERRUPTS
diff --git a/driver/mag_bmm150.c b/driver/mag_bmm150.c
index 59ce76785e..87933945fb 100644
--- a/driver/mag_bmm150.c
+++ b/driver/mag_bmm150.c
@@ -226,8 +226,7 @@ void bmm150_normalize(const struct motion_sensor_t *s,
 }
 
 int bmm150_set_offset(const struct motion_sensor_t *s,
-		      const int16_t *offset,
-		      int16_t    temp)
+		      const vector_3_t offset)
 {
 	struct bmm150_comp_registers *regs = BMM150_COMP_REG(s);
 	regs->offset[X] = offset[X];
@@ -237,13 +236,11 @@ int bmm150_set_offset(const struct motion_sensor_t *s,
 }
 
 int bmm150_get_offset(const struct motion_sensor_t *s,
-		      int16_t   *offset,
-		      int16_t    *temp)
+		      vector_3_t offset)
 {
 	struct bmm150_comp_registers *regs = BMM150_COMP_REG(s);
 	offset[X] = regs->offset[X];
 	offset[Y] = regs->offset[Y];
 	offset[Z] = regs->offset[Z];
-	*temp = EC_MOTION_SENSE_INVALID_CALIB_TEMP;
 	return EC_SUCCESS;
 }
diff --git a/driver/mag_bmm150.h b/driver/mag_bmm150.h
index 1bbf998f6a..f3a0a38f50 100644
--- a/driver/mag_bmm150.h
+++ b/driver/mag_bmm150.h
@@ -103,11 +103,9 @@ void bmm150_normalize(const struct motion_sensor_t *s,
 		      uint8_t *data);
 
 int bmm150_set_offset(const struct motion_sensor_t *s,
-		      const int16_t *offset,
-		      int16_t    temp);
+		      const vector_3_t offset);
 
 int bmm150_get_offset(const struct motion_sensor_t *s,
-		      int16_t   *offset,
-		      int16_t    *temp);
+		      vector_3_t   offset);
 
 #endif /* __CROS_EC_MAG_BMM150_H */