BACKPORT: motion: add config option to use the old accelerometer ref frame

Add config option to use the old accelerometer reference frame,
which is used on samus and products using 3.14 or earlier kernel.

This fixes samus so that the lid angle calculation is correct
again.

This also moves the accel_orientation structure out of the board
directory and into common code, since it purely is a function of
the reference frame being used.

BUG=chrome-os-partner:43494
BRANCH=samus
TEST=test on samus, verify lid angle calculation is correct once
again. also, enable the motion_lid test and verify that it passes.

Change-Id: I948a74a71964b54c68be66e828a030ddd0418947
Signed-off-by: Alec Berg <alecaberg@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/300510
Tested-by: Aseda Aboagye <aaboagye@chromium.org>
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>
(cherry picked from commit 5717b3150c8a8d43e07ce2dc8065c3515d3651f7)
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/370531
Reviewed-by: Aseda Aboagye <aaboagye@chromium.org>

8 files changed, 79 insertions, 39 deletions

diff --git a/board/host/board.c b/board/host/board.c
index c101ad08f8..408bfdcf0b 100644
--- a/board/host/board.c
+++ b/board/host/board.c
@@ -55,22 +55,6 @@ const struct button_config buttons[] = {
 BUILD_ASSERT(ARRAY_SIZE(buttons) == CONFIG_BUTTON_COUNT);
 #endif
 
-/* Define the accelerometer orientation matrices. */
-const struct accel_orientation acc_orient = {
-	/* Hinge aligns with y axis. */
-	.rot_hinge_90 = {
-		{ 0,  0,  1},
-		{ 0,  1,  0},
-		{ -1, 0,  0}
-	},
-	.rot_hinge_180 = {
-		{-1,  0,  0},
-		{ 0,  1,  0},
-		{ 0,  0, -1}
-	},
-	.hinge_axis = {0, 1, 0},
-};
-
 #ifdef CONFIG_I2C
 /* I2C ports */
 const struct i2c_port_t i2c_ports[] = {
diff --git a/board/samus/board.c b/board/samus/board.c
index 8f595d4e58..79d7aaa267 100644
--- a/board/samus/board.c
+++ b/board/samus/board.c
@@ -395,19 +395,3 @@ struct motion_sensor_t motion_sensors[] = {
 
 };
 const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);
-
-/* Define the accelerometer orientation matrices. */
-const struct accel_orientation acc_orient = {
-	/* Hinge aligns with y axis. */
-	.rot_hinge_90 = {
-		{ FLOAT_TO_FP(1),  0,  0},
-		{ 0,  FLOAT_TO_FP(1),  0},
-		{ 0,  0,  FLOAT_TO_FP(1)}
-	},
-	.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},
-};
diff --git a/board/samus/board.h b/board/samus/board.h
index 68a8fb665a..463a76be92 100644
--- a/board/samus/board.h
+++ b/board/samus/board.h
@@ -17,6 +17,7 @@
 /* Optional features */
 #define CONFIG_ACCELGYRO_LSM6DS0
 #define CONFIG_ACCEL_KXCJ9
+#define CONFIG_ACCEL_STD_REF_FRAME_OLD
 #define CONFIG_ALS
 #define CONFIG_ALS_ISL29035
 #define CONFIG_BOARD_VERSION
diff --git a/common/motion_lid.c b/common/motion_lid.c
index 1beb79febb..f88d5d0915 100644
--- a/common/motion_lid.c
+++ b/common/motion_lid.c
@@ -38,6 +38,42 @@ static int lid_angle_is_reliable;
  */
 #define HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD FLOAT_TO_FP(0.96593)
 
+/*
+ * Define the accelerometer orientation matrices based on the standard
+ * reference frame in use (note: accel data is converted to standard ref
+ * 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},
+};
+#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},
+};
+#endif
 
 /* Pointer to constant acceleration orientation data. */
 const struct accel_orientation * const p_acc_orient = &acc_orient;
@@ -147,11 +183,25 @@ 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
+	 */
+	vector_3_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,
-			accel_lid->xyz,
+			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_KEY_SCAN
 	lidangle_keyscan_update(motion_lid_get_angle());
diff --git a/common/motion_sense.c b/common/motion_sense.c
index bea0bef1cf..9add2d1712 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -41,6 +41,10 @@ static int accel_disp;
 
 #define SENSOR_ACTIVE(_sensor) (sensor_active & (_sensor)->active_mask)
 
