diff options
| -rw-r--r-- | common/build.mk | 3 | ||||
| -rw-r--r-- | common/console_output.c | 1 | ||||
| -rw-r--r-- | common/math_util.c | 192 | ||||
| -rw-r--r-- | common/motion_calibrate.c | 251 | ||||
| -rw-r--r-- | common/motion_sense.c | 214 | ||||
| -rw-r--r-- | include/accelerometer.h | 7 | ||||
| -rw-r--r-- | include/config.h | 8 | ||||
| -rw-r--r-- | include/console.h | 1 | ||||
| -rw-r--r-- | include/math_util.h | 93 | ||||
| -rw-r--r-- | include/motion_sense.h | 71 |
10 files changed, 841 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 */ 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 114e6e982f..a2f2808009 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 @@ -245,6 +248,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 @@ -880,6 +884,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 */ |