scottk/OpenCellular
1
0
Fork 0
mirror of https://github.com/Telecominfraproject/OpenCellular.git synced 2025-12-27 18:25:05 +00:00
Code Issues Packages Projects Releases Wiki Activity

Samus: Split motion sense and lid angle

Browse Source 
Split motion_sense.c.
Translate the accel data in the Android coordinate right away.

BUG=chrome-os-partner:32002
BRANCH=ToT
TEST=On samus, check lid angle are still correct.

Change-Id: If743e25245dc1ce4cdacb8a4d5af22616c4a79e4
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/225486
Reviewed-by: Sheng-liang Song <ssl@chromium.org>
Reviewed-by: Alec Berg <alecaberg@chromium.org>
This commit is contained in:
Gwendal Grignou
2014-10-23 16:58:21 -07:00
committed by chrome-internal-fetch
parent d5b32aa6e1
commit 66164f2784
17 changed files with 437 additions and 353 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
board/host/board.c
View File

@@ -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},
};

9
board/host/board.h
View File

@@ -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 */

32
board/samus/board.c
View File

@@ -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},
};

5
board/samus/board.h
View File

@@ -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 */

1
common/build.mk
View File

@@ -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

1
common/console_output.c
View File

@@ -48,6 +48,7 @@ static const char * const channel_names[] = {
"lidangle",
"lightbar",
"lpc",
"motionlid",
"motionsense",
"pdhostcmd",
"port80",

192
common/motion_lid.c Normal file
View File

@@ -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;
}

343
common/motion_sense.c
View File

@@ -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,6 +50,15 @@ static int lid_angle_is_reliable;
/* Accelerometer polling intervals based on chipset state. */
static int accel_interval_ap_on_ms = 10;
/*
* Sampling interval for measuring acceleration and calculating lid angle.
* Set to accel_interval_ap_on_ms when ap is on.
*/
static int accel_interval_ms;
#ifdef CONFIG_CMD_ACCEL_INFO
static int accel_disp;
#endif
/*
* Angle threshold for how close the hinge aligns with gravity before
@@ -62,107 +68,6 @@ static int accel_interval_ap_on_ms = 10;
*/
#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
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.
*/
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_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;
/*
* 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));
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);
}
}
}
#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;

6
driver/accelgyro_lsm6ds0.c
View File

@@ -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;
}

20
include/config.h
View File

@@ -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
/*****************************************************************************/
@@ -728,12 +740,6 @@
#undef CONFIG_LED_DRIVER_DS2413 /* Maxim DS2413, on one-wire interface */
#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.
*

1
include/console.h
View File

@@ -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,

50
include/motion_lid.h Normal file
View File

@@ -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 */

79
include/motion_sense.h
View File

@@ -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 */

4
test/build.mk
View File

@@ -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

27
test/motion_sense.c → test/motion_lid.c
View File

@@ -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;
}

0
test/motion_sense.tasklist → test/motion_lid.tasklist
View File

6
test/test_config.h
View File

@@ -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