+#if defined(CONFIG_LPC) || defined(TEST_MOTION_LID)
+#define UPDATE_HOST_MEM_MAP
+#endif
+
 /*
  * Mutex to protect sensor values between host command task and
  * motion sense task:
@@ -388,7 +392,7 @@ static inline void set_present(uint8_t *lpc_status)
 	*lpc_status |= EC_MEMMAP_ACC_STATUS_PRESENCE_BIT;
 }
 
-#ifdef CONFIG_LPC
+#ifdef UPDATE_HOST_MEM_MAP
 /* Update/Write LPC data */
 static inline void update_sense_data(uint8_t *lpc_status,
 		uint16_t *lpc_data, int *psample_id)
@@ -526,7 +530,7 @@ void motion_sense_task(void)
 #ifdef CONFIG_ACCEL_FIFO
 	timestamp_t ts_last_int;
 #endif
-#ifdef CONFIG_LPC
+#ifdef UPDATE_HOST_MEM_MAP
 	int sample_id = 0;
 	uint8_t *lpc_status;
 	uint16_t *lpc_data;
@@ -591,7 +595,7 @@ void motion_sense_task(void)
 			CPRINTF("]\n");
 		}
 #endif
-#ifdef CONFIG_LPC
+#ifdef UPDATE_HOST_MEM_MAP
 		update_sense_data(lpc_status, lpc_data, &sample_id);
 #endif
 
diff --git a/include/config.h b/include/config.h
index 692744951f..4f3a9222d2 100644
--- a/include/config.h
+++ b/include/config.h
@@ -51,6 +51,22 @@
 #undef CONFIG_ACCELGYRO_BMI160
 
 /*
+ * Use the old standard reference frame for accelerometers. The old
+ * reference frame is:
+ * Z-axis: perpendicular to keyboard, pointing up, such that if the device
+ *  is sitting flat on a table, the accel reads +G.
+ * X-axis: in the plane of the keyboard, pointing from the front lip to the
+ *  hinge, such that if the device is oriented with the front lip touching
+ *  the table and the hinge directly above, the accel reads +G.
+ * Y-axis: in the plane of the keyboard, pointing to the right, such that
+ *  if the device is on it's left side, the accel reads +G.
+ *
+ * Also, in the old reference frame, the lid accel matches the base accel
+ * readings when lid is closed.
+ */
+#undef CONFIG_ACCEL_STD_REF_FRAME_OLD
+
+/*
  * Define the event to raise when BMI160 interrupt.
  * Must be within TASK_EVENT_MOTION_INTERRUPT_MASK.
  */
diff --git a/test/motion_lid.c b/test/motion_lid.c
index 964bd9f026..ff97662843 100644
--- a/test/motion_lid.c
+++ b/test/motion_lid.c
@@ -29,7 +29,7 @@
 /*
  * Time in ms to wait for the task to read the vectors.
  */
-#define TEST_LID_SLEEP_RATE (TEST_LID_EC_RATE / (5 * MSEC))
+#define TEST_LID_SLEEP_RATE (TEST_LID_EC_RATE / 5)
 
 /*****************************************************************************/
 /* Mock functions */
@@ -184,7 +184,7 @@ static void wait_for_valid_sample(void)
 	uint8_t *lpc_status = host_get_memmap(EC_MEMMAP_ACC_STATUS);
 
 	sample = *lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK;
-	msleep(TEST_LID_EC_RATE/MSEC);
+	msleep(TEST_LID_EC_RATE);
 	task_wake(TASK_ID_MOTIONSENSE);
 	while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample)
 		msleep(TEST_LID_SLEEP_RATE);
diff --git a/test/test_config.h b/test/test_config.h
index 029a092405..1ece18df78 100644
--- a/test/test_config.h
+++ b/test/test_config.h
@@ -55,6 +55,7 @@
 #endif
 
 #ifdef TEST_MOTION_LID
+#define CONFIG_ACCEL_STD_REF_FRAME_OLD
 #define CONFIG_LID_ANGLE
 #define CONFIG_LID_ANGLE_SENSOR_BASE 0
 #define CONFIG_LID_ANGLE_SENSOR_LID 1