rambi: Add motion sense task to track motion

Added motion sense task to Clapper and Glimmer. This task samples
the accelerometers and calculate a lid angle. Note that as
the machine is rotated towards the hinge angle aligning
with gravity, the lid calculation becomes less trustworthy.

Added a math_util file to hold various mathematical functions
useful for calculating lid angle that may be helpful in other
places.

For each board with accelerometers we need to define some
orientation specific data in board.c. There is a calibration
procedure through the EC console that can be enabled by
defining CONFIG_ACCEL_CALIBRATE. The calibration procedure can
help determine the orientation data required.

For debugging purposes there is a console command to regularly
print to the EC console the accelerometer data and derived lid
angle. The console command can be enabled by defining
CONFIG_CMD_LID_ANGLE.

BUG=none
Original-BUG=chrome-os-partner:24703
BRANCH=rambi
TEST=Ran the calibration procedure on a Glimmer unit, and then
rotated the machine in space. Verified that the lid angle
calculated roughly matched actual lid angle.

Original-Change-Id: I63a5e384b7f6b628b4ea01de49843355fb8d6ebe
Signed-off-by: Alec Berg <alecaberg@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/184783
Reviewed-by: Randall Spangler <rspangler@chromium.org>
Signed-off-by: Alec Berg <alecaberg@chromium.org>
(cherry picked from commit efb07945a5159fa0e7a746c666b2519ebdca9c22)

Conflicts:
	board/clapper/board.c
	board/clapper/ec.tasklist
	board/glimmer/board.c
	board/glimmer/ec.tasklist

Change-Id: Ibc492ef5c11e7084e87f01338c4d7775f9a08c18
Signed-off-by: Alec Berg <alecaberg@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/187433
Reviewed-by: Randall Spangler <rspangler@chromium.org>

5 files changed, 661 insertions, 0 deletions

diff --git a/common/build.mk b/common/build.mk
index ea2546dbe9..40b393bc09 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -10,6 +10,7 @@ common-y=main.o util.o console_output.o uart_buffering.o
 common-y+=memory_commands.o shared_mem.o system.o hooks.o
 common-y+=gpio.o version.o printf.o queue.o
 
+common-$(CONFIG_ACCEL_CALIBRATE)+=motion_calibrate.o
 common-$(CONFIG_ADC)+=adc.o
 common-$(CONFIG_ALS)+=als.o
 common-$(CONFIG_AP_HANG_DETECT)+=ap_hang_detect.o
@@ -63,4 +64,6 @@ common-$(HAS_TASK_CONSOLE)+=console.o
 common-$(HAS_TASK_HOSTCMD)+=acpi.o host_command.o host_event_commands.o
 common-$(HAS_TASK_KEYSCAN)+=keyboard_scan.o
 common-$(HAS_TASK_LIGHTBAR)+=lightbar.o
+common-$(HAS_TASK_MOTIONSENSE)+=motion_sense.o math_util.o
 common-$(TEST_BUILD)+=test_util.o
