Samus: Split motion sense and lid angle

Split motion_sense.c.
Translate the accel data in the Android coordinate right away.

BUG=chrome-os-partner:32002
BRANCH=ToT
TEST=On samus, check lid angle are still correct.

Change-Id: If743e25245dc1ce4cdacb8a4d5af22616c4a79e4
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/225486
Reviewed-by: Sheng-liang Song <ssl@chromium.org>
Reviewed-by: Alec Berg <alecaberg@chromium.org>

17 files changed, 437 insertions, 353 deletions

diff --git a/board/host/board.c b/board/host/board.c
index c43a36ae8b..7ed0e37f28 100644
--- a/board/host/board.c
+++ b/board/host/board.c
@@ -7,9 +7,11 @@
 #include "button.h"
 #include "extpower.h"
 #include "gpio.h"
+#include "host_command.h"
 #include "inductive_charging.h"
 #include "lid_switch.h"
 #include "motion_sense.h"
+#include "motion_lid.h"
 #include "power_button.h"
 #include "temp_sensor.h"
 #include "timer.h"
@@ -54,13 +56,6 @@ BUILD_ASSERT(ARRAY_SIZE(buttons) == CONFIG_BUTTON_COUNT);
 
 /* Define the accelerometer orientation matrices. */
 const struct accel_orientation acc_orient = {
-	/* Lid and base sensor are already aligned. */
-	.rot_align = {
-		{ 1,  0,  0},
-		{ 0,  1,  0},
-		{ 0,  0,  1}
-	},
-
 	/* Hinge aligns with y axis. */
 	.rot_hinge_90 = {
 		{ 0,  0,  1},
@@ -72,11 +67,6 @@ const struct accel_orientation acc_orient = {
 		{ 0,  1,  0},
 		{ 0,  0, -1}
 	},
-	.rot_standard_ref = {
-		{ 1,  0,  0},
-		{ 0,  1,  0},
-		{ 0,  0,  1}
-	},
 	.hinge_axis = {0, 1, 0},
 };
 
diff --git a/board/host/board.h b/board/host/board.h
index 3c11f58b8f..46d1a001b2 100644
--- a/board/host/board.h
+++ b/board/host/board.h
@@ -39,13 +39,4 @@ enum adc_channel {
 	ADC_CH_COUNT
 };
 
-/* Identifiers for each accelerometer used. */
-enum accel_id {
-	ACCEL_BASE,
-	ACCEL_LID,
-
-	/* Number of accelerometers. */
-	ACCEL_COUNT
-};
-
 #endif /* __BOARD_H */
diff --git a/board/samus/board.c b/board/samus/board.c
index 08dae68780..e6682d4717 100644
--- a/board/samus/board.c
+++ b/board/samus/board.c
@@ -28,6 +28,7 @@
 #include "lid_switch.h"
 #include "lightbar.h"
 #include "motion_sense.h"
+#include "motion_lid.h"
 #include "peci.h"
 #include "power.h"
 #include "power_button.h"
@@ -264,6 +265,19 @@ static struct mutex g_lid_mutex;
 struct kxcj9_data g_kxcj9_data;
 
 /* Four Motion sensors */
+/* Matrix to rotate accelrator into standard reference frame */
+const matrix_3x3_t base_standard_ref = {
+	{-1,  0,  0},
+	{ 0, -1,  0},
+	{ 0,  0, -1}
+};
+
+const matrix_3x3_t lid_standard_ref = {
+	{ 0,  1,  0},
+	{-1,  0,  0},
+	{ 0,  0, -1}
+};
+
 struct motion_sensor_t motion_sensors[] = {
 
 	/*
@@ -274,30 +288,23 @@ struct motion_sensor_t motion_sensors[] = {
 	{SENSOR_ACTIVE_S0_S3_S5, "Base", SENSOR_CHIP_LSM6DS0,
 		SENSOR_ACCELEROMETER, LOCATION_BASE,
 		&lsm6ds0_drv, &g_base_mutex, NULL,
-		LSM6DS0_ADDR1, 119000, 2},
+		LSM6DS0_ADDR1, &base_standard_ref, 119000, 2},
 
 	{SENSOR_ACTIVE_S0, "Lid",  SENSOR_CHIP_KXCJ9,
 		SENSOR_ACCELEROMETER, LOCATION_LID,
 		&kxcj9_drv, &g_lid_mutex, &g_kxcj9_data,
-		KXCJ9_ADDR0, 100000, 2},
+		KXCJ9_ADDR0, &lid_standard_ref, 100000, 2},
 
 	{SENSOR_ACTIVE_S0, "Base Gyro", SENSOR_CHIP_LSM6DS0,
 		SENSOR_GYRO, LOCATION_BASE,
 		&lsm6ds0_drv, &g_base_mutex, NULL,
-		LSM6DS0_ADDR1, 119000, 2000},
+		LSM6DS0_ADDR1, NULL, 119000, 2000},
 
 };
 const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);
 
 /* Define the accelerometer orientation matrices. */
 const struct accel_orientation acc_orient = {
-	/* Lid and base sensor are already aligned. */
-	.rot_align = {
-		{ 0, -1,  0},
-		{ 1,  0,  0},
-		{ 0,  0,  1}
-	},
-
 	/* Hinge aligns with y axis. */
 	.rot_hinge_90 = {
 		{ 1,  0,  0},
@@ -309,10 +316,5 @@ const struct accel_orientation acc_orient = {
 		{ 0,  1,  0},
 		{ 0,  0,  1}
 	},
-	.rot_standard_ref = {
-		{-1,  0,  0},
-		{ 0, -1,  0},
-		{ 0,  0, -1}
-	},
 	.hinge_axis = {0, 1, 0},
 };
diff --git a/board/samus/board.h b/board/samus/board.h
index a203b94a9b..757f0ca950 100644
--- a/board/samus/board.h
+++ b/board/samus/board.h
@@ -21,11 +21,13 @@
 #define CONFIG_ALS_ISL29035
 #define CONFIG_BOARD_VERSION
 #define CONFIG_CMD_ACCELS
+#define CONFIG_CMD_ACCEL_INFO
 #define CONFIG_POWER_COMMON
 #define CONFIG_CHIPSET_CAN_THROTTLE
 #define CONFIG_KEYBOARD_BOARD_CONFIG
 #define CONFIG_KEYBOARD_PROTOCOL_8042
 #define CONFIG_KEYBOARD_COL2_INVERTED
+#define CONFIG_LID_ANGLE
 #define CONFIG_LIGHTBAR_POWER_RAILS
 #define CONFIG_LOW_POWER_IDLE
 #define CONFIG_POWER_BUTTON
@@ -189,6 +191,9 @@ int board_discharge_on_ac(int enable);
 #define CONFIG_GESTURE_TAP_MIN_INTERSTICE_T 120
 #define CONFIG_GESTURE_TAP_MAX_INTERSTICE_T 500
 
+#define CONFIG_SENSOR_BASE 0
+#define CONFIG_SENSOR_LID 1
+
 #endif /* !__ASSEMBLER__ */
 
 #endif /* __BOARD_H */
diff --git a/common/build.mk b/common/build.mk
index 9fc2a9ebf0..acc650522a 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -85,4 +85,5 @@ common-$(HAS_TASK_KEYSCAN)+=keyboard_scan.o
 common-$(HAS_TASK_LIGHTBAR)+=lb_common.o lightbar.o
 common-$(HAS_TASK_MOTIONSENSE)+=motion_sense.o math_util.o
 common-$(CONFIG_GESTURE_DETECTION)+=gesture.o
+common-$(CONFIG_LID_ANGLE)+=motion_lid.o
 common-$(TEST_BUILD)+=test_util.o
diff --git a/common/console_output.c b/common/console_output.c
index e2aa33d08b..492375e71e 100644
--- a/common/console_output.c
+++ b/common/console_output.c
@@ -48,6 +48,7 @@ static const char * const channel_names[] = {
 	"lidangle",
 	"lightbar",
 	"lpc",
+	"motionlid",
 	"motionsense",
 	"pdhostcmd",
 	"port80",
diff --git a/common/motion_lid.c b/common/motion_lid.c
new file mode 100644
index 0000000000..4b991dee6b
--- /dev/null
+++ b/common/motion_lid.c
@@ -0,0 +1,192 @@
+/* Copyright (c) 2014 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/* Motion sense module to read from various motion sensors. */
+
+#include "accelgyro.h"
+#include "chipset.h"
+#include "common.h"
+#include "console.h"
+#include "gesture.h"
+#include "hooks.h"
+#include "host_command.h"
+#include "lid_angle.h"
+#include "math_util.h"
+#include "motion_lid.h"
+#include "motion_sense.h"
+#include "power.h"
+#include "timer.h"
+#include "task.h"
+#include "util.h"
+
+/* Console output macros */
+#define CPUTS(outstr) cputs(CC_MOTION_LID, outstr)
+#define CPRINTS(format, args...) cprints(CC_MOTION_LID, format, ## args)
+#define CPRINTF(format, args...) cprintf(CC_MOTION_LID, format, ## args)
+
+/* For vector_3_t, define which coordinates are in which location. */
+enum {
+	X, Y, Z
+};
+
+/* Current acceleration vectors and current lid angle. */
+static float lid_angle_deg;
+static int lid_angle_is_reliable;
+
+/*
+ * Angle threshold for how close the hinge aligns with gravity before
+ * considering the lid angle calculation unreliable. For computational
+ * efficiency, value is given unit-less, so if you want the threshold to be
+ * at 15 degrees, the value would be cos(15 deg) = 0.96593.
+ */
+#define HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD 0.96593F
+
+
+/* Pointer to constant acceleration orientation data. */
+const struct accel_orientation * const p_acc_orient = &acc_orient;
+
+struct motion_sensor_t *accel_base = &motion_sensors[CONFIG_SENSOR_BASE];
+struct motion_sensor_t *accel_lid = &motion_sensors[CONFIG_SENSOR_LID];
+
+/**
+ * Calculate the lid angle using two acceleration vectors, one recorded in
+ * the base and one in the lid.
+ *
+ * @param base Base accel vector
+ * @param lid  Lid accel vector
+ * @param lid_angle Pointer to location to store lid angle result
+ *
+ * @return flag representing if resulting lid angle calculation is reliable.
+ */
+static int calculate_lid_angle(const vector_3_t base, const vector_3_t lid,
+		float *lid_angle)
+{
+	vector_3_t v;
+	float ang_lid_to_base, ang_lid_90, ang_lid_270;
+	float lid_to_base, base_to_hinge;
+	int reliable = 1;
+
+	/*
+	 * The angle between lid and base is:
+	 * acos((cad(base, lid) - cad(base, hinge)^2) /(1 - cad(base, hinge)^2))
+	 * where cad() is the cosine_of_angle_diff() function.
+	 *
+	 * Make sure to check for divide by 0.
+	 */
+	lid_to_base = cosine_of_angle_diff(base, lid);
+	base_to_hinge = cosine_of_angle_diff(base, p_acc_orient->hinge_axis);
+
+	/*
+	 * If hinge aligns too closely with gravity, then result may be
+	 * unreliable.
+	 */
+	if (ABS(base_to_hinge) > HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD)
+		reliable = 0;
+
+	base_to_hinge = SQ(base_to_hinge);
+
+	/* Check divide by 0. */
+	if (ABS(1.0F - base_to_hinge) < 0.01F) {
+		*lid_angle = 0.0;
+		return 0;
+	}
+
+	ang_lid_to_base = arc_cos(
+			(lid_to_base - base_to_hinge) / (1 - base_to_hinge));
+
+	/*
+	 * The previous calculation actually has two solutions, a positive and
+	 * a negative solution. To figure out the sign of the answer, calculate
+	 * the angle between the actual lid angle and the estimated vector if
+	 * the lid were open to 90 deg, ang_lid_90. Also calculate the angle
+	 * between the actual lid angle and the estimated vector if the lid
+	 * were open to 270 deg, ang_lid_270. The smaller of the two angles
+	 * represents which one is closer. If the lid is closer to the
+	 * estimated 270 degree vector then the result is negative, otherwise
+	 * it is positive.
+	 */
+	rotate(base, p_acc_orient->rot_hinge_90, v);
+	ang_lid_90 = cosine_of_angle_diff(v, lid);
+	rotate(v, p_acc_orient->rot_hinge_180, v);
+	ang_lid_270 = cosine_of_angle_diff(v, lid);
+
+	/*
+	 * Note that ang_lid_90 and ang_lid_270 are not in degrees, because
+	 * the arc_cos() was never performed. But, since arc_cos() is
+	 * monotonically decreasing, we can do this comparison without ever
+	 * taking arc_cos(). But, since the function is monotonically
+	 * decreasing, the logic of this comparison is reversed.
+	 */
+	if (ang_lid_270 > ang_lid_90)
+		ang_lid_to_base = -ang_lid_to_base;
+
+	/* Place lid angle between 0 and 360 degrees. */
+	if (ang_lid_to_base < 0)
+		ang_lid_to_base += 360;
+
+	*lid_angle = ang_lid_to_base;
+	return reliable;
+}
+
+int motion_lid_get_angle(void)
+{
+	if (lid_angle_is_reliable)
+		/*
+		 * Round to nearest int by adding 0.5. Note, only works because
+		 * lid angle is known to be positive.
+		 */
+		return (int)(lid_angle_deg + 0.5F);
+	else
+		return (int)LID_ANGLE_UNRELIABLE;
+}
+
+/*
+ * Calculate lid angle and massage the results
+ */
+void motion_lid_calc(void)
+{
+	/* Calculate angle of lid accel. */
+	lid_angle_is_reliable = calculate_lid_angle(
+			accel_base->xyz,
+			accel_lid->xyz,
+			&lid_angle_deg);
+
+#ifdef CONFIG_LID_ANGLE_KEY_SCAN
+	lidangle_keyscan_update(motion_lid_get_angle());
+#endif
+
+}
+
+/*****************************************************************************/
+/* Host commands */
+
+
+int host_cmd_motion_lid(struct host_cmd_handler_args *args)
+{
+	const struct ec_params_motion_sense *in = args->params;
+	struct ec_response_motion_sense *out = args->response;
+
+	switch (in->cmd) {
+	case MOTIONSENSE_CMD_KB_WAKE_ANGLE:
+#ifdef CONFIG_LID_ANGLE_KEY_SCAN
+		/* Set new keyboard wake lid angle if data arg has value. */
+		if (in->kb_wake_angle.data != EC_MOTION_SENSE_NO_VALUE)
+			lid_angle_set_kb_wake_angle(in->kb_wake_angle.data);
+
+		out->kb_wake_angle.ret = lid_angle_get_kb_wake_angle();
+#else
+		out->kb_wake_angle.ret = 0;
+#endif
+		args->response_size = sizeof(out->kb_wake_angle);
+
+		break;
+
+	default:
+		return EC_RES_INVALID_PARAM;
+	}
+
+	return EC_RES_SUCCESS;
+}
+
diff --git a/common/motion_sense.c b/common/motion_sense.c
index 5a8b4a6075..9ed0ab92da 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -15,6 +15,7 @@
 #include "lid_angle.h"
 #include "math_util.h"
 #include "motion_sense.h"
+#include "motion_lid.h"
 #include "power.h"
 #include "timer.h"
 #include "task.h"
@@ -36,10 +37,6 @@ enum {
 	X, Y, Z
 };
 
-/* Current acceleration vectors and current lid angle. */
-static float lid_angle_deg;
-static int lid_angle_is_reliable;
-
 /* Bounds for setting the sensor polling interval. */
 #define MIN_POLLING_INTERVAL_MS 5
 #define MAX_POLLING_INTERVAL_MS 1000
@@ -53,116 +50,24 @@ static int lid_angle_is_reliable;
 
 /* Accelerometer polling intervals based on chipset state. */
 static int accel_interval_ap_on_ms = 10;
-
 /*
- * Angle threshold for how close the hinge aligns with gravity before
- * considering the lid angle calculation unreliable. For computational
- * efficiency, value is given unit-less, so if you want the threshold to be
- * at 15 degrees, the value would be cos(15 deg) = 0.96593.
+ * Sampling interval for measuring acceleration and calculating lid angle.
+ * Set to accel_interval_ap_on_ms when ap is on.
  */
-#define HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD 0.96593F
-
-/* Sampling interval for measuring acceleration and calculating lid angle. */
 static int accel_interval_ms;
 
-#ifdef CONFIG_CMD_LID_ANGLE
+#ifdef CONFIG_CMD_ACCEL_INFO
 static int accel_disp;
 #endif
 
-/* Pointer to constant acceleration orientation data. */
-const struct accel_orientation * const p_acc_orient = &acc_orient;
-
-/**
- * Calculate the lid angle using two acceleration vectors, one recorded in
- * the base and one in the lid.
- *
- * @param base Base accel vector
- * @param lid  Lid accel vector
- * @param lid_angle Pointer to location to store lid angle result
- *
- * @return flag representing if resulting lid angle calculation is reliable.
+/*
+ * Angle threshold for how close the hinge aligns with gravity before
+ * considering the lid angle calculation unreliable. For computational
+ * efficiency, value is given unit-less, so if you want the threshold to be
+ * at 15 degrees, the value would be cos(15 deg) = 0.96593.
  */
-static int calculate_lid_angle(const vector_3_t base, const vector_3_t lid,
-		float *lid_angle)
-{
-	vector_3_t v;
-	float ang_lid_to_base, ang_lid_90, ang_lid_270;
-	float lid_to_base, base_to_hinge;
-	int reliable = 1;
-
-	/*
-	 * The angle between lid and base is:
-	 * acos((cad(base, lid) - cad(base, hinge)^2) /(1 - cad(base, hinge)^2))
-	 * where cad() is the cosine_of_angle_diff() function.
-	 *
-	 * Make sure to check for divide by 0.
-	 */
-	lid_to_base = cosine_of_angle_diff(base, lid);
-	base_to_hinge = cosine_of_angle_diff(base, p_acc_orient->hinge_axis);
-
-	/*
-	 * If hinge aligns too closely with gravity, then result may be
-	 * unreliable.
-	 */
-	if (ABS(base_to_hinge) > HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD)
-		reliable = 0;
-
-	base_to_hinge = SQ(base_to_hinge);
-
-	/* Check divide by 0. */
-	if (ABS(1.0F - base_to_hinge) < 0.01F) {
-		*lid_angle = 0.0;
-		return 0;
-	}
-
-	ang_lid_to_base = arc_cos(
-			(lid_to_base - base_to_hinge) / (1 - base_to_hinge));
-
-	/*
-	 * The previous calculation actually has two solutions, a positive and
-	 * a negative solution. To figure out the sign of the answer, calculate
-	 * the angle between the actual lid angle and the estimated vector if
-	 * the lid were open to 90 deg, ang_lid_90. Also calculate the angle
-	 * between the actual lid angle and the estimated vector if the lid
-	 * were open to 270 deg, ang_lid_270. The smaller of the two angles
-	 * represents which one is closer. If the lid is closer to the
-	 * estimated 270 degree vector then the result is negative, otherwise
-	 * it is positive.
-	 */
-	rotate(base, p_acc_orient->rot_hinge_90, v);
-	ang_lid_90 = cosine_of_angle_diff(v, lid);
-	rotate(v, p_acc_orient->rot_hinge_180, v);
-	ang_lid_270 = cosine_of_angle_diff(v, lid);
-
-	/*
-	 * Note that ang_lid_90 and ang_lid_270 are not in degrees, because
-	 * the arc_cos() was never performed. But, since arc_cos() is
-	 * monotonically decreasing, we can do this comparison without ever
-	 * taking arc_cos(). But, since the function is monotonically
-	 * decreasing, the logic of this comparison is reversed.
-	 */
-	if (ang_lid_270 > ang_lid_90)
-		ang_lid_to_base = -ang_lid_to_base;
-
-	/* Place lid angle between 0 and 360 degrees. */
-	if (ang_lid_to_base < 0)
-		ang_lid_to_base += 360;
-
-	*lid_angle = ang_lid_to_base;
-	return reliable;
-}
+#define HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD 0.96593F
 
-int motion_get_lid_angle(void)
-{
-	if (lid_angle_is_reliable)
-		/*
-		 * Round to nearest int by adding 0.5. Note, only works because
-		 * lid angle is known to be positive.
-		 */
-		return (int)(lid_angle_deg + 0.5F);
-	else
-		return (int)LID_ANGLE_UNRELIABLE;
-}
 
 static void motion_sense_shutdown(void)
 {
@@ -254,7 +159,11 @@ static inline void update_sense_data(uint8_t *lpc_status,
 	 * with un-calibrated accels. The AP calculates a separate,
 	 * more accurate lid angle.
 	 */
-	lpc_data[0] = motion_get_lid_angle();
+#ifdef CONFIG_LID_ANGLE
+	lpc_data[0] = motion_lid_get_angle();
+#else
+	lpc_data[0] = LID_ANGLE_UNRELIABLE;
+#endif
 	for (i = 0; i < motion_sensor_count; i++) {
 		sensor = &motion_sensors[i];
 		lpc_data[1+3*i] = sensor->xyz[X];
@@ -295,9 +204,9 @@ static int motion_sense_read(struct motion_sensor_t *sensor)
 
 	/* Read all raw X,Y,Z accelerations. */
 	ret = sensor->drv->read(sensor,
-		&sensor->raw_xyz[X],
-		&sensor->raw_xyz[Y],
-		&sensor->raw_xyz[Z]);
+		&sensor->xyz[X],
+		&sensor->xyz[Y],
+		&sensor->xyz[Z]);
 
 	if (ret != EC_SUCCESS)
 		return EC_ERROR_UNKNOWN;
@@ -322,8 +231,6 @@ void motion_sense_task(void)
 	int sample_id = 0;
 	int rd_cnt;
 	struct motion_sensor_t *sensor;
-	struct motion_sensor_t *accel_base = NULL;
-	struct motion_sensor_t *accel_lid = NULL;
 
 	lpc_status = host_get_memmap(EC_MEMMAP_ACC_STATUS);
 	lpc_data = (uint16_t *)host_get_memmap(EC_MEMMAP_ACC_DATA);
@@ -334,15 +241,6 @@ void motion_sense_task(void)
 
 		sensor->odr = sensor->default_odr;
 		sensor->range = sensor->default_range;
-
-		if ((LOCATION_BASE == sensor->location)
-			&& (SENSOR_ACCELEROMETER == sensor->type))
-			accel_base = sensor;
-
-		if ((LOCATION_LID == sensor->location)
-			&& (SENSOR_ACCELEROMETER == sensor->type)) {
-			accel_lid = sensor;
-		}
 	}
 
 	set_present(lpc_status);
@@ -364,69 +262,49 @@ void motion_sense_task(void)
 				if (sensor->state == SENSOR_NOT_INITIALIZED)
 					motion_sense_init(sensor);
 
-				if (EC_SUCCESS == motion_sense_read(sensor))
-					rd_cnt++;
+				if (EC_SUCCESS != motion_sense_read(sensor))
+					continue;
+
+				rd_cnt++;
+				/*
+				 * Rotate the accel vector so the reference for
+				 * all sensors are in the same space.
+				 */
+				if (*sensor->rot_standard_ref != NULL) {
+					rotate(sensor->xyz,
+					       *sensor->rot_standard_ref,
+					       sensor->xyz);
+				}
 			}
-
-			/*
-			 * Rotate the lid accel vector
-			 * so the reference frame aligns with the base sensor.
-			 */
-			if ((LOCATION_LID == sensor->location)
-				&& (SENSOR_ACCELEROMETER == sensor->type))
-				rotate(accel_lid->raw_xyz,
-					p_acc_orient->rot_align,
-					accel_lid->xyz);
-			else
-				memcpy(sensor->xyz, sensor->raw_xyz,
-					sizeof(vector_3_t));
 		}
 
 #ifdef CONFIG_GESTURE_DETECTION
 		/* Run gesture recognition engine */
 		gesture_calc();
 #endif
+#ifdef CONFIG_LID_ANGLE
+		if (rd_cnt == motion_sensor_count)
+			motion_lid_calc();
 
-		if (rd_cnt != motion_sensor_count)
-			goto motion_wait;
-
-		/* Calculate angle of lid accel. */
-		lid_angle_is_reliable = calculate_lid_angle(
-				accel_base->xyz,
-				accel_lid->xyz,
-				&lid_angle_deg);
-
-		for (i = 0; i < motion_sensor_count; ++i) {
-			sensor = &motion_sensors[i];
-			/* Rotate accels into standard reference frame. */
-			if (sensor->type == SENSOR_ACCELEROMETER)
-				rotate(sensor->xyz,
-					p_acc_orient->rot_standard_ref,
-					sensor->xyz);
-		}
-
-#ifdef CONFIG_LID_ANGLE_KEY_SCAN
-		lidangle_keyscan_update(motion_get_lid_angle());
 #endif
-
-#ifdef CONFIG_CMD_LID_ANGLE
+#ifdef CONFIG_CMD_ACCEL_INFO
 		if (accel_disp) {
 			CPRINTF("[%T ");
 			for (i = 0; i < motion_sensor_count; ++i) {
 				sensor = &motion_sensors[i];
 				CPRINTF("%s=%-5d, %-5d, %-5d ", sensor->name,
-					sensor->raw_xyz[X],
-					sensor->raw_xyz[Y],
-					sensor->raw_xyz[Z]);
+					sensor->xyz[X],
+					sensor->xyz[Y],
+					sensor->xyz[Z]);
 			}
-			CPRINTF("a=%-6.1d r=%d", (int)(10*lid_angle_deg),
-					lid_angle_is_reliable);
+#ifdef CONFIG_LID_ANGLE
+			CPRINTF("a=%-6.1d", 10 * motion_lid_get_angle());
+#endif
 			CPRINTF("]\n");
 		}
 #endif
 		update_sense_data(lpc_status, lpc_data, &sample_id);
 
-motion_wait:
 		/* Delay appropriately to keep sampling time consistent. */
 		ts1 = get_time();
 		wait_us = accel_interval_ms * MSEC - (ts1.val-ts0.val);
@@ -442,24 +320,6 @@ motion_wait:
 	}
 }
 
