rambi: Rotate accelerometer data into standard reference frame

Added rotation of accelerometer data into a standard reference frame
so that the host does not have to know about the orientation of the
sensors.

Also added a calibration routine to calibrate the rotation matrix to
get to the standard reference frame. Cleanup up calibration in the
process to make it more user friendly.

Changed the default accelerometer sampling rate to 100Hz.

BUG=chrome-os-partner:25599
BRANCH=rambi
TEST=Tested the full calibration routine on a glimmer at my desk.
Used 'taskinfo' and verified that the higher sampling rate does not
bog down the EC. I found that the motion sense task is running for
about 200ms every 10 seconds, so about 2% CPU load.

Change-Id: I9ca1a4252f62a54016009c7d5e43b4cb1adf7e1d
Original-Change-Id: Id554511f7cc9549dfc9ed2d6337216bfa639359d
Signed-off-by: Alec Berg <alecaberg@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/187172
Reviewed-by: Randall Spangler <rspangler@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/188385

5 files changed, 201 insertions, 52 deletions

diff --git a/board/host/board.c b/board/host/board.c
index daf78d8cb9..1d9ab77c23 100644
--- a/board/host/board.c
+++ b/board/host/board.c
@@ -84,6 +84,11 @@ 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 55d66afb7b..130bdcd385 100644
--- a/board/host/board.h
+++ b/board/host/board.h
@@ -9,6 +9,7 @@
 #define __BOARD_H
 
 /* Optional features */
+#undef CONFIG_ACCEL_CALIBRATE
 #define CONFIG_EXTPOWER_GPIO
 #undef CONFIG_FMAP
 #define CONFIG_POWER_BUTTON
diff --git a/common/motion_calibrate.c b/common/motion_calibrate.c
index 3a1033d534..af237591dd 100644
--- a/common/motion_calibrate.c
+++ b/common/motion_calibrate.c
@@ -12,6 +12,7 @@
 #include "motion_sense.h"
 #include "timer.h"
 #include "task.h"
+#include "uart.h"
 #include "util.h"
 
 /*
@@ -21,6 +22,17 @@
 #define AUTO_CAL_DIR_THRESHOLD (ACCEL_G * 3 / 4)
 #define AUTO_CAL_MAG_THRESHOLD (ACCEL_G / 20)
 
+/*
+ * Solution to standard reference frame calibration equation. Note, this matrix
+ * depends on the exact instructions regarding the orientation given to the user
+ * for calibrating the standard reference frame.
+ */
+static matrix_3x3_t standard_ref_calib = {
+	{ 1024,  0,  0},
+	{ 0, -1024,  0},
+	{ 0,  0, -1024}
+};
+
 /*****************************************************************************/
 /* Console commands */
 
@@ -52,6 +64,13 @@ static int command_print_orientation(int argc, char **argv)
 	(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_standard_ref;
+	ccprintf("Standard ref frame 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]);
@@ -119,7 +138,7 @@ static int calibrate_orientation(int type)
 		}
 
 		/* Wait until next reading. */
-		task_wait_event(50*MSEC);
+		task_wait_event(50 * MSEC);
 	}
 
 	/* Solve for the rotation matrix and display final rotation matrix. */
@@ -185,67 +204,178 @@ static int calibrate_hinge(void)
 	return EC_SUCCESS;
 }
 
