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=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.

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>

5 files changed, 180 insertions, 0 deletions

diff --git a/include/accelerometer.h b/include/accelerometer.h
index 48d61ac3ab..a69c227401 100644
--- a/include/accelerometer.h
+++ b/include/accelerometer.h
@@ -3,6 +3,11 @@
  * found in the LICENSE file.
  */
 
+#ifndef __CROS_EC_ACCELEROMETER_H
+#define __CROS_EC_ACCELEROMETER_H
+
+/* Header file for accelerometer drivers. */
+
 /* This array must be defined in board.c. */
 extern const int accel_addr[];
 
@@ -34,3 +39,5 @@ int accel_read(enum accel_id id, int *x_acc, int *y_acc, int *z_acc);
  * @return EC_SUCCESS if successful, non-zero if error.
  */
 int accel_init(enum accel_id id);
+
+#endif /* __CROS_EC_ACCELEROMETER_H */
diff --git a/include/config.h b/include/config.h
index 86aafdff2b..187bbd5289 100644
--- a/include/config.h
+++ b/include/config.h
@@ -36,6 +36,9 @@
  * BOARD_*, CHIP_*, and CHIP_FAMILY_*.
  */
 
+/* Use to enable EC console functions for calibrating accelerometers. */
+#undef CONFIG_ACCEL_CALIBRATE
+
 /* Specify type of accelerometers attached. */
 #undef CONFIG_ACCEL_KXCJ9
 
@@ -243,6 +246,7 @@
 #undef CONFIG_CMD_GSV
 #undef CONFIG_CMD_ILIM
 #undef CONFIG_CMD_JUMPTAGS
+#define CONFIG_CMD_LID_ANGLE
 #undef CONFIG_CMD_PLL
 #undef CONFIG_CMD_PMU
 #undef CONFIG_CMD_POWERLED
@@ -875,6 +879,10 @@
 #undef CONFIG_KEYBOARD_PROTOCOL_MKBP
 #endif
 
+#ifndef HAS_TASK_MOTIONSENSE
+#undef CONFIG_ACCEL_CALIBRATE
+#endif
+
 /*****************************************************************************/
 /*
  * Apply test config overrides last, since tests need to override some of the
diff --git a/include/console.h b/include/console.h
index 1dd37e42c9..41bc9decbd 100644
--- a/include/console.h
+++ b/include/console.h
@@ -40,6 +40,7 @@ enum console_channel {
 	CC_KEYSCAN,
 	CC_LIGHTBAR,
 	CC_LPC,
+	CC_MOTION_SENSE,
 	CC_PORT80,
 	CC_PWM,
 	CC_SPI,
diff --git a/include/math_util.h b/include/math_util.h
new file mode 100644
index 0000000000..ffdbec424b
--- /dev/null
+++ b/include/math_util.h
@@ -0,0 +1,93 @@
+/* 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 file for common math functions. */
+
+#ifndef __CROS_MATH_UTIL_H
+#define __CROS_MATH_UTIL_H
+
+typedef float matrix_3x3_t[3][3];
+typedef int vector_3_t[3];
+
+
+/* Some useful math functions. */
+#define SQ(x) ((x) * (x))
+#define ABS(x) ((x) >= 0 ? (x) : -(x))
+
+
+/**
+ * Find acos(x) in degrees. Argument is clipped to [-1.0, 1.0].
+ *
+ * @param x
+ *
+ * @return acos(x) in degrees.
+ */
+float arc_cos(float x);
+
+/**
+ * Find the cosine of the angle between two vectors.
+ *
+ * @param v1
+ * @param v2
+ *
+ * @return Cosine of the angle between v1 and v2.
+ */
+float cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2);
+
+/**
+ * Rotate vector v by rotation matrix R.
+ *
+ * @param v Vector to be rotated.
+ * @param R Pointer to rotation matrix.
+ * @param res Pointer to the resultant vector.
+ */
+void rotate(const vector_3_t v, const matrix_3x3_t (* const R),
+		vector_3_t *res);
+
+
+#ifdef CONFIG_ACCEL_CALIBRATE
+
+/**
+ * Multiply two 3x3 matrices.
+ *
+ * @param m1
+ * @param m2
+ * @param res Pointer to resultant matrix R = a1*a2;
+ */
+void matrix_multiply(matrix_3x3_t *m1, matrix_3x3_t *m2, matrix_3x3_t *res);
+
+/**
+ * Given an input matrix and an output matrix, solve for the rotation
+ * matrix to get from the input matrix to the output matrix. Note, that this
+ * operation is not guaranteed. In order to successfully calculate the rotation
+ * matrix, the input must be linearly independent so that the matrix can be
+ * inverted.
+ *
+ * This function solves the following matrix equation for R:
+ * in * R = out
+ *
+ * If input matrix is invertible the resulting rotation matrix is stored in R.
+ *
+ * @param in
+ * @param out
+ * @param R Pointer to resultant matrix.
+ *
+ * @return EC_SUCCESS if successful
+ */
+int solve_rotation_matrix(matrix_3x3_t *in, matrix_3x3_t *out, matrix_3x3_t *R);
+
+/**
+ * Calculate magnitude of a vector.
+ *
+ * @param v Vector to be measured.
+ *
+ * @return Magnitued of vector v.
+ */
+int vector_magnitude(const vector_3_t v);
+
+#endif
+
+
+#endif /* __CROS_MATH_UTIL_H */
diff --git a/include/motion_sense.h b/include/motion_sense.h
new file mode 100644
index 0000000000..a6aa79b5bf
--- /dev/null
+++ b/include/motion_sense.h
@@ -0,0 +1,71 @@
+/* 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_SENSE_H
+#define __CROS_EC_MOTION_SENSE_H
+
+#include "math_util.h"
+
+/**
+ * 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;
+
+	/* Vector pointing along hinge axis. */
+	vector_3_t hinge_axis;
+};
+
+/* Link global structure for orientation. This must be defined in board.c. */
+extern
+#ifndef CONFIG_ACCEL_CALIBRATE
+const
+#endif
+struct accel_orientation acc_orient;
+
+
+/**
+ * Get last calculated lid angle.
+ *
+ * @return lid angle in degrees in range [-180, 180].
+ */
+int motion_get_lid_angle(void);
+
+
+#ifdef CONFIG_ACCEL_CALIBRATE
+/**
+ * Get the last measured lid acceleration vector.
+ *
+ * @param v Pointer to location to store vector.
+ * @param adjusted If false use the raw vector, if true use the adjusted vector.
+ */
+void motion_get_accel_lid(vector_3_t *v, int adjusted);
+
+/**
+ * Get the last measured base acceleration vector.
+ *
+ * @param v Pointer to location to store vector.
+ */
+void motion_get_accel_base(vector_3_t *v);
+#endif
+
+
+#endif /* __CROS_EC_MOTION_SENSE_H */