-void accel_int_lid(enum gpio_signal signal)
-{
-	/*
-	 * Print statement is here for testing with console accelint command.
-	 * Remove print statement when interrupt is used for real.
-	 */
-	CPRINTS("Accelerometer wake-up interrupt occurred on lid");
-}
-
-void accel_int_base(enum gpio_signal signal)
-{
-	/*
-	 * Print statement is here for testing with console accelint command.
-	 * Remove print statement when interrupt is used for real.
-	 */
-	CPRINTS("Accelerometer wake-up interrupt occurred on base");
-}
-
 /*****************************************************************************/
 /* Host commands */
 
@@ -510,7 +370,7 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 	const struct ec_params_motion_sense *in = args->params;
 	struct ec_response_motion_sense *out = args->response;
 	struct motion_sensor_t *sensor;
-	int i, data;
+	int i, data, ret = EC_RES_INVALID_PARAM;
 
 	switch (in->cmd) {
 	case MOTIONSENSE_CMD_DUMP:
@@ -637,24 +497,15 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 		out->sensor_range.ret = data;
 		args->response_size = sizeof(out->sensor_range);
 		break;
-
-	case MOTIONSENSE_CMD_KB_WAKE_ANGLE:
-#ifdef CONFIG_LID_ANGLE_KEY_SCAN
-		/* Set new keyboard wake lid angle if data arg has value. */
-		if (in->kb_wake_angle.data != EC_MOTION_SENSE_NO_VALUE)
-			lid_angle_set_kb_wake_angle(in->kb_wake_angle.data);
-
-		out->kb_wake_angle.ret = lid_angle_get_kb_wake_angle();
-#else
-		out->kb_wake_angle.ret = 0;
-#endif
-		args->response_size = sizeof(out->kb_wake_angle);
-
-		break;
-
 	default:
-		CPRINTS("MS bad cmd 0x%x", in->cmd);
-		return EC_RES_INVALID_PARAM;
+		/* Call other users of the motion task */
+#ifdef CONFIG_LID_ANGLE
+		if (ret == EC_RES_INVALID_PARAM)
+			ret = host_cmd_motion_lid(args);
+#endif
+		if (ret == EC_RES_INVALID_PARAM)
+			CPRINTS("MS bad cmd 0x%x", in->cmd);
+		return ret;
 	}
 
 	return EC_RES_SUCCESS;
