summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--board/host/board.c14
-rw-r--r--board/host/board.h9
-rw-r--r--board/samus/board.c32
-rw-r--r--board/samus/board.h5
-rw-r--r--common/build.mk1
-rw-r--r--common/console_output.c1
-rw-r--r--common/motion_lid.c192
-rw-r--r--common/motion_sense.c343
-rw-r--r--driver/accelgyro_lsm6ds0.c6
-rw-r--r--include/config.h20
-rw-r--r--include/console.h1
-rw-r--r--include/motion_lid.h50
-rw-r--r--include/motion_sense.h79
-rw-r--r--test/build.mk4
-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.h6
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