UPSTREAM: 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>
(cherry picked from commit 57ec5805fe302b8da22e4141a3e5620812fd7663)
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/370508

4 files changed, 13 insertions, 18 deletions

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_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;
 }