@@ -666,43 +517,6 @@ DECLARE_HOST_COMMAND(EC_CMD_MOTION_SENSE_CMD,
 
 /*****************************************************************************/
 /* Console commands */
-#ifdef CONFIG_CMD_LID_ANGLE
-static int command_ctrl_print_lid_angle_calcs(int argc, char **argv)
-{
-	char *e;
-	int val;
-
-	if (argc > 3)
-		return EC_ERROR_PARAM_COUNT;
-
-	/* First argument is on/off whether to display accel data. */
-	if (argc > 1) {
-		if (!parse_bool(argv[1], &val))
-			return EC_ERROR_PARAM1;
-
-		accel_disp = val;
-	}
-
-	/*
-	 * Second arg changes the accel task time interval. Note accel
-	 * sampling interval will be clobbered when chipset suspends or
-	 * resumes.
-	 */
-	if (argc > 2) {
-		val = strtoi(argv[2], &e, 0);
-		if (*e)
-			return EC_ERROR_PARAM2;
-
-		accel_interval_ms = val;
-	}
-
-	return EC_SUCCESS;
-}
-DECLARE_CONSOLE_COMMAND(lidangle, command_ctrl_print_lid_angle_calcs,
-	"on/off [interval]",
-	"Print lid angle calculations and set calculation frequency.", NULL);
-#endif /* CONFIG_CMD_LID_ANGLE */
-
 #ifdef CONFIG_CMD_ACCELS
 static int command_accelrange(int argc, char **argv)
 {
@@ -905,7 +719,64 @@ DECLARE_CONSOLE_COMMAND(accelinit, command_accel_init,
 	"id",
 	"Init sensor", NULL);
 
+#ifdef CONFIG_CMD_ACCEL_INFO
+static int command_display_accel_info(int argc, char **argv)
+{
+	char *e;
+	int val;
+
+	if (argc > 3)
+		return EC_ERROR_PARAM_COUNT;
+
+	/* First argument is on/off whether to display accel data. */
+	if (argc > 1) {
+		if (!parse_bool(argv[1], &val))
+			return EC_ERROR_PARAM1;
+
+		accel_disp = val;
+	}
+
+	/*
+	 * Second arg changes the accel task time interval. Note accel
+	 * sampling interval will be clobbered when chipset suspends or
+	 * resumes.
+	 */
+	if (argc > 2) {
+		val = strtoi(argv[2], &e, 0);
+		if (*e)
+			return EC_ERROR_PARAM2;
+
+		accel_interval_ms = val;
+	}
+
+	return EC_SUCCESS;
+}
+DECLARE_CONSOLE_COMMAND(accelinfo, command_display_accel_info,
+	"on/off [interval]",
+	"Print motion sensor info, lid angle calculations"
+	" and set calculation frequency.", NULL);
+#endif /* CONFIG_CMD_ACCEL_INFO */
+
 #ifdef CONFIG_ACCEL_INTERRUPTS
+/* TODO(crosbug.com/p/426659): this code is broken, does not compile. */
+void accel_int_lid(enum gpio_signal signal)
+{
+	/*
+	 * Print statement is here for testing with console accelint command.
+	 * Remove print statement when interrupt is used for real.
+	 */
+	CPRINTS("Accelerometer wake-up interrupt occurred on lid");
+}
+
+void accel_int_base(enum gpio_signal signal)
+{
+	/*
+	 * Print statement is here for testing with console accelint command.
+	 * Remove print statement when interrupt is used for real.
+	 */
+	CPRINTS("Accelerometer wake-up interrupt occurred on base");
+}
+
 static int command_accelerometer_interrupt(int argc, char **argv)
 {
 	char *e;
diff --git a/driver/accelgyro_lsm6ds0.c b/driver/accelgyro_lsm6ds0.c
index 1083cb5c71..d25b91b594 100644
--- a/driver/accelgyro_lsm6ds0.c
+++ b/driver/accelgyro_lsm6ds0.c
@@ -335,9 +335,9 @@ static int read(const struct motion_sensor_t *s,
 	 * to get the latest updated sensor data quickly.
 	 */
 	if (!tmp) {
-		*x = s->raw_xyz[0];
-		*y = s->raw_xyz[1];
-		*z = s->raw_xyz[2];
+		*x = s->xyz[0];
+		*y = s->xyz[1];
+		*z = s->xyz[2];
 		return EC_SUCCESS;
 	}
 
diff --git a/include/config.h b/include/config.h
index 309d061541..dd0815e22c 100644
--- a/include/config.h
+++ b/include/config.h
@@ -308,6 +308,7 @@
  */
 
 #undef CONFIG_CMD_ACCELS
+#undef CONFIG_CMD_ACCEL_INFO
 #undef CONFIG_CMD_BATDEBUG
 #undef CONFIG_CMD_CLOCKGATES
 #undef CONFIG_CMD_COMXTEST
@@ -317,7 +318,6 @@
 #undef CONFIG_CMD_HOSTCMD
 #undef CONFIG_CMD_ILIM
 #undef CONFIG_CMD_JUMPTAGS
-#define CONFIG_CMD_LID_ANGLE
 #undef CONFIG_CMD_PLL
 #undef CONFIG_CMD_PMU
 #define CONFIG_CMD_POWERINDEBUG
@@ -564,6 +564,18 @@
 
 #endif
 
+#undef CONFIG_LID_ANGLE
+#ifndef CONFIG_LID_ANGLE
+#undef CONFIG_SENSOR_BASE
+#undef CONFIG_SENSOR_LID
+
+/*
+ * Allows using the lid angle measurement to determine if key scanning should
+ * be enabled or disabled when chipset is suspended.
+ */
+#undef CONFIG_LID_ANGLE_KEY_SCAN
+
+#endif
 
 
 /*****************************************************************************/
@@ -729,12 +741,6 @@
 #undef CONFIG_LED_DRIVER_LP5562  /* LP5562, on I2C interface */
 
 /*
- * Allows using the lid angle measurement to determine if key scanning should
- * be enabled or disabled when chipset is suspended.
- */
-#undef CONFIG_LID_ANGLE_KEY_SCAN
-
-/*
  * Compile lid switch support.
  *
  * This is enabled by default because all boards other than reference boards
diff --git a/include/console.h b/include/console.h
index bf2cd536f1..29599dda4b 100644
--- a/include/console.h
+++ b/include/console.h
@@ -43,6 +43,7 @@ enum console_channel {
 	CC_LIDANGLE,
 	CC_LIGHTBAR,
 	CC_LPC,
+	CC_MOTION_LID,
 	CC_MOTION_SENSE,
 	CC_PD_HOST_CMD,
 	CC_PORT80,
diff --git a/include/motion_lid.h b/include/motion_lid.h
new file mode 100644
index 0000000000..b3348c020e
--- /dev/null
+++ b/include/motion_lid.h
@@ -0,0 +1,50 @@
+/* Copyright (c) 2014 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/* Header for motion_sense.c */
+
+#ifndef __CROS_EC_MOTION_LID_H
+#define __CROS_EC_MOTION_LID_H
+
+/* Anything outside of lid angle range [-180, 180] should work. */
+#define LID_ANGLE_UNRELIABLE 500.0F
+
+/**
+ * This structure defines all of the data needed to specify the orientation
+ * of the base and lid accelerometers in order to calculate the lid angle.
+ */
+struct accel_orientation {
+	/* Rotation matrix to rotate positive 90 degrees around the hinge. */
+	matrix_3x3_t rot_hinge_90;
+
+	/*
+	 * Rotation matrix to rotate 180 degrees around the hinge. The value
+	 * here should be rot_hinge_90 ^ 2.
+	 */
+	matrix_3x3_t rot_hinge_180;
+
+	/* Vector pointing along hinge axis. */
+	vector_3_t hinge_axis;
+};
+
+/* Link global structure for orientation. This must be defined in board.c. */
+extern const struct accel_orientation acc_orient;
+
+
+/**
+ * Get last calculated lid angle. Note, the lid angle calculated by the EC
+ * is un-calibrated and is an approximate angle.
+ *
+ * @return lid angle in degrees in range [0, 360].
+ */
+int motion_lid_get_angle(void);
+
+int host_cmd_motion_lid(struct host_cmd_handler_args *args);
+
+void motion_lid_calc(void);
+
+#endif  /* __CROS_EC_MOTION_LID_H */
+
+
diff --git a/include/motion_sense.h b/include/motion_sense.h
index be764a8d00..bb5acc308c 100644
--- a/include/motion_sense.h
+++ b/include/motion_sense.h
@@ -12,66 +12,6 @@
 #include "math_util.h"
 #include "chipset.h"
 
-/* Anything outside of lid angle range [-180, 180] should work. */
-#define LID_ANGLE_UNRELIABLE 500.0F
-
-/**
- * This structure defines all of the data needed to specify the orientation
- * of the base and lid accelerometers in order to calculate the lid angle.
- */
-struct accel_orientation {
-	/*
-	 * Rotation matrix to rotate the lid sensor into the same reference
-	 * frame as the base sensor.
-	 */
-	matrix_3x3_t rot_align;
-
-	/* Rotation matrix to rotate positive 90 degrees around the hinge. */
-	matrix_3x3_t rot_hinge_90;
-
-	/*
-	 * Rotation matrix to rotate 180 degrees around the hinge. The value
-	 * here should be rot_hinge_90 ^ 2.
-	 */
-	matrix_3x3_t rot_hinge_180;
-
-	/*
-	 * Rotation matrix to rotate base sensor into the standard reference
-	 * frame.
-	 */
-	matrix_3x3_t rot_standard_ref;
-
-	/* Vector pointing along hinge axis. */
-	vector_3_t hinge_axis;
-};
-
-/* Link global structure for orientation. This must be defined in board.c. */
-extern const struct accel_orientation acc_orient;
-
-
-/**
- * Get last calculated lid angle. Note, the lid angle calculated by the EC
- * is un-calibrated and is an approximate angle.
- *
- * @return lid angle in degrees in range [0, 360].
- */
-int motion_get_lid_angle(void);
-
-
-/**
- * Interrupt function for lid accelerometer.
- *
- * @param signal GPIO signal that caused interrupt
- */
-void accel_int_lid(enum gpio_signal signal);
-
-/**
- * Interrupt function for base accelerometer.
- *
- * @param signal GPIO signal that caused interrupt
- */
-void accel_int_base(enum gpio_signal signal);
-
 enum sensor_location_t {
 	LOCATION_BASE = 0,
 	LOCATION_LID  = 1,
@@ -110,6 +50,7 @@ struct motion_sensor_t {
 	struct mutex *mutex;
 	void *drv_data;
 	uint8_t i2c_addr;
+	const matrix_3x3_t *rot_standard_ref;
 
 	/* Default configuration parameters, RO only */
 	int default_odr;
@@ -122,9 +63,7 @@ struct motion_sensor_t {
 	/* state parameters */
 	enum sensor_state state;
 	enum chipset_state_mask active;
-	vector_3_t raw_xyz;
 	vector_3_t xyz;
-
 };
 
 /* Defined at board level. */
@@ -137,4 +76,20 @@ extern const unsigned int motion_sensor_count;
  */
 #define MOTION_SENSE_HOOK_PRIO (HOOK_PRIO_DEFAULT)
 
+#ifdef CONFIG_ACCEL_INTERRUPTS
+/**
+ * Interrupt function for lid accelerometer.
+ *
+ * @param signal GPIO signal that caused interrupt
+ */
+void accel_int_lid(enum gpio_signal signal);
+
+/**
+ * Interrupt function for base accelerometer.
+ *
+ * @param signal GPIO signal that caused interrupt
+ */
+void accel_int_base(enum gpio_signal signal);
+#endif
+
 #endif /* __CROS_EC_MOTION_SENSE_H */
diff --git a/test/build.mk b/test/build.mk
index 8995c41fb9..593142b58d 100644
--- a/test/build.mk
+++ b/test/build.mk
@@ -30,7 +30,7 @@ test-list-host=mutex pingpong utils kb_scan kb_mkbp lid_sw power_button hooks
 test-list-host+=thermal flash queue kb_8042 extpwr_gpio console_edit system
 test-list-host+=sbs_charging adapter host_command thermal_falco led_spring
 test-list-host+=bklight_lid bklight_passthru interrupt timer_dos button
-test-list-host+=motion_sense math_util sbs_charging_v2 battery_get_params_smart
+test-list-host+=motion_lid math_util sbs_charging_v2 battery_get_params_smart
 test-list-host+=lightbar inductive_charging usb_pd
 
 adapter-y=adapter.o
@@ -52,7 +52,7 @@ kb_scan-y=kb_scan.o
 led_spring-y=led_spring.o led_spring_impl.o
 lid_sw-y=lid_sw.o
 math_util-y=math_util.o
-motion_sense-y=motion_sense.o
+motion_lid-y=motion_lid.o
 mutex-y=mutex.o
 pingpong-y=pingpong.o
 power_button-y=power_button.o
diff --git a/test/motion_sense.c b/test/motion_lid.c
index 5148935533..e8880e85a8 100644
--- a/test/motion_sense.c
+++ b/test/motion_lid.c
@@ -11,6 +11,7 @@
 #include "common.h"
 #include "hooks.h"
 #include "host_command.h"
+#include "motion_lid.h"
 #include "motion_sense.h"
 #include "task.h"
 #include "test_util.h"
@@ -88,15 +89,27 @@ const struct accelgyro_drv test_motion_sense = {
 	.get_data_rate = accel_get_data_rate,
 };
 
+const matrix_3x3_t base_standard_ref = {
+	{ 1, 0, 0},
+	{ 0, 1, 0},
+	{ 0, 0, 1}
+};
+
+const matrix_3x3_t lid_standard_ref = {
+	{ 1, 0, 0},
+	{ 1, 0, 0},
+	{ 0, 0, 1}
+};
+
 struct motion_sensor_t motion_sensors[] = {
 	{SENSOR_ACTIVE_S0_S3_S5, "base", SENSOR_CHIP_LSM6DS0,
 		SENSOR_ACCELEROMETER, LOCATION_BASE,
 		&test_motion_sense, NULL, NULL,
-		0, 119000, 2},
+		0, &base_standard_ref, 119000, 2},
 	{SENSOR_ACTIVE_S0, "lid", SENSOR_CHIP_KXCJ9,
 		SENSOR_ACCELEROMETER, LOCATION_LID,
 		&test_motion_sense, NULL, NULL,
-		0, 100000, 2},
+		0, &lid_standard_ref, 100000, 2},
 };
 const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);
 
@@ -130,7 +143,7 @@ static int test_lid_angle(void)
 	task_wake(TASK_ID_MOTIONSENSE);
 	while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample)
 		msleep(5);
-	TEST_ASSERT(motion_get_lid_angle() == 0);
+	TEST_ASSERT(motion_lid_get_angle() == 0);
 
 	/* Set lid open to 90 degrees. */
 	lid->xyz[X] = -1000;
@@ -140,7 +153,7 @@ static int test_lid_angle(void)
 	task_wake(TASK_ID_MOTIONSENSE);
 	while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample)
 		msleep(5);
-	TEST_ASSERT(motion_get_lid_angle() == 90);
+	TEST_ASSERT(motion_lid_get_angle() == 90);
 
 	/* Set lid open to 225. */
 	lid->xyz[X] = 500;
@@ -150,7 +163,7 @@ static int test_lid_angle(void)
 	task_wake(TASK_ID_MOTIONSENSE);
 	while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample)
 		msleep(5);
