diff options
| -rw-r--r-- | board/host/board.c | 5 | ||||
| -rw-r--r-- | board/host/board.h | 1 | ||||
| -rw-r--r-- | common/motion_calibrate.c | 218 | ||||
| -rw-r--r-- | common/motion_sense.c | 23 | ||||
| -rw-r--r-- | include/motion_sense.h | 6 |
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; }; |