diff options
Diffstat (limited to 'common')
-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 |
4 files changed, 301 insertions, 236 deletions
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; |