diff options
| -rw-r--r-- | board/host/board.c | 14 | ||||
| -rw-r--r-- | board/host/board.h | 9 | ||||
| -rw-r--r-- | board/samus/board.c | 32 | ||||
| -rw-r--r-- | board/samus/board.h | 5 | ||||
| -rw-r--r-- | common/build.mk | 1 | ||||
| -rw-r--r-- | common/console_output.c | 1 | ||||
| -rw-r--r-- | common/motion_lid.c | 192 | ||||
| -rw-r--r-- | common/motion_sense.c | 343 | ||||
| -rw-r--r-- | driver/accelgyro_lsm6ds0.c | 6 | ||||
| -rw-r--r-- | include/config.h | 20 | ||||
| -rw-r--r-- | include/console.h | 1 | ||||
| -rw-r--r-- | include/motion_lid.h | 50 | ||||
| -rw-r--r-- | include/motion_sense.h | 79 | ||||
| -rw-r--r-- | test/build.mk | 4 | ||||
| -rw-r--r-- | test/motion_lid.c (renamed from test/motion_sense.c) | 27 | ||||
| -rw-r--r-- | test/motion_lid.tasklist (renamed from test/motion_sense.tasklist) | 0 | ||||
| -rw-r--r-- | test/test_config.h | 6 |
17 files changed, 437 insertions, 353 deletions
diff --git a/board/host/board.c b/board/host/board.c index c43a36ae8b..7ed0e37f28 100644 --- a/board/host/board.c +++ b/board/host/board.c @@ -7,9 +7,11 @@ #include "button.h" #include "extpower.h" #include "gpio.h" +#include "host_command.h" #include "inductive_charging.h" #include "lid_switch.h" #include "motion_sense.h" +#include "motion_lid.h" #include "power_button.h" #include "temp_sensor.h" #include "timer.h" @@ -54,13 +56,6 @@ BUILD_ASSERT(ARRAY_SIZE(buttons) == CONFIG_BUTTON_COUNT); /* Define the accelerometer orientation matrices. */ const struct accel_orientation acc_orient = { - /* Lid and base sensor are already aligned. */ - .rot_align = { - { 1, 0, 0}, - { 0, 1, 0}, - { 0, 0, 1} - }, - /* Hinge aligns with y axis. */ .rot_hinge_90 = { { 0, 0, 1}, @@ -72,11 +67,6 @@ 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 3c11f58b8f..46d1a001b2 100644 --- a/board/host/board.h +++ b/board/host/board.h @@ -39,13 +39,4 @@ enum adc_channel { ADC_CH_COUNT }; -/* Identifiers for each accelerometer used. */ -enum accel_id { - ACCEL_BASE, - ACCEL_LID, - - /* Number of accelerometers. */ - ACCEL_COUNT -}; - #endif /* __BOARD_H */ diff --git a/board/samus/board.c b/board/samus/board.c index 08dae68780..e6682d4717 100644 --- a/board/samus/board.c +++ b/board/samus/board.c @@ -28,6 +28,7 @@ #include "lid_switch.h" #include "lightbar.h" #include "motion_sense.h" +#include "motion_lid.h" #include "peci.h" #include "power.h" #include "power_button.h" @@ -264,6 +265,19 @@ static struct mutex g_lid_mutex; struct kxcj9_data g_kxcj9_data; /* Four Motion sensors */ +/* Matrix to rotate accelrator into standard reference frame */ +const matrix_3x3_t base_standard_ref = { + {-1, 0, 0}, + { 0, -1, 0}, + { 0, 0, -1} +}; + +const matrix_3x3_t lid_standard_ref = { + { 0, 1, 0}, + {-1, 0, 0}, + { 0, 0, -1} +}; + struct motion_sensor_t motion_sensors[] = { /* @@ -274,30 +288,23 @@ struct motion_sensor_t motion_sensors[] = { {SENSOR_ACTIVE_S0_S3_S5, "Base", SENSOR_CHIP_LSM6DS0, SENSOR_ACCELEROMETER, LOCATION_BASE, &lsm6ds0_drv, &g_base_mutex, NULL, - LSM6DS0_ADDR1, 119000, 2}, + LSM6DS0_ADDR1, &base_standard_ref, 119000, 2}, {SENSOR_ACTIVE_S0, "Lid", SENSOR_CHIP_KXCJ9, SENSOR_ACCELEROMETER, LOCATION_LID, &kxcj9_drv, &g_lid_mutex, &g_kxcj9_data, - KXCJ9_ADDR0, 100000, 2}, + KXCJ9_ADDR0, &lid_standard_ref, 100000, 2}, {SENSOR_ACTIVE_S0, "Base Gyro", SENSOR_CHIP_LSM6DS0, SENSOR_GYRO, LOCATION_BASE, &lsm6ds0_drv, &g_base_mutex, NULL, - LSM6DS0_ADDR1, 119000, 2000}, + LSM6DS0_ADDR1, NULL, 119000, 2000}, }; const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors); /* Define the accelerometer orientation matrices. */ const struct accel_orientation acc_orient = { - /* Lid and base sensor are already aligned. */ - .rot_align = { - { 0, -1, 0}, - { 1, 0, 0}, - { 0, 0, 1} - }, - /* Hinge aligns with y axis. */ .rot_hinge_90 = { { 1, 0, 0}, @@ -309,10 +316,5 @@ 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/samus/board.h b/board/samus/board.h index a203b94a9b..757f0ca950 100644 --- a/board/samus/board.h +++ b/board/samus/board.h @@ -21,11 +21,13 @@ #define CONFIG_ALS_ISL29035 #define CONFIG_BOARD_VERSION #define CONFIG_CMD_ACCELS +#define CONFIG_CMD_ACCEL_INFO #define CONFIG_POWER_COMMON #define CONFIG_CHIPSET_CAN_THROTTLE #define CONFIG_KEYBOARD_BOARD_CONFIG #define CONFIG_KEYBOARD_PROTOCOL_8042 #define CONFIG_KEYBOARD_COL2_INVERTED +#define CONFIG_LID_ANGLE #define CONFIG_LIGHTBAR_POWER_RAILS #define CONFIG_LOW_POWER_IDLE #define CONFIG_POWER_BUTTON @@ -189,6 +191,9 @@ int board_discharge_on_ac(int enable); #define CONFIG_GESTURE_TAP_MIN_INTERSTICE_T 120 #define CONFIG_GESTURE_TAP_MAX_INTERSTICE_T 500 +#define CONFIG_SENSOR_BASE 0 +#define CONFIG_SENSOR_LID 1 + #endif /* !__ASSEMBLER__ */ #endif /* __BOARD_H */ diff --git a/common/build.mk b/common/build.mk index 9fc2a9ebf0..acc650522a 100644 --- a/common/build.mk +++ b/common/build.mk @@ -85,4 +85,5 @@ common-$(HAS_TASK_KEYSCAN)+=keyboard_scan.o common-$(HAS_TASK_LIGHTBAR)+=lb_common.o lightbar.o common-$(HAS_TASK_MOTIONSENSE)+=motion_sense.o math_util.o common-$(CONFIG_GESTURE_DETECTION)+=gesture.o +common-$(CONFIG_LID_ANGLE)+=motion_lid.o common-$(TEST_BUILD)+=test_util.o diff --git a/common/console_output.c b/common/console_output.c index e2aa33d08b..492375e71e 100644 --- a/common/console_output.c +++ b/common/console_output.c @@ -48,6 +48,7 @@ static const char * const channel_names[] = { "lidangle", "lightbar", "lpc", + "motionlid", "motionsense", "pdhostcmd", "port80", diff --git a/common/motion_lid.c b/common/motion_lid.c new file mode 100644 index 0000000000..4b991dee6b --- /dev/null +++ b/common/motion_lid.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. + */ + +/* Motion sense module to read from various motion sensors. */ + +#include "accelgyro.h" +#include "chipset.h" +#include "common.h" +#include "console.h" +#include "gesture.h" +#include "hooks.h" +#include "host_command.h" +#include "lid_angle.h" +#include "math_util.h" +#include "motion_lid.h" +#include "motion_sense.h" +#include "power.h" +#include "timer.h" +#include "task.h" +#include "util.h" + +/* Console output macros */ +#define CPUTS(outstr) cputs(CC_MOTION_LID, outstr) +#define CPRINTS(format, args...) cprints(CC_MOTION_LID, format, ## args) +#define CPRINTF(format, args...) cprintf(CC_MOTION_LID, format, ## args) + +/* For vector_3_t, define which coordinates are in which location. */ +enum { + X, Y, Z +}; + +/* Current acceleration vectors and current lid angle. */ +static float lid_angle_deg; +static int lid_angle_is_reliable; + +/* + * Angle threshold for how close the hinge aligns with gravity before + * considering the lid angle calculation unreliable. For computational + * efficiency, value is given unit-less, so if you want the threshold to be + * at 15 degrees, the value would be cos(15 deg) = 0.96593. + */ +#define HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD 0.96593F + + +/* Pointer to constant acceleration orientation data. */ +const struct accel_orientation * const p_acc_orient = &acc_orient; + +struct motion_sensor_t *accel_base = &motion_sensors[CONFIG_SENSOR_BASE]; +struct motion_sensor_t *accel_lid = &motion_sensors[CONFIG_SENSOR_LID]; + +/** + * Calculate the lid angle using two acceleration vectors, one recorded in + * the base and one in the lid. + * + * @param base Base accel vector + * @param lid Lid accel vector + * @param lid_angle Pointer to location to store lid angle result + * + * @return flag representing if resulting lid angle calculation is reliable. + */ +static int calculate_lid_angle(const vector_3_t base, const vector_3_t lid, + float *lid_angle) +{ + vector_3_t v; + float ang_lid_to_base, ang_lid_90, ang_lid_270; + float lid_to_base, base_to_hinge; + int reliable = 1; + + /* + * 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); + + /* + * If hinge aligns too closely with gravity, then result may be + * unreliable. + */ + if (ABS(base_to_hinge) > HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD) + reliable = 0; + + base_to_hinge = SQ(base_to_hinge); + + /* Check divide by 0. */ + if (ABS(1.0F - base_to_hinge) < 0.01F) { + *lid_angle = 0.0; + return 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; + + /* Place lid angle between 0 and 360 degrees. */ + if (ang_lid_to_base < 0) + ang_lid_to_base += 360; + + *lid_angle = ang_lid_to_base; + return reliable; +} + +int motion_lid_get_angle(void) +{ + if (lid_angle_is_reliable) + /* + * Round to nearest int by adding 0.5. Note, only works because + * lid angle is known to be positive. + */ + return (int)(lid_angle_deg + 0.5F); + else + return (int)LID_ANGLE_UNRELIABLE; +} + +/* + * Calculate lid angle and massage the results + */ +void motion_lid_calc(void) +{ + /* Calculate angle of lid accel. */ + lid_angle_is_reliable = calculate_lid_angle( + accel_base->xyz, + accel_lid->xyz, + &lid_angle_deg); + +#ifdef CONFIG_LID_ANGLE_KEY_SCAN + lidangle_keyscan_update(motion_lid_get_angle()); +#endif + +} + +/*****************************************************************************/ +/* Host commands */ + + +int host_cmd_motion_lid(struct host_cmd_handler_args *args) +{ + const struct ec_params_motion_sense *in = args->params; + struct ec_response_motion_sense *out = args->response; + + switch (in->cmd) { + case MOTIONSENSE_CMD_KB_WAKE_ANGLE: +#ifdef CONFIG_LID_ANGLE_KEY_SCAN + /* Set new keyboard wake lid angle if data arg has value. */ + if (in->kb_wake_angle.data != EC_MOTION_SENSE_NO_VALUE) + lid_angle_set_kb_wake_angle(in->kb_wake_angle.data); + + out->kb_wake_angle.ret = lid_angle_get_kb_wake_angle(); +#else + out->kb_wake_angle.ret = 0; +#endif + args->response_size = sizeof(out->kb_wake_angle); + + break; + + default: + return EC_RES_INVALID_PARAM; + } + + return EC_RES_SUCCESS; +} + diff --git a/common/motion_sense.c b/common/motion_sense.c index 5a8b4a6075..9ed0ab92da 100644 --- a/common/motion_sense.c +++ b/common/motion_sense.c @@ -15,6 +15,7 @@ #include "lid_angle.h" #include "math_util.h" #include "motion_sense.h" +#include "motion_lid.h" #include "power.h" #include "timer.h" #include "task.h" @@ -36,10 +37,6 @@ enum { X, Y, Z }; -/* Current acceleration vectors and current lid angle. */ -static float lid_angle_deg; -static int lid_angle_is_reliable; - /* Bounds for setting the sensor polling interval. */ #define MIN_POLLING_INTERVAL_MS 5 #define MAX_POLLING_INTERVAL_MS 1000 @@ -53,116 +50,24 @@ static int lid_angle_is_reliable; /* Accelerometer polling intervals based on chipset state. */ static int accel_interval_ap_on_ms = 10; - /* - * Angle threshold for how close the hinge aligns with gravity before - * considering the lid angle calculation unreliable. For computational - * efficiency, value is given unit-less, so if you want the threshold to be - * at 15 degrees, the value would be cos(15 deg) = 0.96593. + * Sampling interval for measuring acceleration and calculating lid angle. + * Set to accel_interval_ap_on_ms when ap is on. */ -#define HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD 0.96593F - -/* Sampling interval for measuring acceleration and calculating lid angle. */ static int accel_interval_ms; -#ifdef CONFIG_CMD_LID_ANGLE +#ifdef CONFIG_CMD_ACCEL_INFO 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. - * - * @param base Base accel vector - * @param lid Lid accel vector - * @param lid_angle Pointer to location to store lid angle result - * - * @return flag representing if resulting lid angle calculation is reliable. +/* + * Angle threshold for how close the hinge aligns with gravity before + * considering the lid angle calculation unreliable. For computational + * efficiency, value is given unit-less, so if you want the threshold to be + * at 15 degrees, the value would be cos(15 deg) = 0.96593. */ -static int calculate_lid_angle(const vector_3_t base, const vector_3_t lid, - float *lid_angle) -{ - vector_3_t v; - float ang_lid_to_base, ang_lid_90, ang_lid_270; - float lid_to_base, base_to_hinge; - int reliable = 1; - - /* - * 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); - - /* - * If hinge aligns too closely with gravity, then result may be - * unreliable. - */ - if (ABS(base_to_hinge) > HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD) - reliable = 0; - - base_to_hinge = SQ(base_to_hinge); - - /* Check divide by 0. */ - if (ABS(1.0F - base_to_hinge) < 0.01F) { - *lid_angle = 0.0; - return 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; - - /* Place lid angle between 0 and 360 degrees. */ - if (ang_lid_to_base < 0) - ang_lid_to_base += 360; - - *lid_angle = ang_lid_to_base; - return reliable; -} +#define HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD 0.96593F -int motion_get_lid_angle(void) -{ - if (lid_angle_is_reliable) - /* - * Round to nearest int by adding 0.5. Note, only works because - * lid angle is known to be positive. - */ - return (int)(lid_angle_deg + 0.5F); - else - return (int)LID_ANGLE_UNRELIABLE; -} static void motion_sense_shutdown(void) { @@ -254,7 +159,11 @@ static inline void update_sense_data(uint8_t *lpc_status, * with un-calibrated accels. The AP calculates a separate, * more accurate lid angle. */ - lpc_data[0] = motion_get_lid_angle(); +#ifdef CONFIG_LID_ANGLE + lpc_data[0] = motion_lid_get_angle(); +#else + lpc_data[0] = LID_ANGLE_UNRELIABLE; +#endif for (i = 0; i < motion_sensor_count; i++) { sensor = &motion_sensors[i]; lpc_data[1+3*i] = sensor->xyz[X]; @@ -295,9 +204,9 @@ static int motion_sense_read(struct motion_sensor_t *sensor) /* Read all raw X,Y,Z accelerations. */ ret = sensor->drv->read(sensor, - &sensor->raw_xyz[X], - &sensor->raw_xyz[Y], - &sensor->raw_xyz[Z]); + &sensor->xyz[X], + &sensor->xyz[Y], + &sensor->xyz[Z]); if (ret != EC_SUCCESS) return EC_ERROR_UNKNOWN; @@ -322,8 +231,6 @@ void motion_sense_task(void) int sample_id = 0; int rd_cnt; struct motion_sensor_t *sensor; - struct motion_sensor_t *accel_base = NULL; - struct motion_sensor_t *accel_lid = NULL; lpc_status = host_get_memmap(EC_MEMMAP_ACC_STATUS); lpc_data = (uint16_t *)host_get_memmap(EC_MEMMAP_ACC_DATA); @@ -334,15 +241,6 @@ void motion_sense_task(void) sensor->odr = sensor->default_odr; sensor->range = sensor->default_range; - - if ((LOCATION_BASE == sensor->location) - && (SENSOR_ACCELEROMETER == sensor->type)) - accel_base = sensor; - - if ((LOCATION_LID == sensor->location) - && (SENSOR_ACCELEROMETER == sensor->type)) { - accel_lid = sensor; - } } set_present(lpc_status); @@ -364,69 +262,49 @@ void motion_sense_task(void) if (sensor->state == SENSOR_NOT_INITIALIZED) motion_sense_init(sensor); - if (EC_SUCCESS == motion_sense_read(sensor)) - rd_cnt++; + if (EC_SUCCESS != motion_sense_read(sensor)) + continue; + + rd_cnt++; + /* + * Rotate the accel vector so the reference for + * all sensors are in the same space. + */ + if (*sensor->rot_standard_ref != NULL) { + rotate(sensor->xyz, + *sensor->rot_standard_ref, + sensor->xyz); + } } - - /* - * Rotate the lid accel vector - * so the reference frame aligns with the base sensor. - */ - if ((LOCATION_LID == sensor->location) - && (SENSOR_ACCELEROMETER == sensor->type)) - rotate(accel_lid->raw_xyz, - p_acc_orient->rot_align, - accel_lid->xyz); - else - memcpy(sensor->xyz, sensor->raw_xyz, - sizeof(vector_3_t)); } #ifdef CONFIG_GESTURE_DETECTION /* Run gesture recognition engine */ gesture_calc(); #endif +#ifdef CONFIG_LID_ANGLE + if (rd_cnt == motion_sensor_count) + motion_lid_calc(); - if (rd_cnt != motion_sensor_count) - goto motion_wait; - - /* Calculate angle of lid accel. */ - lid_angle_is_reliable = calculate_lid_angle( - accel_base->xyz, - accel_lid->xyz, - &lid_angle_deg); - - for (i = 0; i < motion_sensor_count; ++i) { - sensor = &motion_sensors[i]; - /* Rotate accels into standard reference frame. */ - if (sensor->type == SENSOR_ACCELEROMETER) - rotate(sensor->xyz, - p_acc_orient->rot_standard_ref, - sensor->xyz); - } - -#ifdef CONFIG_LID_ANGLE_KEY_SCAN - lidangle_keyscan_update(motion_get_lid_angle()); #endif - -#ifdef CONFIG_CMD_LID_ANGLE +#ifdef CONFIG_CMD_ACCEL_INFO if (accel_disp) { CPRINTF("[%T "); for (i = 0; i < motion_sensor_count; ++i) { sensor = &motion_sensors[i]; CPRINTF("%s=%-5d, %-5d, %-5d ", sensor->name, - sensor->raw_xyz[X], - sensor->raw_xyz[Y], - sensor->raw_xyz[Z]); + sensor->xyz[X], + sensor->xyz[Y], + sensor->xyz[Z]); } - CPRINTF("a=%-6.1d r=%d", (int)(10*lid_angle_deg), - lid_angle_is_reliable); +#ifdef CONFIG_LID_ANGLE + CPRINTF("a=%-6.1d", 10 * motion_lid_get_angle()); +#endif CPRINTF("]\n"); } #endif update_sense_data(lpc_status, lpc_data, &sample_id); -motion_wait: /* Delay appropriately to keep sampling time consistent. */ ts1 = get_time(); wait_us = accel_interval_ms * MSEC - (ts1.val-ts0.val); @@ -442,24 +320,6 @@ motion_wait: } } -void accel_int_lid(enum gpio_signal signal) -{ - /* - * Print statement is here for testing with console accelint command. - * Remove print statement when interrupt is used for real. - */ - CPRINTS("Accelerometer wake-up interrupt occurred on lid"); -} - -void accel_int_base(enum gpio_signal signal) -{ - /* - * Print statement is here for testing with console accelint command. - * Remove print statement when interrupt is used for real. - */ - CPRINTS("Accelerometer wake-up interrupt occurred on base"); -} - /*****************************************************************************/ /* Host commands */ @@ -510,7 +370,7 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args) const struct ec_params_motion_sense *in = args->params; struct ec_response_motion_sense *out = args->response; struct motion_sensor_t *sensor; - int i, data; + int i, data, ret = EC_RES_INVALID_PARAM; switch (in->cmd) { case MOTIONSENSE_CMD_DUMP: @@ -637,24 +497,15 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args) out->sensor_range.ret = data; args->response_size = sizeof(out->sensor_range); break; - - case MOTIONSENSE_CMD_KB_WAKE_ANGLE: -#ifdef CONFIG_LID_ANGLE_KEY_SCAN - /* Set new keyboard wake lid angle if data arg has value. */ - if (in->kb_wake_angle.data != EC_MOTION_SENSE_NO_VALUE) - lid_angle_set_kb_wake_angle(in->kb_wake_angle.data); - - out->kb_wake_angle.ret = lid_angle_get_kb_wake_angle(); -#else - out->kb_wake_angle.ret = 0; -#endif - args->response_size = sizeof(out->kb_wake_angle); - - break; - default: - CPRINTS("MS bad cmd 0x%x", in->cmd); - return EC_RES_INVALID_PARAM; + /* Call other users of the motion task */ +#ifdef CONFIG_LID_ANGLE + if (ret == EC_RES_INVALID_PARAM) + ret = host_cmd_motion_lid(args); +#endif + if (ret == EC_RES_INVALID_PARAM) + CPRINTS("MS bad cmd 0x%x", in->cmd); + return ret; } return EC_RES_SUCCESS; @@ -666,43 +517,6 @@ DECLARE_HOST_COMMAND(EC_CMD_MOTION_SENSE_CMD, /*****************************************************************************/ /* 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. Note accel - * sampling interval will be clobbered when chipset suspends or - * resumes. - */ - 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 */ - #ifdef CONFIG_CMD_ACCELS static int command_accelrange(int argc, char **argv) { @@ -905,7 +719,64 @@ DECLARE_CONSOLE_COMMAND(accelinit, command_accel_init, "id", "Init sensor", NULL); +#ifdef CONFIG_CMD_ACCEL_INFO +static int command_display_accel_info(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. Note accel + * sampling interval will be clobbered when chipset suspends or + * resumes. + */ + 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(accelinfo, command_display_accel_info, + "on/off [interval]", + "Print motion sensor info, lid angle calculations" + " and set calculation frequency.", NULL); +#endif /* CONFIG_CMD_ACCEL_INFO */ + #ifdef CONFIG_ACCEL_INTERRUPTS +/* TODO(crosbug.com/p/426659): this code is broken, does not compile. */ +void accel_int_lid(enum gpio_signal signal) +{ + /* + * Print statement is here for testing with console accelint command. + * Remove print statement when interrupt is used for real. + */ + CPRINTS("Accelerometer wake-up interrupt occurred on lid"); +} + +void accel_int_base(enum gpio_signal signal) +{ + /* + * Print statement is here for testing with console accelint command. + * Remove print statement when interrupt is used for real. + */ + CPRINTS("Accelerometer wake-up interrupt occurred on base"); +} + static int command_accelerometer_interrupt(int argc, char **argv) { char *e; diff --git a/driver/accelgyro_lsm6ds0.c b/driver/accelgyro_lsm6ds0.c index 1083cb5c71..d25b91b594 100644 --- a/driver/accelgyro_lsm6ds0.c +++ b/driver/accelgyro_lsm6ds0.c @@ -335,9 +335,9 @@ static int read(const struct motion_sensor_t *s, * to get the latest updated sensor data quickly. */ if (!tmp) { - *x = s->raw_xyz[0]; - *y = s->raw_xyz[1]; - *z = s->raw_xyz[2]; + *x = s->xyz[0]; + *y = s->xyz[1]; + *z = s->xyz[2]; return EC_SUCCESS; } diff --git a/include/config.h b/include/config.h index 309d061541..dd0815e22c 100644 --- a/include/config.h +++ b/include/config.h @@ -308,6 +308,7 @@ */ #undef CONFIG_CMD_ACCELS +#undef CONFIG_CMD_ACCEL_INFO #undef CONFIG_CMD_BATDEBUG #undef CONFIG_CMD_CLOCKGATES #undef CONFIG_CMD_COMXTEST @@ -317,7 +318,6 @@ #undef CONFIG_CMD_HOSTCMD #undef CONFIG_CMD_ILIM #undef CONFIG_CMD_JUMPTAGS -#define CONFIG_CMD_LID_ANGLE #undef CONFIG_CMD_PLL #undef CONFIG_CMD_PMU #define CONFIG_CMD_POWERINDEBUG @@ -564,6 +564,18 @@ #endif +#undef CONFIG_LID_ANGLE +#ifndef CONFIG_LID_ANGLE +#undef CONFIG_SENSOR_BASE +#undef CONFIG_SENSOR_LID + +/* + * Allows using the lid angle measurement to determine if key scanning should + * be enabled or disabled when chipset is suspended. + */ +#undef CONFIG_LID_ANGLE_KEY_SCAN + +#endif /*****************************************************************************/ @@ -729,12 +741,6 @@ #undef CONFIG_LED_DRIVER_LP5562 /* LP5562, on I2C interface */ /* - * Allows using the lid angle measurement to determine if key scanning should - * be enabled or disabled when chipset is suspended. - */ -#undef CONFIG_LID_ANGLE_KEY_SCAN - -/* * Compile lid switch support. * * This is enabled by default because all boards other than reference boards diff --git a/include/console.h b/include/console.h index bf2cd536f1..29599dda4b 100644 --- a/include/console.h +++ b/include/console.h @@ -43,6 +43,7 @@ enum console_channel { CC_LIDANGLE, CC_LIGHTBAR, CC_LPC, + CC_MOTION_LID, CC_MOTION_SENSE, CC_PD_HOST_CMD, CC_PORT80, diff --git a/include/motion_lid.h b/include/motion_lid.h new file mode 100644 index 0000000000..b3348c020e --- /dev/null +++ b/include/motion_lid.h @@ -0,0 +1,50 @@ +/* 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_LID_H +#define __CROS_EC_MOTION_LID_H + +/* Anything outside of lid angle range [-180, 180] should work. */ +#define LID_ANGLE_UNRELIABLE 500.0F + +/** + * 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 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 const struct accel_orientation acc_orient; + + +/** + * Get last calculated lid angle. Note, the lid angle calculated by the EC + * is un-calibrated and is an approximate angle. + * + * @return lid angle in degrees in range [0, 360]. + */ +int motion_lid_get_angle(void); + +int host_cmd_motion_lid(struct host_cmd_handler_args *args); + +void motion_lid_calc(void); + +#endif /* __CROS_EC_MOTION_LID_H */ + + diff --git a/include/motion_sense.h b/include/motion_sense.h index be764a8d00..bb5acc308c 100644 --- a/include/motion_sense.h +++ b/include/motion_sense.h @@ -12,66 +12,6 @@ #include "math_util.h" #include "chipset.h" -/* Anything outside of lid angle range [-180, 180] should work. */ -#define LID_ANGLE_UNRELIABLE 500.0F - -/** - * 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; - - /* - * 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; -}; - -/* Link global structure for orientation. This must be defined in board.c. */ -extern const struct accel_orientation acc_orient; - - -/** - * Get last calculated lid angle. Note, the lid angle calculated by the EC - * is un-calibrated and is an approximate angle. - * - * @return lid angle in degrees in range [0, 360]. - */ -int motion_get_lid_angle(void); - - -/** - * Interrupt function for lid accelerometer. - * - * @param signal GPIO signal that caused interrupt - */ -void accel_int_lid(enum gpio_signal signal); - -/** - * Interrupt function for base accelerometer. - * - * @param signal GPIO signal that caused interrupt - */ -void accel_int_base(enum gpio_signal signal); - enum sensor_location_t { LOCATION_BASE = 0, LOCATION_LID = 1, @@ -110,6 +50,7 @@ struct motion_sensor_t { struct mutex *mutex; void *drv_data; uint8_t i2c_addr; + const matrix_3x3_t *rot_standard_ref; /* Default configuration parameters, RO only */ int default_odr; @@ -122,9 +63,7 @@ struct motion_sensor_t { /* state parameters */ enum sensor_state state; enum chipset_state_mask active; - vector_3_t raw_xyz; vector_3_t xyz; - }; /* Defined at board level. */ @@ -137,4 +76,20 @@ extern const unsigned int motion_sensor_count; */ #define MOTION_SENSE_HOOK_PRIO (HOOK_PRIO_DEFAULT) +#ifdef CONFIG_ACCEL_INTERRUPTS +/** + * Interrupt function for lid accelerometer. + * + * @param signal GPIO signal that caused interrupt + */ +void accel_int_lid(enum gpio_signal signal); + +/** + * Interrupt function for base accelerometer. + * + * @param signal GPIO signal that caused interrupt + */ +void accel_int_base(enum gpio_signal signal); +#endif + #endif /* __CROS_EC_MOTION_SENSE_H */ diff --git a/test/build.mk b/test/build.mk index 8995c41fb9..593142b58d 100644 --- a/test/build.mk +++ b/test/build.mk @@ -30,7 +30,7 @@ test-list-host=mutex pingpong utils kb_scan kb_mkbp lid_sw power_button hooks test-list-host+=thermal flash queue kb_8042 extpwr_gpio console_edit system test-list-host+=sbs_charging adapter host_command thermal_falco led_spring test-list-host+=bklight_lid bklight_passthru interrupt timer_dos button -test-list-host+=motion_sense math_util sbs_charging_v2 battery_get_params_smart +test-list-host+=motion_lid math_util sbs_charging_v2 battery_get_params_smart test-list-host+=lightbar inductive_charging usb_pd adapter-y=adapter.o @@ -52,7 +52,7 @@ kb_scan-y=kb_scan.o led_spring-y=led_spring.o led_spring_impl.o lid_sw-y=lid_sw.o math_util-y=math_util.o -motion_sense-y=motion_sense.o +motion_lid-y=motion_lid.o mutex-y=mutex.o pingpong-y=pingpong.o power_button-y=power_button.o diff --git a/test/motion_sense.c b/test/motion_lid.c index 5148935533..e8880e85a8 100644 --- a/test/motion_sense.c +++ b/test/motion_lid.c @@ -11,6 +11,7 @@ #include "common.h" #include "hooks.h" #include "host_command.h" +#include "motion_lid.h" #include "motion_sense.h" #include "task.h" #include "test_util.h" @@ -88,15 +89,27 @@ const struct accelgyro_drv test_motion_sense = { .get_data_rate = accel_get_data_rate, }; +const matrix_3x3_t base_standard_ref = { + { 1, 0, 0}, + { 0, 1, 0}, + { 0, 0, 1} +}; + +const matrix_3x3_t lid_standard_ref = { + { 1, 0, 0}, + { 1, 0, 0}, + { 0, 0, 1} +}; + struct motion_sensor_t motion_sensors[] = { {SENSOR_ACTIVE_S0_S3_S5, "base", SENSOR_CHIP_LSM6DS0, SENSOR_ACCELEROMETER, LOCATION_BASE, &test_motion_sense, NULL, NULL, - 0, 119000, 2}, + 0, &base_standard_ref, 119000, 2}, {SENSOR_ACTIVE_S0, "lid", SENSOR_CHIP_KXCJ9, SENSOR_ACCELEROMETER, LOCATION_LID, &test_motion_sense, NULL, NULL, - 0, 100000, 2}, + 0, &lid_standard_ref, 100000, 2}, }; const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors); @@ -130,7 +143,7 @@ static int test_lid_angle(void) task_wake(TASK_ID_MOTIONSENSE); while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample) msleep(5); - TEST_ASSERT(motion_get_lid_angle() == 0); + TEST_ASSERT(motion_lid_get_angle() == 0); /* Set lid open to 90 degrees. */ lid->xyz[X] = -1000; @@ -140,7 +153,7 @@ static int test_lid_angle(void) task_wake(TASK_ID_MOTIONSENSE); while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample) msleep(5); - TEST_ASSERT(motion_get_lid_angle() == 90); + TEST_ASSERT(motion_lid_get_angle() == 90); /* Set lid open to 225. */ lid->xyz[X] = 500; @@ -150,7 +163,7 @@ static int test_lid_angle(void) task_wake(TASK_ID_MOTIONSENSE); while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample) msleep(5); - TEST_ASSERT(motion_get_lid_angle() == 225); + TEST_ASSERT(motion_lid_get_angle() == 225); /* * Align base with hinge and make sure it returns unreliable for angle. @@ -163,7 +176,7 @@ static int test_lid_angle(void) task_wake(TASK_ID_MOTIONSENSE); while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample) msleep(5); - TEST_ASSERT(motion_get_lid_angle() == LID_ANGLE_UNRELIABLE); + TEST_ASSERT(motion_lid_get_angle() == LID_ANGLE_UNRELIABLE); /* * Use all three axes and set lid to negative base and make sure @@ -179,7 +192,7 @@ static int test_lid_angle(void) task_wake(TASK_ID_MOTIONSENSE); while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample) msleep(5); - TEST_ASSERT(motion_get_lid_angle() == 180); + TEST_ASSERT(motion_lid_get_angle() == 180); return EC_SUCCESS; } diff --git a/test/motion_sense.tasklist b/test/motion_lid.tasklist index 60688f9c34..60688f9c34 100644 --- a/test/motion_sense.tasklist +++ b/test/motion_lid.tasklist diff --git a/test/test_config.h b/test/test_config.h index 24ccb19024..9a06858834 100644 --- a/test/test_config.h +++ b/test/test_config.h @@ -50,6 +50,12 @@ #define I2C_PORT_CHARGER 1 #endif +#ifdef TEST_MOTION_LID +#define CONFIG_LID_ANGLE +#define CONFIG_SENSOR_BASE 0 +#define CONFIG_SENSOR_LID 1 +#endif + #ifdef TEST_SBS_CHARGING #define CONFIG_BATTERY_MOCK #define CONFIG_BATTERY_SMART |