ryu: Fix orientation of accel and compass

Add matrices to align sensors vectors with the device reference.
Move rotation in read routines, to allow fifo to contains processed
information.

BRANCH=smaug
TEST=Worsk on smaug
BUG=chrome-os-partner:39900

Change-Id: I009e7f24ef6ee0574ed664aeb5fd649fcd7039fd
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/286659
Reviewed-by: Alec Berg <alecaberg@chromium.org>

6 files changed, 34 insertions, 24 deletions

diff --git a/board/ryu/board.c b/board/ryu/board.c
index 297ba2512e..2f08dd9bb2 100644
--- a/board/ryu/board.c
+++ b/board/ryu/board.c
@@ -292,6 +292,20 @@ const unsigned int i2c_ports_used = ARRAY_SIZE(i2c_ports);
 /* Sensor mutex */
 static struct mutex g_mutex;
 
+/* Matrix to rotate sensor vector into standard reference frame */
+const matrix_3x3_t accelgyro_standard_ref = {
+	{FLOAT_TO_FP(-1),  0,  0},
+	{ 0, FLOAT_TO_FP(-1),  0},
+	{ 0,  0, FLOAT_TO_FP(1)}
+};
+
+const matrix_3x3_t mag_standard_ref = {
+	{ 0,  FLOAT_TO_FP(-1),  0},
+	{FLOAT_TO_FP(-1),  0,  0},
+	{ 0,  0, FLOAT_TO_FP(1)}
+};
+
+
 struct motion_sensor_t motion_sensors[] = {
 
 	/*
@@ -308,7 +322,7 @@ struct motion_sensor_t motion_sensors[] = {
 	 .mutex = &g_mutex,
 	 .drv_data = &g_bmi160_data,
 	 .i2c_addr = BMI160_ADDR0,
-	 .rot_standard_ref = NULL,
+	 .rot_standard_ref = &accelgyro_standard_ref,
 	 .default_config = {
 		 .odr = 100000,
 		 .range = 8,  /* g */
@@ -325,7 +339,7 @@ struct motion_sensor_t motion_sensors[] = {
 	 .mutex = &g_mutex,
 	 .drv_data = &g_bmi160_data,
 	 .i2c_addr = BMI160_ADDR0,
-	 .rot_standard_ref = NULL,
+	 .rot_standard_ref = &accelgyro_standard_ref,
 	 .default_config = {
 		 .odr = 0,
 		 .range = 1000, /* dps */
@@ -342,7 +356,7 @@ struct motion_sensor_t motion_sensors[] = {
 	 .mutex = &g_mutex,
 	 .drv_data = &g_bmi160_data,
 	 .i2c_addr = BMI160_ADDR0,
-	 .rot_standard_ref = NULL,
+	 .rot_standard_ref = &mag_standard_ref,
 	 .default_config = {
 		 .odr = 0,
 		 .range = 1,
diff --git a/common/motion_sense.c b/common/motion_sense.c
index 57a4bd26bb..629a523910 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -397,18 +397,9 @@ void motion_sense_task(void)
 				if (ret != EC_SUCCESS)
 					continue;
 				rd_cnt++;
-				/*
-				 * Rotate the accel vector so the reference for
-				 * all sensors are in the same space.
-				 */
 				mutex_lock(&g_sensor_mutex);
-				if (*sensor->rot_standard_ref != NULL)
-					rotate(sensor->raw_xyz,
-					       *sensor->rot_standard_ref,
-					       sensor->xyz);
-				else
-					memcpy(sensor->xyz, sensor->raw_xyz,
-						sizeof(vector_3_t));
+				memcpy(sensor->xyz, sensor->raw_xyz,
+					sizeof(sensor->xyz));
 				mutex_unlock(&g_sensor_mutex);
 			}
 		}
@@ -937,7 +928,7 @@ static int command_accel_read_xyz(int argc, char **argv)
 	while ((n == -1) || (n-- > 0)) {
 		ret = sensor->drv->read(sensor, v);
 		if (ret == 0)
-			ccprintf("Current raw data %d: %-5d %-5d %-5d\n",
+			ccprintf("Current data %d: %-5d %-5d %-5d\n",
 				 id, v[X], v[Y], v[Z]);
 		else
 			ccprintf("vector not ready\n");
diff --git a/driver/accel_kxcj9.c b/driver/accel_kxcj9.c
index 292b43f3fc..cabd7ef589 100644
--- a/driver/accel_kxcj9.c
+++ b/driver/accel_kxcj9.c
@@ -425,6 +425,8 @@ static int read(const struct motion_sensor_t *s, vector_3_t v)
 		v[i] <<= (16 - resolution);
 		v[i] += (data->offset[i] << 5) / range;
 	}
+	if (*s->rot_standard_ref != NULL)
+		rotate(v, *s->rot_standard_ref, v);
 	return EC_SUCCESS;
 }
 
diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c
index 10e4905fa9..e81aa242ee 100644
--- a/driver/accelgyro_bmi160.c
+++ b/driver/accelgyro_bmi160.c
@@ -508,6 +508,8 @@ void normalize(const struct motion_sensor_t *s, vector_3_t v, uint8_t *data)
 	v[0] = ((int16_t)((data[1] << 8) | data[0]));
 	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);
 }
 
 #ifdef CONFIG_ACCEL_INTERRUPTS
@@ -776,9 +778,8 @@ static int read(const struct motion_sensor_t *s, vector_3_t v)
 	 * to get the latest updated sensor data quickly.
 	 */
 	if (status & BMI160_DRDY_MASK(s->type)) {
-		v[0] = s->raw_xyz[0];
-		v[1] = s->raw_xyz[1];
-		v[2] = s->raw_xyz[2];
+		if (v != s->raw_xyz)
+			memcpy(v, s->raw_xyz, sizeof(s->raw_xyz));
 		return EC_SUCCESS;
 	}
 
diff --git a/driver/accelgyro_lsm6ds0.c b/driver/accelgyro_lsm6ds0.c
index ebd9bd745e..001f7bb5c7 100644
--- a/driver/accelgyro_lsm6ds0.c
+++ b/driver/accelgyro_lsm6ds0.c
@@ -356,9 +356,8 @@ static int read(const struct motion_sensor_t *s, vector_3_t v)
 	 * to get the latest updated sensor data quickly.
 	 */
 	if (!tmp) {
-		v[0] = s->raw_xyz[0];
-		v[1] = s->raw_xyz[1];
-		v[2] = s->raw_xyz[2];
+		if (v != s->raw_xyz)
+			memcpy(v, s->raw_xyz, sizeof(s->raw_xyz));
 		return EC_SUCCESS;
 	}
 
@@ -381,6 +380,8 @@ static int read(const struct motion_sensor_t *s, vector_3_t v)
 		v[i] = ((int16_t)((raw[i * 2 + 1] << 8) | raw[i * 2]));
 		v[i] += (data->offset[i] << 5) / range;
 	}
+	if (*s->rot_standard_ref != NULL)
+		rotate(v, *s->rot_standard_ref, v);
 
 	return EC_SUCCESS;
 }
diff --git a/test/motion_lid.c b/test/motion_lid.c
index d06a76135d..a3ce4925f1 100644
--- a/test/motion_lid.c
+++ b/test/motion_lid.c
@@ -40,9 +40,10 @@ static int accel_init(const struct motion_sensor_t *s)
 
 static int accel_read(const struct motion_sensor_t *s, vector_3_t v)
 {
-	v[X] = s->xyz[X];
-	v[Y] = s->xyz[Y];
-	v[Z] = s->xyz[Z];
+	if (*s->rot_standard_ref != NULL)
+		rotate(s->xyz, *s->rot_standard_ref, v);
+	else if (s->xyz != v)
+		memcpy(v, s->xyz, sizeof(v));
 	return EC_SUCCESS;
 }