-	TEST_ASSERT(motion_get_lid_angle() == 225);
+	TEST_ASSERT(motion_lid_get_angle() == 225);
 
 	/*
 	 * Align base with hinge and make sure it returns unreliable for angle.
@@ -163,7 +176,7 @@ static int test_lid_angle(void)
 	task_wake(TASK_ID_MOTIONSENSE);
 	while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample)
 		msleep(5);
-	TEST_ASSERT(motion_get_lid_angle() == LID_ANGLE_UNRELIABLE);
+	TEST_ASSERT(motion_lid_get_angle() == LID_ANGLE_UNRELIABLE);
 
 	/*
 	 * Use all three axes and set lid to negative base and make sure
@@ -179,7 +192,7 @@ static int test_lid_angle(void)
 	task_wake(TASK_ID_MOTIONSENSE);
 	while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample)
 		msleep(5);
-	TEST_ASSERT(motion_get_lid_angle() == 180);
+	TEST_ASSERT(motion_lid_get_angle() == 180);
 
 	return EC_SUCCESS;
 }
diff --git a/test/motion_sense.tasklist b/test/motion_lid.tasklist
index 60688f9c34..60688f9c34 100644
--- a/test/motion_sense.tasklist
+++ b/test/motion_lid.tasklist
diff --git a/test/test_config.h b/test/test_config.h
index 24ccb19024..9a06858834 100644
--- a/test/test_config.h
+++ b/test/test_config.h
@@ -50,6 +50,12 @@
 #define I2C_PORT_CHARGER 1
 #endif
 
+#ifdef TEST_MOTION_LID
+#define CONFIG_LID_ANGLE
+#define CONFIG_SENSOR_BASE 0
+#define CONFIG_SENSOR_LID 1
+#endif
+
 #ifdef TEST_SBS_CHARGING
 #define CONFIG_BATTERY_MOCK
 #define CONFIG_BATTERY_SMART