+
diff --git a/common/console_output.c b/common/console_output.c
index 994e954e0d..4c6dde279c 100644
--- a/common/console_output.c
+++ b/common/console_output.c
@@ -44,6 +44,7 @@ static const char * const channel_names[] = {
 	"keyscan",
 	"lightbar",
 	"lpc",
+	"motionsense",
 	"port80",
 	"pwm",
 	"spi",
diff --git a/common/math_util.c b/common/math_util.c
new file mode 100644
index 0000000000..7487019c44
--- /dev/null
+++ b/common/math_util.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.
+ */
+
+/* Common math functions. */
+
+#include "common.h"
+#include "math.h"
+#include "math_util.h"
+#include "util.h"
+
+/* For cosine lookup table, define the increment and the size of the table. */
+#define COSINE_LUT_INCR_DEG	5
+#define COSINE_LUT_SIZE		((180 / COSINE_LUT_INCR_DEG) + 1)
+
+/* Lookup table for the value of cosine from 0 degrees to 180 degrees. */
+static const float cos_lut[] = {
+	 1.00000,  0.99619,  0.98481,  0.96593,  0.93969,
+	 0.90631,  0.86603,  0.81915,  0.76604,  0.70711,
+	 0.64279,  0.57358,  0.50000,  0.42262,  0.34202,
+	 0.25882,  0.17365,  0.08716,  0.00000, -0.08716,
+	-0.17365, -0.25882, -0.34202, -0.42262, -0.50000,
+	-0.57358, -0.64279, -0.70711, -0.76604, -0.81915,
+	-0.86603, -0.90631, -0.93969, -0.96593, -0.98481,
+	-0.99619, -1.00000,
+};
+BUILD_ASSERT(ARRAY_SIZE(cos_lut) == COSINE_LUT_SIZE);
+
+
+float arc_cos(float x)
+{
+	int i;
+
+	/* Cap x if out of range. */
+	if (x < -1.0)
+		x = -1.0;
+	else if (x > 1.0)
+		x = 1.0;
+
+	/*
+	 * Increment through lookup table to find index and then linearly
+	 * interpolate for precision.
+	 */
+	/* TODO(crosbug.com/p/25600): Optimize with binary search. */
+	for (i = 0; i < COSINE_LUT_SIZE-1; i++)
+		if (x >= cos_lut[i+1])
+			return COSINE_LUT_INCR_DEG *
+			(i + (cos_lut[i] - x) / (cos_lut[i] - cos_lut[i+1]));
+
+	/*
+	 * Shouldn't be possible to get here because inputs are clipped to
+	 * [-1, 1] and the cos_lut[] table goes over the same range. If we
+	 * are here, throw an assert.
+	 */
+	ASSERT(0);
+
+	return 0;
+}
+
+int vector_magnitude(const vector_3_t v)
+{
+	return sqrtf(SQ(v[0]) + SQ(v[1]) + SQ(v[2]));
+}
+
+float cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2)
+{
+	int dotproduct;
+	float denominator;
+
+	/*
+	 * Angle between two vectors is acos(A dot B / |A|*|B|). To return
+	 * cosine of angle between vectors, then don't do acos operation.
+	 */
+
+	dotproduct = v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
+
+	denominator = vector_magnitude(v1) * vector_magnitude(v2);
+
+	/* Check for divide by 0 although extremely unlikely. */
+	if (ABS(denominator) < 0.01F)
+		return 0.0;
+
+	return (float)dotproduct / (denominator);
+}
+
+void rotate(const vector_3_t v, const matrix_3x3_t (* const R),
+		vector_3_t *res)
+{
+	(*res)[0] =	v[0] * (*R)[0][0] +
+			v[1] * (*R)[1][0] +
+			v[2] * (*R)[2][0];
+	(*res)[1] =	v[0] * (*R)[0][1] +
+			v[1] * (*R)[1][1] +
+			v[2] * (*R)[2][1];
+	(*res)[2] =	v[0] * (*R)[0][2] +
+			v[1] * (*R)[1][2] +
+			v[2] * (*R)[2][2];
+}
+
+#ifdef CONFIG_ACCEL_CALIBRATE
+
+void matrix_multiply(matrix_3x3_t *m1, matrix_3x3_t *m2, matrix_3x3_t *res)
+{
+	(*res)[0][0] = (*m1)[0][0] * (*m2)[0][0] + (*m1)[0][1] * (*m2)[1][0] +
+			(*m1)[0][2] * (*m2)[2][0];
+	(*res)[0][1] = (*m1)[0][0] * (*m2)[0][1] + (*m1)[0][1] * (*m2)[1][1] +
+				(*m1)[0][2] * (*m2)[2][1];
+	(*res)[0][2] = (*m1)[0][0] * (*m2)[0][2] + (*m1)[0][1] * (*m2)[1][2] +
+				(*m1)[0][2] * (*m2)[2][2];
+
+	(*res)[1][0] = (*m1)[1][0] * (*m2)[0][0] + (*m1)[1][1] * (*m2)[1][0] +
+				(*m1)[1][2] * (*m2)[2][0];
+	(*res)[1][1] = (*m1)[1][0] * (*m2)[0][1] + (*m1)[1][1] * (*m2)[1][1] +
+				(*m1)[1][2] * (*m2)[2][1];
+	(*res)[1][2] = (*m1)[1][0] * (*m2)[0][2] + (*m1)[1][1] * (*m2)[1][2] +
+				(*m1)[1][2] * (*m2)[2][2];
+
+	(*res)[2][0] = (*m1)[2][0] * (*m2)[0][0] + (*m1)[2][1] * (*m2)[1][0] +
+				(*m1)[2][2] * (*m2)[2][0];
+	(*res)[2][1] = (*m1)[2][0] * (*m2)[0][1] + (*m1)[2][1] * (*m2)[1][1] +
+				(*m1)[2][2] * (*m2)[2][1];
+	(*res)[2][2] = (*m1)[2][0] * (*m2)[0][2] + (*m1)[2][1] * (*m2)[1][2] +
+				(*m1)[2][2] * (*m2)[2][2];
+}
+
+/**
+ * Find determinant of matrix.
+ *
+ * @param m Pointer to matrix to take determinant of.
+ *
+ * @return Determinant of matrix m.
+ */
+static float matrix_determinant(matrix_3x3_t *m)
+{
+		return  (*m)[0][0]*(*m)[1][1]*(*m)[2][2] +
+			(*m)[1][0]*(*m)[2][1]*(*m)[0][2] +
+			(*m)[2][0]*(*m)[0][1]*(*m)[1][2] -
+			(*m)[0][0]*(*m)[2][1]*(*m)[1][2] -
+			(*m)[2][0]*(*m)[1][1]*(*m)[0][2] -
+			(*m)[1][0]*(*m)[0][1]*(*m)[2][2];
+}
+
+/**
+ * Find inverse of matrix.
+ *
+ * @param m Pointer to matrix to invert.
+ * @param res Pointer to resultant matrix.
+ */
+static int matrix_inverse(matrix_3x3_t *m, matrix_3x3_t *res)
+{
+	float det = matrix_determinant(m);
+
+	/*
+	 * If determinant is too close to zero, then input is not linearly
+	 * independent enough, return an error.
+	 */
+	if (ABS(det) < 1e7)
+		return EC_ERROR_UNKNOWN;
+
+	/* Find inverse of input matrix. */
+	(*res)[0][0] = ((*m)[1][1]*(*m)[2][2] - (*m)[1][2]*(*m)[2][1]) / det;
+	(*res)[0][1] = ((*m)[0][2]*(*m)[2][1] - (*m)[0][1]*(*m)[2][2]) / det;
+	(*res)[0][2] = ((*m)[0][1]*(*m)[1][2] - (*m)[0][2]*(*m)[1][1]) / det;
+
+	(*res)[1][0] = ((*m)[1][2]*(*m)[2][0] - (*m)[1][0]*(*m)[2][2]) / det;
+	(*res)[1][1] = ((*m)[0][0]*(*m)[2][2] - (*m)[0][2]*(*m)[2][0]) / det;
+	(*res)[1][2] = ((*m)[0][2]*(*m)[1][0] - (*m)[0][0]*(*m)[1][2]) / det;
+
+	(*res)[2][0] = ((*m)[1][0]*(*m)[2][1] - (*m)[1][1]*(*m)[2][0]) / det;
+	(*res)[2][1] = ((*m)[0][1]*(*m)[2][0] - (*m)[0][0]*(*m)[2][1]) / det;
+	(*res)[2][2] = ((*m)[0][0]*(*m)[1][1] - (*m)[0][1]*(*m)[1][0]) / det;
+
+	return EC_SUCCESS;
+}
+
+int solve_rotation_matrix(matrix_3x3_t *in, matrix_3x3_t *out, matrix_3x3_t *R)
+{
+	static matrix_3x3_t i;
+	int ret;
+
+	/* Calculate inverse of input matrix. */
+	ret = matrix_inverse(in, &i);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	/* Multiply inverse of in matrix by out matrix and store into R. */
+	matrix_multiply(&i, out, R);
+
+	return EC_SUCCESS;
+}
+#endif /* CONFIG_ACCEL_CALIBRATE */
diff --git a/common/motion_calibrate.c b/common/motion_calibrate.c
new file mode 100644
index 0000000000..3a1033d534
--- /dev/null
+++ b/common/motion_calibrate.c
@@ -0,0 +1,251 @@
+/* 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 sensor calibration code. */
+
+#include "accelerometer.h"
+#include "common.h"
+#include "console.h"
+#include "math_util.h"
+#include "motion_sense.h"
+#include "timer.h"
+#include "task.h"
+#include "util.h"
+
+/*
+ * Threshold to capture a sample when performing auto-calibrate. The units are
+ * the same as the units of the accelerometer acceleration values.
+ */
+#define AUTO_CAL_DIR_THRESHOLD (ACCEL_G * 3 / 4)
+#define AUTO_CAL_MAG_THRESHOLD (ACCEL_G / 20)
+
+/*****************************************************************************/
+/* Console commands */
+
+/**
+ * Print all orientation calibration data.
+ */
+static int command_print_orientation(int argc, char **argv)
+{
+	matrix_3x3_t (*R);
+
+	R = &acc_orient.rot_align;
+	ccprintf("Lid to base alignment R:\n%.2d\t%.2d\t%.2d\n%.2d\t%.2d\t%.2d"
+			"\n%.2d\t%.2d\t%.2d\n\n",
+	(int)((*R)[0][0]*100), (int)((*R)[0][1]*100), (int)((*R)[0][2]*100),
+	(int)((*R)[1][0]*100), (int)((*R)[1][1]*100), (int)((*R)[1][2]*100),
+	(int)((*R)[2][0]*100), (int)((*R)[2][1]*100), (int)((*R)[2][2]*100));
+
+	R = &acc_orient.rot_hinge_90;
+	ccprintf("Hinge rotation 90 R:\n%.2d\t%.2d\t%.2d\n%.2d\t%.2d\t%.2d\n"
+			"%.2d\t%.2d\t%.2d\n\n",
+	(int)((*R)[0][0]*100), (int)((*R)[0][1]*100), (int)((*R)[0][2]*100),
+	(int)((*R)[1][0]*100), (int)((*R)[1][1]*100), (int)((*R)[1][2]*100),
+	(int)((*R)[2][0]*100), (int)((*R)[2][1]*100), (int)((*R)[2][2]*100));
+
+	R = &acc_orient.rot_hinge_180;
+	ccprintf("Hinge rotation 180 R:\n%.2d\t%.2d\t%.2d\n%.2d\t%.2d\t%.2d\n"
+			"%.2d\t%.2d\t%.2d\n\n",
+	(int)((*R)[0][0]*100), (int)((*R)[0][1]*100), (int)((*R)[0][2]*100),
+	(int)((*R)[1][0]*100), (int)((*R)[1][1]*100), (int)((*R)[1][2]*100),
+	(int)((*R)[2][0]*100), (int)((*R)[2][1]*100), (int)((*R)[2][2]*100));
+
+	ccprintf("Hinge Axis:\t%d\t%d\t%d\n", acc_orient.hinge_axis[0],
+			acc_orient.hinge_axis[1],
+			acc_orient.hinge_axis[2]);
+
+	return EC_SUCCESS;
+}
+DECLARE_CONSOLE_COMMAND(accelorient, command_print_orientation,
+	"",
+	"Print all orientation calibration data", NULL);
+
+/**
+ * Calibrate the orientation and print results to console.
+ *
+ * @param type	0 is for calibrating lid to base alignment,
+ *		1 is for calibrating hinge 90 rotation
+ */
+static int calibrate_orientation(int type)
+{
+	int mag, ret, i, j;
+
+	/* Captured flags. Set true when the corresponding axis is captured. */
+	int captured[3] = {0, 0, 0};
+
+	/* Current acceleration vectors. */
+	vector_3_t base, lid;
+
+	static matrix_3x3_t rec_base, rec_lid;
+
+	while (1) {
+		/* Capture the current acceleration vectors. */
+		motion_get_accel_lid(&lid, type);
+		motion_get_accel_base(&base);
+
+		/* Measure magnitude of base accelerometer. */
+		mag = vector_magnitude(base);
+
+		/*
+		 * Only capture a sample if the magnitude of the acceleration
+		 * is close to G, because this assures we won't calibrate with
+		 * values biased by motion.
+		 */
+		if ((mag > ACCEL_G - AUTO_CAL_MAG_THRESHOLD) &&
+			(mag < ACCEL_G + AUTO_CAL_MAG_THRESHOLD)) {
+
+			/*
+			 * Capture a sample when each axis exceeds some
+			 * threshold. This guarantees linear independence.
+			 */
+			for (i = 0; i < 3; i++) {
+				if (!captured[i] &&
+					ABS(base[i]) > AUTO_CAL_DIR_THRESHOLD) {
+
+					for (j = 0; j < 3; j++) {
+						rec_base[i][j] = (float)base[j];
+						rec_lid[i][j] = (float)lid[j];
+					}
+					ccprintf("Captured axis %d\n", i);
+					captured[i] = 1;
+				}
+			}
+
+			/* If all axes are captured, we are done. */
+			if (captured[0] && captured[1] && captured[2])
+				break;
+		}
+
+		/* Wait until next reading. */
+		task_wait_event(50*MSEC);
+	}
+
+	/* Solve for the rotation matrix and display final rotation matrix. */
+	if (type == 0)
+		ret = solve_rotation_matrix(&rec_lid, &rec_base,
+					&acc_orient.rot_align);
+	else
+		ret = solve_rotation_matrix(&rec_base, &rec_lid,
+					&acc_orient.rot_hinge_90);
+
+	if (ret != EC_SUCCESS)
+		ccprintf("Failed to find rotation matrix.\n");
+
+	return ret;
+}
+
+/**
+ * Calibrate the hinge axis and hinge 180 rotation matrix.
+ */
+static int calibrate_hinge(void)
+{
+	static matrix_3x3_t tmp;
+	float d;
+	int i, j;
+	vector_3_t base;
+
+	motion_get_accel_base(&base);
+	memcpy(&acc_orient.hinge_axis, &base, sizeof(vector_3_t));
+
+	/*
+	 * Calculate a rotation matrix to rotate 180 degrees about hinge axis.
+	 * The formula is:
+	 *
+	 * rot_hinge_180 = I + 2 * tmp^2 / d^2,
+	 * where tmp is a matrix formed from the hinge axis, d is the sqrt
+	 * of the hinge axis vector used in tmp, and I is the 3x3 identity
+	 * matrix.
+	 *
+	 */
+	tmp[0][0] = 0;
+	tmp[0][1] = base[2];
+	tmp[0][2] = -base[1];
+	tmp[1][0] = -base[2];
+	tmp[1][1] = 0;
+	tmp[1][2] = base[0];
+	tmp[2][0] = base[1];
+	tmp[2][1] = -base[0];
+	tmp[2][2] = 0;
+
+	matrix_multiply(&tmp, &tmp, &acc_orient.rot_hinge_180);
+	d = (float)(SQ(base[0]) + SQ(base[1]) + SQ(base[2]));
+
+	for (i = 0; i < 3; i++) {
+		for (j = 0; j < 3; j++) {
+			acc_orient.rot_hinge_180[i][j] *= 2.0F / d;
+
+			/* Add identity matrix. */
+			if (i == j)
+				acc_orient.rot_hinge_180[i][j] += 1;
+		}
+	}
+
+	return EC_SUCCESS;
+}
+
+static int command_auto_calibrate(int argc, char **argv)
+{
+	char *e;
+	int type, ret;
+	static int last_type = -1;
+
+	if (argc != 2)
+		return EC_ERROR_PARAM_COUNT;
+
+	type = strtoi(argv[1], &e, 0);
+
+	if (*e)
+		return EC_ERROR_PARAM1;
+
+	/*
+	 * First time this issued, just display instructions and return. If
+	 * command is repeated, then perform calibration.
+	 */
+	if (type != last_type) {
+		/*
+		 * type 0: calibrate the lid to base alignment rotation matrix.
+		 * type 1: calibrate the hinge 90 rotation matrix.
+		 * type 2: calibrate hinge axis and hinge 180 rotation matrix.
+		 */
+		switch (type) {
+		case 0:
+			ccprintf("To calibrate, close lid, issue this command "
+				"again, and rotate the machine in space until "
+				"all 3 directions are captured.\n");
+			break;
+		case 1:
+			ccprintf("To calibrate, open lid to 90 degrees, issue "
+				" this command again, and rotate in space "
+				"until all 3 directions are captured.\n");
+			break;
+		case 2:
+			ccprintf("To calibrate, align hinge with gravity, and "
+				"issue this command again.\n");
+			break;
+		default:
+			return EC_ERROR_PARAM1;
+		}
+
+		last_type = type;
+		return EC_SUCCESS;
+	}
+
+	/* Call appropriate calibration function. */
+	if (type == 0 || type == 1)
+		ret = calibrate_orientation(type);
+	else
+		ret = calibrate_hinge();
+
+	/* Print results of all calibration. */
+	if (ret == EC_SUCCESS)
+		command_print_orientation(0, NULL);
+
+	return EC_SUCCESS;
+}
+DECLARE_CONSOLE_COMMAND(accelcalib, command_auto_calibrate,
+	"0 - Calibrate lid to base alignment rotation matrix\n1 - Calibrate "
+	"hinge positive 90 rotation matrix\n2 - Calibrate hinge axis and hinge "
+	"180 matrix",
+	"Auto calibrate the accelerometers", NULL);
diff --git a/common/motion_sense.c b/common/motion_sense.c
new file mode 100644
index 0000000000..beb4ddbf2b
--- /dev/null
+++ b/common/motion_sense.c
@@ -0,0 +1,214 @@
+/* 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 "accelerometer.h"
+#include "common.h"
+#include "console.h"
+#include "hooks.h"
+#include "math_util.h"
+#include "motion_sense.h"
+#include "timer.h"
+#include "task.h"
+#include "util.h"
+
+/* Console output macros */
+#define CPUTS(outstr) cputs(CC_MOTION_SENSE, outstr)
+#define CPRINTF(format, args...) cprintf(CC_MOTION_SENSE, format, ## args)
+
+/* Minimum time in between running motion sense task loop. */
+#define MIN_MOTION_SENSE_WAIT_TIME (1 * MSEC)
+
+/* Current acceleration vectors and current lid angle. */
+static vector_3_t acc_lid_raw, acc_lid, acc_base;
+static float lid_angle_deg;
+
+/* Sampling interval for measuring acceleration and calculating lid angle. */
+static int accel_interval_ms = 250;
+
+#ifdef CONFIG_CMD_LID_ANGLE
+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.
+ */
+static float calculate_lid_angle(vector_3_t base, vector_3_t lid)
+{
+	vector_3_t v;
+	float ang_lid_to_base, ang_lid_90, ang_lid_270;
+	float lid_to_base, base_to_hinge;
+
+	/*
+	 * 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);
+	base_to_hinge = SQ(base_to_hinge);
+
+	/* Check divide by 0. */
+	if (ABS(1.0F - base_to_hinge) < 0.01F)
+		return 0.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;
+
+	return ang_lid_to_base;
+}
+
+int motion_get_lid_angle(void)
+{
+	return (int)lid_angle_deg;
+}
+
+#ifdef CONFIG_ACCEL_CALIBRATE
+void motion_get_accel_lid(vector_3_t *v, int adjusted)
+{
+	memcpy(v, adjusted ? &acc_lid : &acc_lid_raw, sizeof(vector_3_t));
+}
+
+void motion_get_accel_base(vector_3_t *v)
+{
+	memcpy(v, &acc_base, sizeof(vector_3_t));
+}
+#endif
+
+
+void motion_sense_task(void)
+{
+	timestamp_t ts0, ts1;
+	int wait_us;
+	int ret;
+
+	/* Initialize accelerometers. */
+	ret = accel_init(ACCEL_LID);
+	ret |= accel_init(ACCEL_BASE);
+
+	/* If accelerometers do not initialize, then end task. */
+	if (ret != EC_SUCCESS) {
+		CPRINTF("[%T, Accelerometers failed to initialize. Stopping "
+				"motion sense task.\n");
+		return;
+	}
+
+	while (1) {
+		ts0 = get_time();
+
+		/* Read all accelerations. */
+		accel_read(ACCEL_LID, &acc_lid_raw[0], &acc_lid_raw[1],
+			   &acc_lid_raw[2]);
+		accel_read(ACCEL_BASE, &acc_base[0], &acc_base[1],
+			   &acc_base[2]);
+
+		/*
+		 * Rotate the lid vector so the reference frame aligns with
+		 * the base sensor.
+		 */
+		rotate(acc_lid_raw, &p_acc_orient->rot_align, &acc_lid);
+
+		/* Calculate angle of lid. */
+		lid_angle_deg = calculate_lid_angle(acc_base, acc_lid);
+
+		/* TODO(crosbug.com/p/25597): Add filter to smooth lid angle. */
+
+		/*
+		 * TODO(crosbug.com/p/25599): Add acceleration data to LPC
+		 * shared memory.
+		 */
+
+#ifdef CONFIG_CMD_LID_ANGLE
+		if (accel_disp) {
+			CPRINTF("[%T ACC base=%-5d, %-5d, %-5d  lid=%-5d, "
+					"%-5d, %-5d  a=%-6.1d]\n",
+					acc_base[0], acc_base[1], acc_base[2],
+					acc_lid[0], acc_lid[1], acc_lid[2],
+					(int)(10*lid_angle_deg));
+		}
+#endif
+
+		/* Delay appropriately to keep sampling time consistent. */
+		ts1 = get_time();
+		wait_us = accel_interval_ms * MSEC - (ts1.val-ts0.val);
+
+		/*
+		 * Guarantee some minimum delay to allow other lower priority
+		 * tasks to run.
+		 */
+		if (wait_us < MIN_MOTION_SENSE_WAIT_TIME)
+			wait_us = MIN_MOTION_SENSE_WAIT_TIME;
+
+		task_wait_event(wait_us);
+	}
+}
+
+/*****************************************************************************/
+/* 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. */
+	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 */