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>

1 files changed, 214 insertions, 0 deletions

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 */