+/**
+ * Calibrate the standard reference frame.
+ */
+static int calibrate_standard_frame(vector_3_t *v_x, vector_3_t *v_y,
+		vector_3_t *v_z)
+{
+	static matrix_3x3_t m;
+	int j;
+
+	for (j = 0; j < 3; j++) {
+		m[0][j] = (*v_x)[j];
+		m[1][j] = (*v_y)[j];
+		m[2][j] = (*v_z)[j];
+	}
+
+	return solve_rotation_matrix(&m, &standard_ref_calib,
+						&acc_orient.rot_standard_ref);
+}
+
+/**
+ * Wait until a specific set of keys is pressed: enter, 'q', or 's'. Return
+ * key that was pressed.
+ */
+static int wait_for_key(void)
+{
+	int c = uart_getc();
+
+	/* Loop until previous character was a new line char, 'q', or 's'. */
+	while (c != '\r' && c != '\n' && c != 'q' && c != 's') {
+		task_wait_event(50 * MSEC);
+		c = uart_getc();
+	}
+
+	return c;
+}
+
 static int command_auto_calibrate(int argc, char **argv)
 {
-	char *e;
-	int type, ret;
-	static int last_type = -1;
+	int c;
+	vector_3_t v_x, v_y, v_z;
 
-	if (argc != 2)
+	if (argc > 1)
 		return EC_ERROR_PARAM_COUNT;
 
-	type = strtoi(argv[1], &e, 0);
+	ccprintf("Calibrating... press 'q' at any time to quit, and 's' "
+			"to skip step.\n");
 
-	if (*e)
-		return EC_ERROR_PARAM1;
+	/*
+	 * Part 1: Calibrate the lid to base alignment rotation matrix.
+	 */
+	ccprintf("\nStep 1: close lid, press enter, and rotate the machine\n"
+		"in space until all 3 directions are captured.\n");
+
+	/* Wait for user to press enter, quit, or skip. */
+	c = wait_for_key();
+	if (c == 'q') {
+		ccprintf("Calibration exited.\n");
+		return EC_SUCCESS;
+	}
+
+	/* If step is not skipped, perform calibration. */
+	if (c != 's') {
+		if (calibrate_orientation(0) != EC_SUCCESS) {
+			ccprintf("Calibration error.\n");
+			return EC_SUCCESS;
+		}
+	}
 
 	/*
-	 * First time this issued, just display instructions and return. If
-	 * command is repeated, then perform calibration.
+	 * Part 2: Calibrate the hinge 90 rotation matrix.
 	 */
-	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;
+	ccprintf("\nStep 2: open lid to 90 degrees, press enter, and rotate\n"
+		"in space until all 3 directions are captured.\n");
+
+
+	/* Wait for user to press enter, quit, or skip. */
+	c = wait_for_key();
+	if (c == 'q') {
+		ccprintf("Calibration exited.\n");
+		return EC_SUCCESS;
+	}
+
+	/* If step is not skipped, perform calibration. */
+	if (c != 's') {
+		if (calibrate_orientation(1) != EC_SUCCESS) {
+			ccprintf("Calibration error.\n");
+			return EC_SUCCESS;
 		}
+	}
 
-		last_type = type;
+	/*
+	 * Part 3: Calibrate the hinge axis and hinge 180 rotation matrix.
+	 */
+	ccprintf("\nStep 3: align hinge with gravity, and press enter.\n");
+
+	/* Wait for user to press enter, quit, or skip. */
+	c = wait_for_key();
+	if (c == 'q') {
+		ccprintf("Calibration exited.\n");
 		return EC_SUCCESS;
 	}
 
-	/* Call appropriate calibration function. */
-	if (type == 0 || type == 1)
-		ret = calibrate_orientation(type);
-	else
-		ret = calibrate_hinge();
+	/* If step is not skipped, perform calibration. */
+	if (c != 's') {
+		if (calibrate_hinge() != EC_SUCCESS) {
+			ccprintf("Calibration error.\n");
+			return EC_SUCCESS;
+		}
+	}
+
+	/*
+	 * Part 4: Calibrate the standard reference frame rotation matrix.
+	 */
+	ccprintf("\nStep 4a: place machine on right side, with hinge\n"
+		"aligned with gravity, and press enter.\n");
+
+	/* Wait for user to press enter, quit, or skip. */
+	c = wait_for_key();
+	if (c == 'q') {
+		ccprintf("Calibration exited.\n");
+		return EC_SUCCESS;
+	}
+
+	if (c == 's')
+		goto auto_calib_done;
 
+	/* In this orientation, the Y axis should be highest. Capture data. */
+	motion_get_accel_base(&v_y);
+
+	ccprintf("\nStep 4b: place machine flat on table, with keyboard\n"
+		"up, and press enter.\n");
+
+	/* Wait for user to press enter, quit, or skip. */
+	c = wait_for_key();
+	if (c == 'q') {
+		ccprintf("Calibration exited.\n");
+		return EC_SUCCESS;
+	}
+
+	if (c == 's')
+		goto auto_calib_done;
+
+	/* In this orientation, the Z axis should be highest. Capture data. */
+	motion_get_accel_base(&v_z);
+
+	ccprintf("\nStep 4c: hold machine perpendicular to table with\n"
+		"the hinge up, and press enter.\n");
+
+	/* Wait for user to press enter, quit, or skip. */
+	c = wait_for_key();
+	if (c == 'q') {
+		ccprintf("Calibration exited.\n");
+		return EC_SUCCESS;
+	}
+
+	if (c == 's')
+		goto auto_calib_done;
+
+	/* In this orientation, the X axis should be highest. Capture data. */
+	motion_get_accel_base(&v_x);
+
+	if (calibrate_standard_frame(&v_x, &v_y, &v_z) != EC_SUCCESS) {
+		ccprintf("Calibration error.\n");
+		return EC_SUCCESS;
+	}
+
+auto_calib_done:
 	/* Print results of all calibration. */
-	if (ret == EC_SUCCESS)
-		command_print_orientation(0, NULL);
+	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
index b0e2645c19..4bf2e7293f 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -25,10 +25,11 @@
 
 /* Current acceleration vectors and current lid angle. */
 static vector_3_t acc_lid_raw, acc_lid, acc_base;
+static vector_3_t acc_lid_host, acc_base_host;
 static float lid_angle_deg;
 
 /* Sampling interval for measuring acceleration and calculating lid angle. */
-static int accel_interval_ms = 250;
+static int accel_interval_ms = 10;
 
 #ifdef CONFIG_CMD_LID_ANGLE
 static int accel_disp;
@@ -119,7 +120,7 @@ void motion_get_accel_base(vector_3_t *v)
 
 void motion_sense_task(void)
 {
-	timestamp_t ts0, ts1;
+	static timestamp_t ts0, ts1;
 	int wait_us;
 	int ret;
 	uint8_t *lpc_status;
@@ -160,6 +161,12 @@ void motion_sense_task(void)
 
 		/* TODO(crosbug.com/p/25597): Add filter to smooth lid angle. */
 
+		/* Rotate accels into standard reference frame for the host. */
+		rotate(acc_base, &p_acc_orient->rot_standard_ref,
+				&acc_base_host);
+		rotate(acc_lid, &p_acc_orient->rot_standard_ref,
+				&acc_lid_host);
+
 		/*
 		 * Set the busy bit before writing the sensor data. Increment
 		 * the counter and clear the busy bit after writing the sensor
@@ -174,12 +181,12 @@ void motion_sense_task(void)
 		 * assumes little endian, which is what the host expects.
 		 */
 		lpc_data[0] = (int)lid_angle_deg;
-		lpc_data[1] = acc_base[X];
-		lpc_data[2] = acc_base[Y];
-		lpc_data[3] = acc_base[Z];
-		lpc_data[4] = acc_lid[X];
-		lpc_data[5] = acc_lid[Y];
-		lpc_data[6] = acc_lid[Z];
+		lpc_data[1] = acc_base_host[X];
+		lpc_data[2] = acc_base_host[Y];
+		lpc_data[3] = acc_base_host[Z];
+		lpc_data[4] = acc_lid_host[X];
+		lpc_data[5] = acc_lid_host[Y];
+		lpc_data[6] = acc_lid_host[Z];
 
 		/*
 		 * Increment sample id and clear busy bit to signal we finished
diff --git a/include/motion_sense.h b/include/motion_sense.h
index a6aa79b5bf..dda02f19f9 100644
--- a/include/motion_sense.h
+++ b/include/motion_sense.h
@@ -30,6 +30,12 @@ struct accel_orientation {
 	 */
 	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;
 };