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: added gyro support for lsm6ds0

Browse Source 
Changed motion_sense task to assume sensors are unpowered in G3
and re-initialize sensors every time coming out of G3.

Added EC command line test utils as well.
Fixed some bug during unit tests.

BUG=chrome-os-partner:27313,27320
BRANCH=ToT
TEST=Verified on Samus.

Tested with accel EC CLIs
accelread, accelrange, accelrate, accelres

Tested accelcalib, a ACCEL calibration util, and it succeeded.

Tested sysfs interface:
cd /sys/bus/iio/devices/iio:device1
 cat in_accel_*_gyro_raw

Signed-off-by: Sheng-Liang Song <ssl@chromium.org>
Change-Id: I5752b00c03e1942c790ea4f28610fda83fa2dcbc
Reviewed-on: https://chromium-review.googlesource.com/211484
Reviewed-by: Alec Berg <alecaberg@chromium.org>
This commit is contained in:
Sheng-Liang Song
2014-08-13 14:17:07 -07:00
committed by chrome-internal-fetch
parent c598e1ac06
commit 7d40063d46
12 changed files with 1101 additions and 624 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
common/math_util.c
View File

@@ -84,18 +84,30 @@ float cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2)
return (float)dotproduct / (denominator);
}
void rotate(const vector_3_t v, const matrix_3x3_t (* const R),
vector_3_t *res)
/*
* rotate a vector v
* - support input v and output res are the same vector
*/
void rotate(const vector_3_t v, const matrix_3x3_t R,
vector_3_t res)
{
(*res)[0] = v[0] * (*R)[0][0] +
v[1] * (*R)[1][0] +
v[2] * (*R)[2][0];
(*res)[1] = v[0] * (*R)[0][1] +
v[1] * (*R)[1][1] +
v[2] * (*R)[2][1];
(*res)[2] = v[0] * (*R)[0][2] +
v[1] * (*R)[1][2] +
v[2] * (*R)[2][2];
vector_3_t t;
/* copy input v to temp vector t */
t[0] = v[0];
t[1] = v[1];
t[2] = v[2];
/* start rotate */
res[0] = t[0] * R[0][0] +
t[1] * R[1][0] +
t[2] * R[2][0];
res[1] = t[0] * R[0][1] +
t[1] * R[1][1] +
t[2] * R[2][1];
res[2] = t[0] * R[0][2] +
t[1] * R[1][2] +
t[2] * R[2][2];
}
#ifdef CONFIG_ACCEL_CALIBRATE

1
common/motion_calibrate.c
View File

@@ -9,6 +9,7 @@
#include "console.h"
#include "math_util.h"
#include "motion_sense.h"
#include "accelgyro.h"
#include "timer.h"
#include "task.h"
#include "uart.h"

537
common/motion_sense.c
View File

@@ -13,6 +13,7 @@
#include "lid_angle.h"
#include "math_util.h"
#include "motion_sense.h"
#include "power.h"
#include "timer.h"
#include "task.h"
#include "util.h"
@@ -24,12 +25,15 @@
/* Minimum time in between running motion sense task loop. */
#define MIN_MOTION_SENSE_WAIT_TIME (1 * MSEC)
static const struct motion_sensor_t *base;
static const struct motion_sensor_t *lid;
/* Time to wait in between failed attempts to initialize sensors */
#define TASK_MOTION_SENSE_WAIT_TIME (500 * MSEC)
/* For vector_3_t, define which coordinates are in which location. */
enum {
X, Y, Z
};
/* Current acceleration vectors and current lid angle. */
static vector_3_t acc_lid_raw, acc_lid, acc_base;
static vector_3_t acc_lid_host, acc_base_host;
static float lid_angle_deg;
static int lid_angle_is_reliable;
@@ -56,11 +60,6 @@ static int accel_interval_ms;
static int accel_disp;
#endif
/* For vector_3_t, define which coordinates are in which location. */
enum {
X, Y, Z
};
/* Pointer to constant acceleration orientation data. */
const struct accel_orientation * const p_acc_orient = &acc_orient;
@@ -74,7 +73,7 @@ const struct accel_orientation * const p_acc_orient = &acc_orient;
*
* @return flag representing if resulting lid angle calculation is reliable.
*/
static int calculate_lid_angle(vector_3_t base, vector_3_t lid,
static int calculate_lid_angle(const vector_3_t base, const vector_3_t lid,
float *lid_angle)
{
vector_3_t v;
@@ -121,9 +120,9 @@ static int calculate_lid_angle(vector_3_t base, vector_3_t lid,
* estimated 270 degree vector then the result is negative, otherwise
* it is positive.
*/
rotate(base, &p_acc_orient->rot_hinge_90, &v);
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);
rotate(v, p_acc_orient->rot_hinge_180, v);
ang_lid_270 = cosine_of_angle_diff(v, lid);
/*
@@ -159,143 +158,247 @@ int motion_get_lid_angle(void)
#ifdef CONFIG_ACCEL_CALIBRATE
void motion_get_accel_lid(vector_3_t *v, int adjusted)
{
memcpy(v, adjusted ? &acc_lid : &acc_lid_raw, sizeof(vector_3_t));
int i;
struct motion_sensor_t *sensor;
struct motion_sensor_t *accel_lid = NULL;
for (i = 0; i < motion_sensor_count; ++i) {
sensor = &motion_sensors[i];
if ((LOCATION_BASE == sensor->location)
&& (SENSOR_ACCELEROMETER == sensor->type)) {
accel_lid = sensor;
break;
}
}
if (accel_lid)
memcpy(v, (adjusted ? accel_lid->xyz : accel_lid->raw_xyz),
sizeof(vector_3_t));
}
void motion_get_accel_base(vector_3_t *v)
{
memcpy(v, &acc_base, sizeof(vector_3_t));
int i;
struct motion_sensor_t *sensor;
struct motion_sensor_t *accel_base = NULL;
for (i = 0; i < motion_sensor_count; ++i) {
sensor = &motion_sensors[i];
if ((LOCATION_BASE == sensor->location)
&& (SENSOR_ACCELEROMETER == sensor->type)) {
accel_base = sensor;
break;
}
}
if (accel_base)
memcpy(v, accel_base->xyz, sizeof(vector_3_t));
}
#endif
static void set_ap_suspend_polling(void)
static void clock_chipset_shutdown(void)
{
accel_interval_ms = accel_interval_ap_suspend_ms;
}
DECLARE_HOOK(HOOK_CHIPSET_SUSPEND, set_ap_suspend_polling, HOOK_PRIO_DEFAULT);
static void set_ap_on_polling(void)
{
accel_interval_ms = accel_interval_ap_on_ms;
}
DECLARE_HOOK(HOOK_CHIPSET_RESUME, set_ap_on_polling, HOOK_PRIO_DEFAULT);
void motion_sense_task(void)
{
static timestamp_t ts0, ts1;
int wait_us;
int ret;
uint8_t *lpc_status;
uint16_t *lpc_data;
int sample_id = 0;
int i;
struct motion_sensor_t *sensor;
accel_interval_ms = accel_interval_ap_suspend_ms;
for (i = 0; i < motion_sensor_count; i++) {
sensor = &motion_sensors[i];
sensor->power = SENSOR_POWER_OFF;
}
}
DECLARE_HOOK(HOOK_CHIPSET_SHUTDOWN, clock_chipset_shutdown, HOOK_PRIO_DEFAULT);
lpc_status = host_get_memmap(EC_MEMMAP_ACC_STATUS);
lpc_data = (uint16_t *)host_get_memmap(EC_MEMMAP_ACC_DATA);
static void clock_chipset_startup(void)
{
int i;
struct motion_sensor_t *sensor;
accel_interval_ms = accel_interval_ap_on_ms;
for (i = 0; i < motion_sensor_count; i++) {
sensor = &motion_sensors[i];
sensor->power = SENSOR_POWER_ON;
}
}
DECLARE_HOOK(HOOK_CHIPSET_STARTUP, clock_chipset_startup, HOOK_PRIO_DEFAULT);
/* Write to LPC status byte to represent that accelerometers are present. */
static inline void set_present(uint8_t *lpc_status)
{
*lpc_status |= EC_MEMMAP_ACC_STATUS_PRESENCE_BIT;
}
/* Update/Write LPC data */
static inline void update_sense_data(uint8_t *lpc_status,
uint16_t *lpc_data, int *psample_id)
{
int i;
struct motion_sensor_t *sensor;
/*
* Set the busy bit before writing the sensor data. Increment
* the counter and clear the busy bit after writing the sensor
* data. On the host side, the host needs to make sure the busy
* bit is not set and that the counter remains the same before
* and after reading the data.
*/
*lpc_status |= EC_MEMMAP_ACC_STATUS_BUSY_BIT;
/*
* TODO(crosbug.com/p/27320): The motion_sense task currently assumes
* one configuration of motion sensors. Namely, it assumes there is
* one accel in the base, one in the lid. Eventually, these
* assumptions will have to be removed when we have other
* configurations of motion sensors.
* Copy sensor data to shared memory. Note that this code
* assumes little endian, which is what the host expects. Also,
* note that we share the lid angle calculation with host only
* for debugging purposes. The EC lid angle is an approximation
* with un-calibrated accels. The AP calculates a separate,
* more accurate lid angle.
*/
for (i = 0; i < motion_sensor_count; ++i) {
if (motion_sensors[i].location == LOCATION_LID)
lid = &motion_sensors[i];
else if (motion_sensors[i].location == LOCATION_BASE)
base = &motion_sensors[i];
lpc_data[0] = motion_get_lid_angle();
for (i = 0; i < motion_sensor_count; i++) {
sensor = &motion_sensors[i];
lpc_data[1+3*i] = sensor->xyz[X];
lpc_data[2+3*i] = sensor->xyz[Y];
lpc_data[3+3*i] = sensor->xyz[Z];
}
if (lid == NULL || base == NULL) {
CPRINTS("Invalid motion_sensors list, lid and base required");
/*
* Increment sample id and clear busy bit to signal we finished
* updating data.
*/
*psample_id = (*psample_id + 1) &
EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK;
*lpc_status = EC_MEMMAP_ACC_STATUS_PRESENCE_BIT | *psample_id;
}
static inline void motion_sense_init(struct motion_sensor_t *sensor)
{
int ret;
if (sensor->power == SENSOR_POWER_OFF)
return;
if (sensor->state != SENSOR_NOT_INITIALIZED)
return;
}
/* Initialize accelerometers. */
ret = lid->drv->init(lid->drv_data, lid->i2c_addr);
ret |= base->drv->init(base->drv_data, base->i2c_addr);
/* If accelerometers do not initialize, then end task. */
ret = sensor->drv->init(sensor);
if (ret != EC_SUCCESS) {
CPRINTS("Accel init failed; stopping MS");
sensor->state = SENSOR_INIT_ERROR;
return;
}
/* Initialize sampling interval. */
accel_interval_ms = accel_interval_ap_suspend_ms;
/* Set default accelerometer parameters. */
lid->drv->set_range(lid->drv_data, 2, 1);
lid->drv->set_resolution(lid->drv_data, 12, 1);
lid->drv->set_datarate(lid->drv_data, 100000, 1);
base->drv->set_range(base->drv_data, 2, 1);
base->drv->set_resolution(base->drv_data, 12, 1);
base->drv->set_datarate(base->drv_data, 100000, 1);
sensor->state = SENSOR_INITIALIZED;
}
/* Write to status byte to represent that accelerometers are present. */
*lpc_status |= EC_MEMMAP_ACC_STATUS_PRESENCE_BIT;
static int motion_sense_read(struct motion_sensor_t *sensor)
{
int ret;
if (sensor->power == SENSOR_POWER_OFF)
return EC_ERROR_UNKNOWN;
if (sensor->state != SENSOR_INITIALIZED)
return EC_ERROR_UNKNOWN;
/* Read all raw X,Y,Z accelerations. */
ret = sensor->drv->read(sensor,
&sensor->raw_xyz[X],
&sensor->raw_xyz[Y],
&sensor->raw_xyz[Z]);
if (ret != EC_SUCCESS) {
sensor->state = SENSOR_INIT_ERROR;
return EC_ERROR_UNKNOWN;
}
return EC_SUCCESS;
}
/*
* Motion Sense Task
* Requirement: motion_sensors[] are defined in board.c file.
* Two (minimium) Accelerometers:
* 1 in the A/B(lid, display) and 1 in the C/D(base, keyboard)
* Gyro Sensor (optional)
*/
void motion_sense_task(void)
{
int i;
int wait_us;
static timestamp_t ts0, ts1;
uint8_t *lpc_status;
uint16_t *lpc_data;
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);
for (i = 0; i < motion_sensor_count; ++i) {
sensor = &motion_sensors[i];
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);
while (1) {
ts0 = get_time();
rd_cnt = 0;
for (i = 0; i < motion_sensor_count; ++i) {
/* Read all accelerations. */
lid->drv->read(lid->drv_data, &acc_lid_raw[X], &acc_lid_raw[Y],
&acc_lid_raw[Z]);
base->drv->read(base->drv_data, &acc_base[X], &acc_base[Y],
&acc_base[Z]);
sensor = &motion_sensors[i];
/*
* Rotate the lid vector so the reference frame aligns with
* the base sensor.
*/
rotate(acc_lid_raw, &p_acc_orient->rot_align, &acc_lid);
if (sensor->power == SENSOR_POWER_OFF)
continue;
/* Calculate angle of lid. */
lid_angle_is_reliable = calculate_lid_angle(acc_base, acc_lid,
motion_sense_init(sensor);
if (EC_SUCCESS == motion_sense_read(sensor))
rd_cnt++;
/*
* 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));
}
if (rd_cnt != motion_sensor_count) {
task_wait_event(TASK_MOTION_SENSE_WAIT_TIME);
continue;
}
/* Calculate angle of lid accel. */
lid_angle_is_reliable = calculate_lid_angle(
accel_base->xyz,
accel_lid->xyz,
&lid_angle_deg);
/* TODO(crosbug.com/p/25597): Add filter to smooth lid angle. */
/* Rotate accels into standard reference frame for the host. */
rotate(acc_base, &p_acc_orient->rot_standard_ref,
&acc_base_host);
rotate(acc_lid, &p_acc_orient->rot_standard_ref,
&acc_lid_host);
/*
* Set the busy bit before writing the sensor data. Increment
* the counter and clear the busy bit after writing the sensor
* data. On the host side, the host needs to make sure the busy
* bit is not set and that the counter remains the same before
* and after reading the data.
*/
*lpc_status |= EC_MEMMAP_ACC_STATUS_BUSY_BIT;
/*
* Copy sensor data to shared memory. Note that this code
* assumes little endian, which is what the host expects. Also,
* note that we share the lid angle calculation with host only
* for debugging purposes. The EC lid angle is an approximation
* with un-calibrated accels. The AP calculates a separate,
* more accurate lid angle.
*/
lpc_data[0] = motion_get_lid_angle();
lpc_data[1] = acc_base_host[X];
lpc_data[2] = acc_base_host[Y];
lpc_data[3] = acc_base_host[Z];
lpc_data[4] = acc_lid_host[X];
lpc_data[5] = acc_lid_host[Y];
lpc_data[6] = acc_lid_host[Z];
/*
* Increment sample id and clear busy bit to signal we finished
* updating data.
*/
sample_id = (sample_id + 1) &
EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK;
*lpc_status = EC_MEMMAP_ACC_STATUS_PRESENCE_BIT | sample_id;
for (i = 0; i < motion_sensor_count; ++i) {
sensor = &motion_sensors[i];
/*
* TODO(crosbug.com/p/25597):
* Add filter to smooth lid angle.
*/
/* 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());
@@ -303,14 +406,18 @@ void motion_sense_task(void)
#ifdef CONFIG_CMD_LID_ANGLE
if (accel_disp) {
CPRINTS("ACC base=%-5d, %-5d, %-5d lid=%-5d, "
"%-5d, %-5d a=%-6.1d r=%d",
acc_base[X], acc_base[Y], acc_base[Z],
acc_lid[X], acc_lid[Y], acc_lid[Z],
(int)(10*lid_angle_deg),
for (i = 0; i < motion_sensor_count; ++i) {
sensor = &motion_sensors[i];
CPRINTS("%s=%-5d, %-5d, %-5d", sensor->name,
sensor->raw_xyz[X],
sensor->raw_xyz[Y],
sensor->raw_xyz[Z]);
}
CPRINTS("a=%-6.1d r=%d", (int)(10*lid_angle_deg),
lid_angle_is_reliable);
}
#endif
update_sense_data(lpc_status, lpc_data, &sample_id);
/* Delay appropriately to keep sampling time consistent. */
ts1 = get_time();
@@ -349,16 +456,43 @@ void accel_int_base(enum gpio_signal signal)
/* Host commands */
/* Function to map host sensor IDs to motion sensor. */
static const struct motion_sensor_t
static struct motion_sensor_t
*host_sensor_id_to_motion_sensor(int host_id)
{
switch (host_id) {
case EC_MOTION_SENSOR_ACCEL_BASE:
return base;
case EC_MOTION_SENSOR_ACCEL_LID:
return lid;
int i;
struct motion_sensor_t *sensor = NULL;
for (i = 0; i < motion_sensor_count; ++i) {
if ((LOCATION_BASE == sensor->location)
&& (SENSOR_ACCELEROMETER == sensor->type)
&& (host_id == EC_MOTION_SENSOR_ACCEL_BASE)) {
sensor = &motion_sensors[i];
break;
}
if ((LOCATION_LID == sensor->location)
&& (SENSOR_ACCELEROMETER == sensor->type)
&& (host_id == EC_MOTION_SENSOR_ACCEL_LID)) {
sensor = &motion_sensors[i];
break;
}
if ((LOCATION_BASE == sensor->location)
&& (SENSOR_GYRO == sensor->type)
&& (host_id == EC_MOTION_SENSOR_GYRO)) {
sensor = &motion_sensors[i];
break;
}
}
if (!sensor)
return NULL;
if ((sensor->power == SENSOR_POWER_ON)
&& (sensor->state == SENSOR_INITIALIZED))
return sensor;
/* If no match then the EC currently doesn't support ID received. */
return NULL;
}
@@ -367,60 +501,55 @@ 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;
const struct motion_sensor_t *sensor;
int data;
struct motion_sensor_t *sensor;
int i, data;
switch (in->cmd) {
case MOTIONSENSE_CMD_DUMP:
/*
* TODO(crosbug.com/p/27320): Need to remove hard coding and
* use some motion_sense data structure from the board file to
* help fill in this response.
*/
out->dump.module_flags =
(*(host_get_memmap(EC_MEMMAP_ACC_STATUS)) &
EC_MEMMAP_ACC_STATUS_PRESENCE_BIT) ?
MOTIONSENSE_MODULE_FLAG_ACTIVE : 0;
out->dump.sensor_flags[0] = MOTIONSENSE_SENSOR_FLAG_PRESENT;
out->dump.sensor_flags[1] = MOTIONSENSE_SENSOR_FLAG_PRESENT;
out->dump.sensor_flags[2] = 0;
out->dump.data[0] = acc_base_host[X];
out->dump.data[1] = acc_base_host[Y];
out->dump.data[2] = acc_base_host[Z];
out->dump.data[3] = acc_lid_host[X];
out->dump.data[4] = acc_lid_host[Y];
out->dump.data[5] = acc_lid_host[Z];
for (i = 0; i < motion_sensor_count; i++) {
sensor = &motion_sensors[i];
out->dump.sensor_flags[i] =
MOTIONSENSE_SENSOR_FLAG_PRESENT;
out->dump.data[0+3*i] = sensor->xyz[X];
out->dump.data[1+3*i] = sensor->xyz[Y];
out->dump.data[2+3*i] = sensor->xyz[Z];
}
args->response_size = sizeof(out->dump);
break;
case MOTIONSENSE_CMD_INFO:
/*
* TODO(crosbug.com/p/27320): Need to remove hard coding and
* use some motion_sense data structure from the board file to
* help fill in this response.
*/
sensor = host_sensor_id_to_motion_sensor(
in->sensor_odr.sensor_num);
if (sensor == NULL)
return EC_RES_INVALID_PARAM;
if (sensor->drv->sensor_type == SENSOR_ACCELEROMETER)
if (sensor->type == SENSOR_ACCELEROMETER)
out->info.type = MOTIONSENSE_TYPE_ACCEL;
else if (sensor->drv->sensor_type == SENSOR_GYRO)
else if (sensor->type == SENSOR_GYRO)
out->info.type = MOTIONSENSE_TYPE_GYRO;
if (sensor->location == LOCATION_BASE)
out->info.location = MOTIONSENSE_LOC_BASE;
else if (sensor->location == LOCATION_LID)
out->info.location = MOTIONSENSE_LOC_LID;
if (sensor->drv->chip_type == CHIP_KXCJ9)
if (sensor->chip == SENSOR_CHIP_KXCJ9)
out->info.chip = MOTIONSENSE_CHIP_KXCJ9;
else if (sensor->drv->chip_type == CHIP_LSM6DS0)
if (sensor->chip == SENSOR_CHIP_LSM6DS0)
out->info.chip = MOTIONSENSE_CHIP_LSM6DS0;
args->response_size = sizeof(out->info);
break;
case MOTIONSENSE_CMD_EC_RATE:
@@ -452,9 +581,9 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
if (sensor == NULL)
return EC_RES_INVALID_PARAM;
/* Set new datarate if the data arg has a value. */
/* Set new data rate if the data arg has a value. */
if (in->sensor_odr.data != EC_MOTION_SENSE_NO_VALUE) {
if (sensor->drv->set_datarate(sensor->drv_data,
if (sensor->drv->set_data_rate(sensor,
in->sensor_odr.data,
in->sensor_odr.roundup)
!= EC_SUCCESS) {
@@ -464,7 +593,7 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
}
}
sensor->drv->get_datarate(sensor->drv_data, &data);
sensor->drv->get_data_rate(sensor, &data);
out->sensor_odr.ret = data;
args->response_size = sizeof(out->sensor_odr);
@@ -477,9 +606,9 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
if (sensor == NULL)
return EC_RES_INVALID_PARAM;
/* Set new datarate if the data arg has a value. */
/* Set new data rate if the data arg has a value. */
if (in->sensor_range.data != EC_MOTION_SENSE_NO_VALUE) {
if (sensor->drv->set_range(sensor->drv_data,
if (sensor->drv->set_range(sensor,
in->sensor_range.data,
in->sensor_range.roundup)
!= EC_SUCCESS) {
@@ -489,7 +618,7 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
}
}
sensor->drv->get_range(sensor->drv_data, &data);
sensor->drv->get_range(sensor, &data);
out->sensor_range.ret = data;
args->response_size = sizeof(out->sensor_range);
@@ -572,9 +701,10 @@ static int command_accelrange(int argc, char **argv)
/* First argument is sensor id. */
id = strtoi(argv[1], &e, 0);
if (*e || id < 0 || id > motion_sensor_count)
if (*e || id < 0 || id >= motion_sensor_count)
return EC_ERROR_PARAM1;
sensor = motion_sensors[id];
sensor = &motion_sensors[id];
if (argc >= 3) {
/* Second argument is data to write. */
@@ -593,12 +723,12 @@ static int command_accelrange(int argc, char **argv)
* Write new range, if it returns invalid arg, then return
* a parameter error.
*/
if (sensor->drv->set_range(sensor->drv_data,
if (sensor->drv->set_range(sensor,
data,
round) == EC_ERROR_INVAL)
return EC_ERROR_PARAM2;
} else {
sensor->drv->get_range(sensor->drv_data, &data);
sensor->drv->get_range(sensor, &data);
ccprintf("Range for sensor %d: %d\n", id, data);
}
@@ -619,9 +749,10 @@ static int command_accelresolution(int argc, char **argv)
/* First argument is sensor id. */
id = strtoi(argv[1], &e, 0);
if (*e || id < 0 || id > motion_sensor_count)
if (*e || id < 0 || id >= motion_sensor_count)
return EC_ERROR_PARAM1;
sensor = motion_sensors[id];
sensor = &motion_sensors[id];
if (argc >= 3) {
/* Second argument is data to write. */
@@ -640,11 +771,11 @@ static int command_accelresolution(int argc, char **argv)
* Write new resolution, if it returns invalid arg, then
* return a parameter error.
*/
if (sensor->drv->set_resolution(sensor->drv_data, data, round)
if (sensor->drv->set_resolution(sensor, data, round)
== EC_ERROR_INVAL)
return EC_ERROR_PARAM2;
} else {
sensor->drv->get_resolution(sensor->drv_data, &data);
sensor->drv->get_resolution(sensor, &data);
ccprintf("Resolution for sensor %d: %d\n", id, data);
}
@@ -654,7 +785,7 @@ DECLARE_CONSOLE_COMMAND(accelres, command_accelresolution,
"id [data [roundup]]",
"Read or write accelerometer resolution", NULL);
static int command_acceldatarate(int argc, char **argv)
static int command_accel_data_rate(int argc, char **argv)
{
char *e;
int id, data, round = 1;
@@ -665,9 +796,10 @@ static int command_acceldatarate(int argc, char **argv)
/* First argument is sensor id. */
id = strtoi(argv[1], &e, 0);
if (*e || id < 0 || id > motion_sensor_count)
if (*e || id < 0 || id >= motion_sensor_count)
return EC_ERROR_PARAM1;
sensor = motion_sensors[id];
sensor = &motion_sensors[id];
if (argc >= 3) {
/* Second argument is data to write. */
@@ -686,19 +818,75 @@ static int command_acceldatarate(int argc, char **argv)
* Write new data rate, if it returns invalid arg, then
* return a parameter error.
*/
if (sensor->drv->set_datarate(sensor->drv_data, data, round)
if (sensor->drv->set_data_rate(sensor, data, round)
== EC_ERROR_INVAL)
return EC_ERROR_PARAM2;
} else {
sensor->drv->get_datarate(sensor->drv_data, &data);
sensor->drv->get_data_rate(sensor, &data);
ccprintf("Data rate for sensor %d: %d\n", id, data);
}
return EC_SUCCESS;
}
DECLARE_CONSOLE_COMMAND(accelrate, command_acceldatarate,
DECLARE_CONSOLE_COMMAND(accelrate, command_accel_data_rate,
"id [data [roundup]]",
"Read or write accelerometer range", NULL);
"Read or write accelerometer ODR", NULL);
static int command_accel_read_xyz(int argc, char **argv)
{
char *e;
int id, x, y, z, n = 1;
struct motion_sensor_t *sensor;
if (argc < 2)
return EC_ERROR_PARAM_COUNT;
/* First argument is sensor id. */
id = strtoi(argv[1], &e, 0);
if (*e || id < 0 || id >= motion_sensor_count)
return EC_ERROR_PARAM1;
if (argc >= 3)
n = strtoi(argv[2], &e, 0);
sensor = &motion_sensors[id];
while ((n == -1) || (n-- > 0)) {
sensor->drv->read(sensor, &x, &y, &z);
ccprintf("XYZ:%d %d %d %d\n", id, x, y, z);
task_wait_event(MIN_MOTION_SENSE_WAIT_TIME);
}
return EC_SUCCESS;
}
DECLARE_CONSOLE_COMMAND(accelread, command_accel_read_xyz,
"id [n]",
"Read sensor x/y/z", NULL);
static int command_accel_init(int argc, char **argv)
{
char *e;
int id;
struct motion_sensor_t *sensor;
if (argc < 2)
return EC_ERROR_PARAM_COUNT;
/* First argument is sensor id. */
id = strtoi(argv[1], &e, 0);
if (*e || id < 0 || id >= motion_sensor_count)
return EC_ERROR_PARAM1;
sensor = &motion_sensors[id];
sensor->drv->init(sensor);
ccprintf("%s\n", sensor->name);
return EC_SUCCESS;
}
DECLARE_CONSOLE_COMMAND(accelinit, command_accel_init,
"id",
"Init sensor", NULL);
#ifdef CONFIG_ACCEL_INTERRUPTS
static int command_accelerometer_interrupt(int argc, char **argv)
@@ -714,14 +902,15 @@ static int command_accelerometer_interrupt(int argc, char **argv)
id = strtoi(argv[1], &e, 0);
if (*e || id < 0 || id >= motion_sensor_count)
return EC_ERROR_PARAM1;
sensor = motion_sensors[id];
sensor = &motion_sensors[id];
/* Second argument is interrupt threshold. */
thresh = strtoi(argv[2], &e, 0);
if (*e)
return EC_ERROR_PARAM2;
sensor->drv->set_interrupt(drv_data, thresh);
sensor->drv->set_interrupt(sensor, thresh);
return EC_SUCCESS;
}

338
driver/accel_kxcj9.c
View File

@@ -45,6 +45,7 @@ static const struct accel_param_pair resolutions[] = {
/* List of ODR values in mHz and their associated register values. */
static const struct accel_param_pair datarates[] = {
{0, KXCJ9_OSA_0_000HZ},
{781, KXCJ9_OSA_0_781HZ},
{1563, KXCJ9_OSA_1_563HZ},
{3125, KXCJ9_OSA_3_125HZ},
@@ -113,33 +114,27 @@ static int raw_write8(const int addr, const int reg, int data)
*
* @return EC_SUCCESS if successful, EC_ERROR_* otherwise
*/
static int disable_sensor(struct kxcj9_data *data, int *ctrl1)
static int disable_sensor(const struct motion_sensor_t *s, int *ctrl1)
{
int ret;
int i, ret;
/*
* Read the current state of the ctrl1 register so that we can restore
* it later.
* Read the current state of the ctrl1 register
* so that we can restore it later.
*/
ret = raw_read8(data->accel_addr, KXCJ9_CTRL1, ctrl1);
if (ret != EC_SUCCESS)
return ret;
for (i = 0; i < SENSOR_ENABLE_ATTEMPTS; i++) {
ret = raw_read8(s->i2c_addr, KXCJ9_CTRL1, ctrl1);
if (ret != EC_SUCCESS)
continue;
/*
* Before disabling the sensor, acquire mutex to prevent another task
* from attempting to access accel parameters until we enable sensor.
*/
mutex_lock(&data->accel_mutex);
*ctrl1 &= ~KXCJ9_CTRL1_PC1;
/* Disable sensor. */
*ctrl1 &= ~KXCJ9_CTRL1_PC1;
ret = raw_write8(data->accel_addr, KXCJ9_CTRL1, *ctrl1);
if (ret != EC_SUCCESS) {
mutex_unlock(&data->accel_mutex);
return ret;
ret = raw_write8(s->i2c_addr, KXCJ9_CTRL1, *ctrl1);
if (ret == EC_SUCCESS)
return EC_SUCCESS;
}
return EC_SUCCESS;
CPRINTF("Error trying to disable accelerometer\n");
return ret;
}
/**
@@ -152,49 +147,50 @@ static int disable_sensor(struct kxcj9_data *data, int *ctrl1)
*
* @return EC_SUCCESS if successful, EC_ERROR_* otherwise
*/
static int enable_sensor(struct kxcj9_data *data, const int ctrl1)
static int enable_sensor(const struct motion_sensor_t *s, int ctrl1)
{
int i, ret;
for (i = 0; i < SENSOR_ENABLE_ATTEMPTS; i++) {
ret = raw_read8(s->i2c_addr, KXCJ9_CTRL1, &ctrl1);
if (ret != EC_SUCCESS)
continue;
/* Enable accelerometer based on ctrl1 value. */
ret = raw_write8(data->accel_addr, KXCJ9_CTRL1,
ret = raw_write8(s->i2c_addr, KXCJ9_CTRL1,
ctrl1 | KXCJ9_CTRL1_PC1);
/* On first success, we are done. */
if (ret == EC_SUCCESS) {
mutex_unlock(&data->accel_mutex);
if (ret == EC_SUCCESS)
return EC_SUCCESS;
}
}
/* Release mutex. */
mutex_unlock(&data->accel_mutex);
/* Cannot enable accel, print warning and return an error. */
CPRINTF("Error trying to enable accelerometer\n");
return ret;
}
static int accel_set_range(void *drv_data,
const int range,
const int rnd)
static int set_range(const struct motion_sensor_t *s,
int range,
int rnd)
{
int ret, ctrl1, ctrl1_new, index;
struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data;
/* Find index for interface pair matching the specified range. */
index = find_param_index(range, rnd, ranges, ARRAY_SIZE(ranges));
/* Disable the sensor to allow for changing of critical parameters. */
ret = disable_sensor(data, &ctrl1);
if (ret != EC_SUCCESS)
mutex_lock(s->mutex);
ret = disable_sensor(s, &ctrl1);
if (ret != EC_SUCCESS) {
mutex_unlock(s->mutex);
return ret;
}
/* Determine new value of CTRL1 reg and attempt to write it. */
ctrl1_new = (ctrl1 & ~KXCJ9_GSEL_ALL) | ranges[index].reg;
ret = raw_write8(data->accel_addr, KXCJ9_CTRL1, ctrl1_new);
ret = raw_write8(s->i2c_addr, KXCJ9_CTRL1, ctrl1_new);
/* If successfully written, then save the range. */
if (ret == EC_SUCCESS) {
@@ -203,38 +199,44 @@ static int accel_set_range(void *drv_data,
}
/* Re-enable the sensor. */
if (enable_sensor(data, ctrl1) != EC_SUCCESS)
return EC_ERROR_UNKNOWN;
if (enable_sensor(s, ctrl1) != EC_SUCCESS)
ret = EC_ERROR_UNKNOWN;
mutex_unlock(s->mutex);
return ret;
}
static int accel_get_range(void *drv_data, int * const range)
static int get_range(const struct motion_sensor_t *s,
int * const range)
{
struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data;
*range = ranges[data->sensor_range].val;
return EC_SUCCESS;
}
static int accel_set_resolution(void *drv_data,
const int res,
const int rnd)
static int set_resolution(const struct motion_sensor_t *s,
int res,
int rnd)
{
int ret, ctrl1, ctrl1_new, index;
struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data;
/* Find index for interface pair matching the specified resolution. */
index = find_param_index(res, rnd, resolutions,
ARRAY_SIZE(resolutions));
/* Disable the sensor to allow for changing of critical parameters. */
ret = disable_sensor(data, &ctrl1);
if (ret != EC_SUCCESS)
mutex_lock(s->mutex);
ret = disable_sensor(s, &ctrl1);
if (ret != EC_SUCCESS) {
mutex_unlock(s->mutex);
return ret;
}
/* Determine new value of CTRL1 reg and attempt to write it. */
ctrl1_new = (ctrl1 & ~KXCJ9_RES_12BIT) | resolutions[index].reg;
ret = raw_write8(data->accel_addr, KXCJ9_CTRL1, ctrl1_new);
ret = raw_write8(s->i2c_addr, KXCJ9_CTRL1, ctrl1_new);
/* If successfully written, then save the range. */
if (ret == EC_SUCCESS) {
@@ -243,36 +245,41 @@ static int accel_set_resolution(void *drv_data,
}
/* Re-enable the sensor. */
if (enable_sensor(data, ctrl1) != EC_SUCCESS)
return EC_ERROR_UNKNOWN;
if (enable_sensor(s, ctrl1) != EC_SUCCESS)
ret = EC_ERROR_UNKNOWN;
mutex_unlock(s->mutex);
return ret;
}
static int accel_get_resolution(void *drv_data, int * const res)
static int get_resolution(const struct motion_sensor_t *s,
int *res)
{
struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data;
*res = resolutions[data->sensor_resolution].val;
return EC_SUCCESS;
}
static int accel_set_datarate(void *drv_data,
const int rate,
const int rnd)
static int set_data_rate(const struct motion_sensor_t *s,
int rate,
int rnd)
{
int ret, ctrl1, index;
struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data;
/* Find index for interface pair matching the specified rate. */
index = find_param_index(rate, rnd, datarates, ARRAY_SIZE(datarates));
/* Disable the sensor to allow for changing of critical parameters. */
ret = disable_sensor(data, &ctrl1);
if (ret != EC_SUCCESS)
mutex_lock(s->mutex);
ret = disable_sensor(s, &ctrl1);
if (ret != EC_SUCCESS) {
mutex_unlock(s->mutex);
return ret;
}
/* Set output data rate. */
ret = raw_write8(data->accel_addr, KXCJ9_DATA_CTRL,
ret = raw_write8(s->i2c_addr, KXCJ9_DATA_CTRL,
datarates[index].reg);
/* If successfully written, then save the range. */
@@ -280,33 +287,39 @@ static int accel_set_datarate(void *drv_data,
data->sensor_datarate = index;
/* Re-enable the sensor. */
if (enable_sensor(data, ctrl1) != EC_SUCCESS)
return EC_ERROR_UNKNOWN;
if (enable_sensor(s, ctrl1) != EC_SUCCESS)
ret = EC_ERROR_UNKNOWN;
mutex_unlock(s->mutex);
return ret;
}
static int accel_get_datarate(void *drv_data, int * const rate)
static int get_data_rate(const struct motion_sensor_t *s,
int *rate)
{
struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data;
*rate = datarates[data->sensor_datarate].val;
return EC_SUCCESS;
}
#ifdef CONFIG_ACCEL_INTERRUPTS
static int accel_set_interrupt(void *drv_data, unsigned int threshold)
static int set_interrupt(const struct motion_sensor_t *s,
unsigned int threshold)
{
int ctrl1, tmp, ret;
struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data;
/* Disable the sensor to allow for changing of critical parameters. */
ret = disable_sensor(data, &ctrl1);
if (ret != EC_SUCCESS)
mutex_lock(s->mutex);
ret = disable_sensor(s, &ctrl1);
if (ret != EC_SUCCESS) {
mutex_unlock(s->mutex);
return ret;
}
/* Set interrupt timer to 1 so it wakes up immediately. */
ret = raw_write8(data->accel_addr, KXCJ9_WAKEUP_TIMER, 1);
ret = raw_write8(s->i2c_addr, KXCJ9_WAKEUP_TIMER, 1);
if (ret != EC_SUCCESS)
goto error_enable_sensor;
@@ -315,7 +328,7 @@ static int accel_set_interrupt(void *drv_data, unsigned int threshold)
* first we need to divide by 16 to get the value to send.
*/
threshold >>= 4;
ret = raw_write8(data->accel_addr, KXCJ9_WAKEUP_THRESHOLD, threshold);
ret = raw_write8(s->i2c_addr, KXCJ9_WAKEUP_THRESHOLD, threshold);
if (ret != EC_SUCCESS)
goto error_enable_sensor;
@@ -324,11 +337,11 @@ static int accel_set_interrupt(void *drv_data, unsigned int threshold)
* function is called once, the interrupt stays enabled and it is
* only necessary to clear KXCJ9_INT_REL to allow the next interrupt.
*/
ret = raw_read8(data->accel_addr, KXCJ9_INT_CTRL1, &tmp);
ret = raw_read8(s->i2c_addr, KXCJ9_INT_CTRL1, &tmp);
if (ret != EC_SUCCESS)
goto error_enable_sensor;
if (!(tmp & KXCJ9_INT_CTRL1_IEN)) {
ret = raw_write8(data->accel_addr, KXCJ9_INT_CTRL1,
ret = raw_write8(s->i2c_addr, KXCJ9_INT_CTRL1,
tmp | KXCJ9_INT_CTRL1_IEN);
if (ret != EC_SUCCESS)
goto error_enable_sensor;
@@ -339,34 +352,34 @@ static int accel_set_interrupt(void *drv_data, unsigned int threshold)
* Note: this register latches motion detected above threshold. Once
* latched, no interrupt can occur until this register is cleared.
*/
ret = raw_read8(data->accel_addr, KXCJ9_INT_REL, &tmp);
ret = raw_read8(s->i2c_addr, KXCJ9_INT_REL, &tmp);
error_enable_sensor:
/* Re-enable the sensor. */
if (enable_sensor(data, ctrl1) != EC_SUCCESS)
return EC_ERROR_UNKNOWN;
if (enable_sensor(s, ctrl1) != EC_SUCCESS)
ret = EC_ERROR_UNKNOWN;
mutex_unlock(s->mutex);
return ret;
}
#endif
static int accel_read(void *drv_data,
int * const x_acc,
int * const y_acc,
int * const z_acc)
static int read(const struct motion_sensor_t *s,
int *x_acc,
int *y_acc,
int *z_acc)
{
uint8_t acc[6];
uint8_t reg = KXCJ9_XOUT_L;
int ret, multiplier;
struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data;
/* Read 6 bytes starting at KXCJ9_XOUT_L. */
mutex_lock(&data->accel_mutex);
mutex_lock(s->mutex);
i2c_lock(I2C_PORT_ACCEL, 1);
ret = i2c_xfer(I2C_PORT_ACCEL, data->accel_addr, &reg, 1, acc, 6,
ret = i2c_xfer(I2C_PORT_ACCEL, s->i2c_addr, &reg, 1, acc, 6,
I2C_XFER_SINGLE);
i2c_lock(I2C_PORT_ACCEL, 0);
mutex_unlock(&data->accel_mutex);
mutex_unlock(s->mutex);
if (ret != EC_SUCCESS)
return ret;
@@ -405,74 +418,42 @@ static int accel_read(void *drv_data,
return EC_SUCCESS;
}
static int accel_init(void *drv_data, int i2c_addr)
#ifdef CONFIG_ACCEL_INTERRUPTS
static int config_interrupt(const struct motion_sensor_t *s)
{
int ret = EC_SUCCESS;
int cnt = 0, ctrl1, ctrl2;
struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
if (data == NULL)
return EC_ERROR_INVAL;
memset(&data->accel_mutex, sizeof(struct mutex), 0);
data->sensor_range = 0;
data->sensor_datarate = 6;
data->sensor_resolution = 1;
data->accel_addr = i2c_addr;
int ctrl1;
mutex_lock(s->mutex);
/* Disable the sensor to allow for changing of critical parameters. */
ret = disable_sensor(data, &ctrl1);
ret = disable_sensor(s, &ctrl1);
if (ret != EC_SUCCESS)
return ret;
goto cleanup_exit;
/*
* This sensor can be powered through an EC reboot, so the state of
* the sensor is unknown here. Initiate software reset to restore
* sensor to default.
*/
ret = raw_write8(data->accel_addr, KXCJ9_CTRL2, KXCJ9_CTRL2_SRST);
if (ret != EC_SUCCESS)
return ret;
/* Wait until software reset is complete or timeout. */
while (1) {
ret = raw_read8(data->accel_addr, KXCJ9_CTRL2, &ctrl2);
/* Reset complete. */
if (ret == EC_SUCCESS && !(ctrl2 & KXCJ9_CTRL2_SRST))
break;
/* Check for timeout. */
if (cnt++ > 5)
return EC_ERROR_TIMEOUT;
/* Give more time for reset action to complete. */
msleep(10);
}
/* Set resolution and range. */
ctrl1 = resolutions[data->sensor_resolution].reg |
ranges[data->sensor_range].reg;
#ifdef CONFIG_ACCEL_INTERRUPTS
/* Enable wake up (motion detect) functionality. */
ctrl1 |= KXCJ9_CTRL1_WUFE;
#endif
ret = raw_write8(data->accel_addr, KXCJ9_CTRL1, ctrl1);
ret = raw_read8(s->i2c_addr, KXCJ9_CTRL1, &tmp);
tmp &= ~KXCJ9_CTRL1_PC1;
tmp |= KXCJ9_CTRL1_WUFE;
ret = raw_write8(s->i2c_addr, KXCJ9_CTRL1, tmp);
#ifdef CONFIG_ACCEL_INTERRUPTS
/* Set interrupt polarity to rising edge and keep interrupt disabled. */
ret |= raw_write8(data->accel_addr,
ret = raw_write8(s->i2c_addr,
KXCJ9_INT_CTRL1,
KXCJ9_INT_CTRL1_IEA);
if (ret != EC_SUCCESS)
goto cleanup_exit;
/* Set output data rate for wake-up interrupt function. */
ret |= raw_write8(data->accel_addr, KXCJ9_CTRL2, KXCJ9_OWUF_100_0HZ);
ret = raw_write8(s->i2c_addr, KXCJ9_CTRL2, KXCJ9_OWUF_100_0HZ);
if (ret != EC_SUCCESS)
goto cleanup_exit;
/* Set interrupt to trigger on motion on any axis. */
ret |= raw_write8(data->accel_addr, KXCJ9_INT_CTRL2,
ret = raw_write8(s->i2c_addr, KXCJ9_INT_CTRL2,
KXCJ9_INT_SRC2_XNWU | KXCJ9_INT_SRC2_XPWU |
KXCJ9_INT_SRC2_YNWU | KXCJ9_INT_SRC2_YPWU |
KXCJ9_INT_SRC2_ZNWU | KXCJ9_INT_SRC2_ZPWU);
if (ret != EC_SUCCESS)
goto cleanup_exit;
/*
* Enable accel interrupts. Note: accels will not initiate an interrupt
@@ -480,30 +461,85 @@ static int accel_init(void *drv_data, int i2c_addr)
*/
gpio_enable_interrupt(GPIO_ACCEL_INT_LID);
gpio_enable_interrupt(GPIO_ACCEL_INT_BASE);
#endif
/* Set output data rate. */
ret |= raw_write8(data->accel_addr, KXCJ9_DATA_CTRL,
datarates[data->sensor_datarate].reg);
/* Enable the sensor. */
ret |= enable_sensor(data, ctrl1);
ret = enable_sensor(s, ctrl1);
cleanup_exit:
mutex_unlock(s->mutex);
return ret;
}
#endif
static int init(const struct motion_sensor_t *s)
{
int ret = EC_SUCCESS;
int cnt = 0, tmp, range, rate;
/*
* This sensor can be powered through an EC reboot, so the state of
* the sensor is unknown here. Initiate software reset to restore
* sensor to default.
*/
mutex_lock(s->mutex);
ret = raw_write8(s->i2c_addr, KXCJ9_CTRL2, KXCJ9_CTRL2_SRST);
mutex_unlock(s->mutex);
if (ret != EC_SUCCESS)
return ret;
/* Wait until software reset is complete or timeout. */
do {
/* Added 1m delay after software reset */
msleep(1);
ret = raw_read8(s->i2c_addr, KXCJ9_CTRL2, &tmp);
if (ret != EC_SUCCESS)
return ret;
/* Reset complete. */
if (ret == EC_SUCCESS && !(tmp & KXCJ9_CTRL2_SRST))
break;
/* Check for timeout. */
if (cnt++ > 5) {
ret = EC_ERROR_TIMEOUT;
CPRINTF("%s: SRST Error.\n", s->name);
return ret;
}
} while (1);
ret = set_range(s, 2, 1);
if (ret != EC_SUCCESS)
return ret;
ret = set_resolution(s, 12, 1);
if (ret != EC_SUCCESS)
return ret;
ret = set_data_rate(s, 100000, 1);
if (ret != EC_SUCCESS)
return ret;
#ifdef CONFIG_ACCEL_INTERRUPTS
config_interrupt(s);
#endif
get_range(s, &range);
get_data_rate(s, &rate);
CPRINTF("[%T %s: Done Init type:0x%X range:%d rate:%d]\n",
s->name, s->type, range, rate);
return ret;
}
const struct accelgyro_info accel_kxcj9 = {
.chip_type = CHIP_KXCJ9,
.sensor_type = SENSOR_ACCELEROMETER,
.init = accel_init,
.read = accel_read,
.set_range = accel_set_range,
.get_range = accel_get_range,
.set_resolution = accel_set_resolution,
.get_resolution = accel_get_resolution,
.set_datarate = accel_set_datarate,
.get_datarate = accel_get_datarate,
const struct accelgyro_drv kxcj9_drv = {
.init = init,
.read = read,
.set_range = set_range,
.get_range = get_range,
.set_resolution = set_resolution,
.get_resolution = get_resolution,
.set_data_rate = set_data_rate,
.get_data_rate = get_data_rate,
#ifdef CONFIG_ACCEL_INTERRUPTS
.set_interrupt = accel_set_interrupt,
.set_interrupt = set_interrupt,
#endif
};

4
driver/accel_kxcj9.h
View File

@@ -88,6 +88,7 @@
#define KXCJ9_INT_CTRL2_XPWUE (1 << 4)
#define KXCJ9_INT_CTRL2_XNWUE (1 << 5)
#define KXCJ9_OSA_0_000HZ 0
#define KXCJ9_OSA_0_781HZ 8
#define KXCJ9_OSA_1_563HZ 9
#define KXCJ9_OSA_3_125HZ 0xa
@@ -102,7 +103,6 @@
#define KXCJ9_OSA_1600_HZ 7
struct kxcj9_data {
struct mutex accel_mutex;
/* Current range of accelerometer. */
int sensor_range;
/* Current output data rate of accelerometer. */
@@ -113,6 +113,6 @@ struct kxcj9_data {
int accel_addr;
};
extern const struct accelgyro_info accel_kxcj9;
extern const struct accelgyro_drv kxcj9_drv;
#endif /* __CROS_EC_ACCEL_KXCJ9_H */

453
driver/accelgyro_lsm6ds0.c
View File

@@ -3,7 +3,10 @@
* found in the LICENSE file.
*/
/* LSM6DS0 accelerometer and gyro module for Chrome EC */
/**
* LSM6DS0 accelerometer and gyro module for Chrome EC
* 3D digital accelerometer & 3D digital gyroscope
*/
#include "accelgyro.h"
#include "common.h"
@@ -14,63 +17,139 @@
#include "task.h"
#include "util.h"
#define CPUTS(outstr) cputs(CC_ACCEL, outstr)
#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
/*
* Struct for pairing an engineering value with the register value for a
* parameter.
*/
struct accel_param_pair {
int val; /* Value in engineering units. */
int reg; /* Corresponding register value. */
int reg_val; /* Corresponding register value. */
};
/* List of range values in +/-G's and their associated register values. */
static const struct accel_param_pair ranges[] = {
static const struct accel_param_pair g_ranges[] = {
{2, LSM6DS0_GSEL_2G},
{4, LSM6DS0_GSEL_4G},
{8, LSM6DS0_GSEL_8G}
};
/* List of ODR values in mHz and their associated register values. */
static const struct accel_param_pair datarates[] = {
/*
* List of angular rate range values in +/-dps's
* and their associated register values.
*/
const struct accel_param_pair dps_ranges[] = {
{245, LSM6DS0_DPS_SEL_245},
{500, LSM6DS0_DPS_SEL_500},
{1000, LSM6DS0_DPS_SEL_1000},
{2000, LSM6DS0_DPS_SEL_2000}
};
static inline const struct accel_param_pair *get_range_table(
enum sensor_type_t type, int *psize)
{
if (SENSOR_ACCELEROMETER == type) {
if (psize)
*psize = ARRAY_SIZE(g_ranges);
return g_ranges;
} else {
if (psize)
*psize = ARRAY_SIZE(dps_ranges);
return dps_ranges;
}
}
/* List of ODR (gyro off) values in mHz and their associated register values.*/
const struct accel_param_pair gyro_on_odr[] = {
{0, LSM6DS0_ODR_PD},
{15000, LSM6DS0_ODR_15HZ},
{59000, LSM6DS0_ODR_59HZ},
{119000, LSM6DS0_ODR_119HZ},
{238000, LSM6DS0_ODR_238HZ},
{476000, LSM6DS0_ODR_476HZ},
{952000, LSM6DS0_ODR_952HZ}
};
/* List of ODR (gyro on) values in mHz and their associated register values. */
const struct accel_param_pair gyro_off_odr[] = {
{0, LSM6DS0_ODR_PD},
{10000, LSM6DS0_ODR_10HZ},
{50000, LSM6DS0_ODR_50HZ},
{119000, LSM6DS0_ODR_119HZ},
{238000, LSM6DS0_ODR_238HZ},
{476000, LSM6DS0_ODR_476HZ},
{952000, LSM6DS0_ODR_982HZ}
{952000, LSM6DS0_ODR_952HZ}
};
static inline const struct accel_param_pair *get_odr_table(
enum sensor_type_t type, int *psize)
{
if (SENSOR_ACCELEROMETER == type) {
if (psize)
*psize = ARRAY_SIZE(gyro_off_odr);
return gyro_off_odr;
} else {
if (psize)
*psize = ARRAY_SIZE(gyro_on_odr);
return gyro_on_odr;
}
}
static inline int get_ctrl_reg(enum sensor_type_t type)
{
return (SENSOR_ACCELEROMETER == type) ?
LSM6DS0_CTRL_REG6_XL : LSM6DS0_CTRL_REG1_G;
}
static inline int get_xyz_reg(enum sensor_type_t type)
{
return (SENSOR_ACCELEROMETER == type) ?
LSM6DS0_OUT_X_L_XL : LSM6DS0_OUT_X_L_G;
}
/**
* Find index into a accel_param_pair that matches the given engineering value
* passed in. The round_up flag is used to specify whether to round up or down.
* Note, this function always returns a valid index. If the request is
* outside the range of values, it returns the closest valid index.
* @return reg value that matches the given engineering value passed in.
* The round_up flag is used to specify whether to round up or down.
* Note, this function always returns a valid reg value. If the request is
* outside the range of values, it returns the closest valid reg value.
*/
static int find_param_index(const int eng_val, const int round_up,
static int get_reg_val(const int eng_val, const int round_up,
const struct accel_param_pair *pairs, const int size)
{
int i;
/* Linear search for index to match. */
for (i = 0; i < size - 1; i++) {
if (eng_val <= pairs[i].val)
return i;
break;
if (eng_val < pairs[i+1].val) {
if (round_up)
return i + 1;
else
return i;
i += 1;
break;
}
}
return pairs[i].reg_val;
}
return i;
/**
* @return engineering value that matches the given reg val
*/
static int get_engineering_val(const int reg_val,
const struct accel_param_pair *pairs, const int size)
{
int i;
for (i = 0; i < size; i++) {
if (reg_val == pairs[i].reg_val)
break;
}
return pairs[i].val;
}
/**
* Read register from accelerometer.
*/
static int raw_read8(const int addr, const int reg, int *data_ptr)
static inline int raw_read8(const int addr, const int reg, int *data_ptr)
{
return i2c_read8(I2C_PORT_ACCEL, addr, reg, data_ptr);
}
@@ -78,107 +157,139 @@ static int raw_read8(const int addr, const int reg, int *data_ptr)
/**
* Write register from accelerometer.
*/
static int raw_write8(const int addr, const int reg, int data)
static inline int raw_write8(const int addr, const int reg, int data)
{
return i2c_write8(I2C_PORT_ACCEL, addr, reg, data);
}
static int accel_set_range(void *drv_data,
const int range,
const int rnd)
static int set_range(const struct motion_sensor_t *s,
int range,
int rnd)
{
int ret, index, ctrl_reg6;
struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data;
int ret, ctrl_val, range_tbl_size;
uint8_t ctrl_reg, reg_val;
const struct accel_param_pair *ranges;
/* Find index for interface pair matching the specified range. */
index = find_param_index(range, rnd, ranges, ARRAY_SIZE(ranges));
ctrl_reg = get_ctrl_reg(s->type);
ranges = get_range_table(s->type, &range_tbl_size);
reg_val = get_reg_val(range, rnd, ranges, range_tbl_size);
/*
* Lock accel resource to prevent another task from attempting
* to write accel parameters until we are done.
*/
mutex_lock(&data->accel_mutex);
mutex_lock(s->mutex);
ret = raw_read8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, &ctrl_reg6);
ret = raw_read8(s->i2c_addr, ctrl_reg, &ctrl_val);
if (ret != EC_SUCCESS)
goto accel_cleanup;
ctrl_reg6 = (ctrl_reg6 & ~LSM6DS0_GSEL_ALL) | ranges[index].reg;
ret = raw_write8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, ctrl_reg6);
ctrl_val = (ctrl_val & ~LSM6DS0_RANGE_MASK) | reg_val;
ret = raw_write8(s->i2c_addr, ctrl_reg, ctrl_val);
accel_cleanup:
/* Unlock accel resource and save new range if written successfully. */
mutex_unlock(&data->accel_mutex);
if (ret == EC_SUCCESS)
data->sensor_range = index;
mutex_unlock(s->mutex);
return EC_SUCCESS;
}
static int accel_get_range(void *drv_data, int * const range)
static int get_range(const struct motion_sensor_t *s,
int *range)
{
struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data;
*range = ranges[data->sensor_range].val;
return EC_SUCCESS;
int ret, ctrl_val, range_tbl_size;
uint8_t ctrl_reg;
const struct accel_param_pair *ranges;
ranges = get_range_table(s->type, &range_tbl_size);
ctrl_reg = get_ctrl_reg(s->type);
ret = raw_read8(s->i2c_addr, ctrl_reg, &ctrl_val);
*range = get_engineering_val(ctrl_val & LSM6DS0_RANGE_MASK,
ranges, range_tbl_size);
return ret;
}
static int accel_set_resolution(void *drv_data,
const int res,
const int rnd)
static int set_resolution(const struct motion_sensor_t *s,
int res,
int rnd)
{
/* Only one resolution, LSM6DS0_RESOLUTION, so nothing to do. */
return EC_SUCCESS;
}
static int accel_get_resolution(void *drv_data,
int * const res)
static int get_resolution(const struct motion_sensor_t *s,
int *res)
{
*res = LSM6DS0_RESOLUTION;
return EC_SUCCESS;
}
static int accel_set_datarate(void *drv_data,
const int rate,
const int rnd)
static int set_data_rate(const struct motion_sensor_t *s,
int rate,
int rnd)
{
int ret, index, ctrl_reg6;
struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data;
int ret, val, odr_tbl_size;
uint8_t ctrl_reg, reg_val;
const struct accel_param_pair *data_rates;
/* Find index for interface pair matching the specified range. */
index = find_param_index(rate, rnd, datarates, ARRAY_SIZE(datarates));
ctrl_reg = get_ctrl_reg(s->type);
data_rates = get_odr_table(s->type, &odr_tbl_size);
reg_val = get_reg_val(rate, rnd, data_rates, odr_tbl_size);
/*
* Lock accel resource to prevent another task from attempting
* to write accel parameters until we are done.
*/
mutex_lock(&data->accel_mutex);
mutex_lock(s->mutex);
ret = raw_read8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, &ctrl_reg6);
ret = raw_read8(s->i2c_addr, ctrl_reg, &val);
if (ret != EC_SUCCESS)
goto accel_cleanup;
ctrl_reg6 = (ctrl_reg6 & ~LSM6DS0_ODR_ALL) | datarates[index].reg;
ret = raw_write8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, ctrl_reg6);
val = (val & ~LSM6DS0_ODR_MASK) | reg_val;
ret = raw_write8(s->i2c_addr, ctrl_reg, val);
/* CTRL_REG3_G 12h
* [7] low-power mode = 0;
* [6] high pass filter disabled;
* [5:4] 0 keep const 0
* [3:0] HPCF_G
* Table 48 Gyroscope high-pass filter cutoff frequency
*/
if (SENSOR_GYRO == s->type) {
ret = raw_read8(s->i2c_addr, LSM6DS0_CTRL_REG3_G, &val);
if (ret != EC_SUCCESS)
goto accel_cleanup;
val &= ~(0x3 << 4); /* clear bit [5:4] */
val = (rate > 119000) ?
(val | (1<<7)) /* set high-power mode */ :
(val & ~(1<<7)); /* set low-power mode */
ret = raw_write8(s->i2c_addr, LSM6DS0_CTRL_REG3_G, val);
}
accel_cleanup:
/* Unlock accel resource and save new ODR if written successfully. */
mutex_unlock(&data->accel_mutex);
if (ret == EC_SUCCESS)
data->sensor_datarate = index;
mutex_unlock(s->mutex);
return EC_SUCCESS;
}
static int accel_get_datarate(void *drv_data,
int * const rate)
static int get_data_rate(const struct motion_sensor_t *s,
int *rate)
{
struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data;
*rate = datarates[data->sensor_datarate].val;
int ret, ctrl_val, odr_tbl_size;
uint8_t ctrl_reg;
const struct accel_param_pair *data_rates;
ctrl_reg = get_ctrl_reg(s->type);
ret = raw_read8(s->i2c_addr, ctrl_reg, &ctrl_val);
if (ret != EC_SUCCESS)
return EC_ERROR_UNKNOWN;
data_rates = get_odr_table(s->type, &odr_tbl_size);
*rate = get_engineering_val(ctrl_val & LSM6DS0_ODR_MASK,
data_rates, odr_tbl_size);
return EC_SUCCESS;
}
#ifdef CONFIG_ACCEL_INTERRUPTS
static int accel_set_interrupt(void *drv_data,
static int set_interrupt(const struct motion_sensor_t *s,
unsigned int threshold)
{
/* Currently unsupported. */
@@ -186,103 +297,169 @@ static int accel_set_interrupt(void *drv_data,
}
#endif
static int accel_read(void *drv_data,
int * const x_acc,
int * const y_acc,
int * const z_acc)
static int is_data_ready(const struct motion_sensor_t *s, int *ready)
{
uint8_t acc[6];
uint8_t reg = LSM6DS0_OUT_X_L_XL;
int ret, multiplier;
struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data;
int ret, tmp;
/* Read 6 bytes starting at LSM6DS0_OUT_X_L_XL. */
mutex_lock(&data->accel_mutex);
i2c_lock(I2C_PORT_ACCEL, 1);
ret = i2c_xfer(I2C_PORT_ACCEL, data->accel_addr, &reg, 1, acc, 6,
I2C_XFER_SINGLE);
i2c_lock(I2C_PORT_ACCEL, 0);
mutex_unlock(&data->accel_mutex);
ret = raw_read8(s->i2c_addr, LSM6DS0_STATUS_REG, &tmp);
if (ret != EC_SUCCESS)
if (ret != EC_SUCCESS) {
CPRINTF("[%T %s type:0x%X RS Error]", s->name, s->type);
return ret;
/* Determine multiplier based on stored range. */
switch (ranges[data->sensor_range].reg) {
case LSM6DS0_GSEL_2G:
multiplier = 1;
break;
case LSM6DS0_GSEL_4G:
multiplier = 2;
break;
case LSM6DS0_GSEL_8G:
multiplier = 4;
break;
default:
return EC_ERROR_UNKNOWN;
}
/*
* Convert data to signed 12-bit value. Note order of registers:
*
* acc[0] = LSM6DS0_OUT_X_L_XL
* acc[1] = LSM6DS0_OUT_X_H_XL
* acc[2] = LSM6DS0_OUT_Y_L_XL
* acc[3] = LSM6DS0_OUT_Y_H_XL
* acc[4] = LSM6DS0_OUT_Z_L_XL
* acc[5] = LSM6DS0_OUT_Z_H_XL
*/
*x_acc = multiplier * ((int16_t)(acc[1] << 8 | acc[0])) >> 4;
*y_acc = multiplier * ((int16_t)(acc[3] << 8 | acc[2])) >> 4;
*z_acc = multiplier * ((int16_t)(acc[5] << 8 | acc[4])) >> 4;
if (SENSOR_ACCELEROMETER == s->type)
*ready = (LSM6DS0_STS_XLDA_UP == (tmp & LSM6DS0_STS_XLDA_MASK));
else
*ready = (LSM6DS0_STS_GDA_UP == (tmp & LSM6DS0_STS_GDA_MASK));
return EC_SUCCESS;
}
static int accel_init(void *drv_data, int i2c_addr)
static int read(const struct motion_sensor_t *s,
int *x,
int *y,
int *z)
{
int ret, ctrl_reg6;
struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data;
uint8_t data[6];
uint8_t xyz_reg;
int ret, tmp = 0, range = 0;
if (data == NULL)
return EC_ERROR_INVAL;
ret = is_data_ready(s, &tmp);
if (ret != EC_SUCCESS)
return ret;
memset(&data->accel_mutex, sizeof(struct mutex), 0);
data->sensor_range = 0;
data->sensor_datarate = 1;
data->accel_addr = i2c_addr;
/*
* If sensor data is not ready, return the previous read data.
* Note: return success so that motion senor task can read again
* 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];
return EC_SUCCESS;
}
xyz_reg = get_xyz_reg(s->type);
/* Read 6 bytes starting at xyz_reg */
i2c_lock(I2C_PORT_ACCEL, 1);
ret = i2c_xfer(I2C_PORT_ACCEL, s->i2c_addr,
&xyz_reg, 1, data, 6, I2C_XFER_SINGLE);
i2c_lock(I2C_PORT_ACCEL, 0);
if (ret != EC_SUCCESS) {
CPRINTF("[%T %s type:0x%X RD XYZ Error]",
s->name, s->type);
return ret;
}
*x = ((int16_t)((data[1] << 8) | data[0]));
*y = ((int16_t)((data[3] << 8) | data[2]));
*z = ((int16_t)((data[5] << 8) | data[4]));
ret = get_range(s, &range);
if (ret)
return EC_ERROR_UNKNOWN;
*x *= range;
*y *= range;
*z *= range;
/* normalize the accel scale: 1G = 1024 */
if (SENSOR_ACCELEROMETER == s->type) {
*x >>= 5;
*y >>= 5;
*z >>= 5;
} else {
*x >>= 8;
*y >>= 8;
*z >>= 8;
}
return EC_SUCCESS;
}
static int init(const struct motion_sensor_t *s)
{
int ret = 0, tmp;
ret = raw_read8(s->i2c_addr, LSM6DS0_WHO_AM_I_REG, &tmp);
if (ret)
return EC_ERROR_UNKNOWN;
if (tmp != LSM6DS0_WHO_AM_I)
return EC_ERROR_ACCESS_DENIED;
/*
* This sensor can be powered through an EC reboot, so the state of
* the sensor is unknown here. Initiate software reset to restore
* sensor to default.
* [6] BDU Enable Block Data Update.
* [0] SW_RESET software reset
*
* lsm6ds0 supports both accel & gyro features
* Board will see two virtual sensor devices: accel & gyro.
* Requirement: Accel need be init before gyro.
* SW_RESET is down for accel only!
*/
ret = raw_write8(data->accel_addr, LSM6DS0_CTRL_REG8, 1);
if (ret != EC_SUCCESS)
goto accel_cleanup;
if (SENSOR_ACCELEROMETER == s->type) {
/* Set ODR and range. */
ctrl_reg6 = datarates[data->sensor_datarate].reg |
ranges[data->sensor_range].reg;
mutex_lock(s->mutex);
ret = raw_read8(s->i2c_addr, LSM6DS0_CTRL_REG8, &tmp);
if (ret) {
mutex_unlock(s->mutex);
return EC_ERROR_UNKNOWN;
}
tmp |= (1 | LSM6DS0_BDU_ENABLE);
ret = raw_write8(s->i2c_addr, LSM6DS0_CTRL_REG8, tmp);
mutex_unlock(s->mutex);
ret = raw_write8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, ctrl_reg6);
if (ret)
return EC_ERROR_UNKNOWN;
accel_cleanup:
/* Power Down Gyro */
ret = raw_write8(s->i2c_addr,
LSM6DS0_CTRL_REG1_G, 0x0);
if (ret)
return EC_ERROR_UNKNOWN;
ret = set_range(s, 2, 1);
if (ret)
return EC_ERROR_UNKNOWN;
ret = set_data_rate(s, 119000, 1);
if (ret)
return EC_ERROR_UNKNOWN;
}
if (SENSOR_GYRO == s->type) {
/* Config GYRO Range */
ret = set_range(s, 2000, 1);
if (ret)
return EC_ERROR_UNKNOWN;
/* Config ACCEL & GYRO ODR */
ret = set_data_rate(s, 119000, 1);
if (ret)
return EC_ERROR_UNKNOWN;
}
CPRINTF("[%T %s: Done Init type:0x%X]", s->name, s->type);
return ret;
}
const struct accelgyro_info accel_lsm6ds0 = {
.chip_type = CHIP_LSM6DS0,
.sensor_type = SENSOR_ACCELEROMETER,
.init = accel_init,
.read = accel_read,
.set_range = accel_set_range,
.get_range = accel_get_range,
.set_resolution = accel_set_resolution,
.get_resolution = accel_get_resolution,
.set_datarate = accel_set_datarate,
.get_datarate = accel_get_datarate,
const struct accelgyro_drv lsm6ds0_drv = {
.init = init,
.read = read,
.set_range = set_range,
.get_range = get_range,
.set_resolution = set_resolution,
.get_resolution = get_resolution,
.set_data_rate = set_data_rate,
.get_data_rate = get_data_rate,
#ifdef CONFIG_ACCEL_INTERRUPTS
.set_interrupt = accel_set_interrupt,
.set_interrupt = set_interrupt,
#endif
};

92
driver/accelgyro_lsm6ds0.h
View File

@@ -17,43 +17,105 @@
#define LSM6DS0_ADDR0 0xd4
#define LSM6DS0_ADDR1 0xd6
/* who am I */
#define LSM6DS0_WHO_AM_I 0x68
/* Chip specific registers. */
#define LSM6DS0_ACT_THS 0x04
#define LSM6DS0_ACT_DUR 0x05
#define LSM6DS0_INT_GEN_CFG_XL 0x06
#define LSM6DS0_INT_GEN_THS_X_XL 0x07
#define LSM6DS0_INT_GEN_THS_Y_XL 0x08
#define LSM6DS0_INT_GEN_THS_Z_XL 0x09
#define LSM6DS0_INT_GEN_DUR_XL 0x0a
#define LSM6DS0_REFERENCE_G 0x0b
#define LSM6DS0_INT_CTRL 0x0c
#define LSM6DS0_WHO_AM_I_REG 0x0f
#define LSM6DS0_CTRL_REG1_G 0x10
#define LSM6DS0_CTRL_REG2_G 0x11
#define LSM6DS0_CTRL_REG3_G 0x12
#define LSM6DS0_ORIENT_CFG_G 0x13
#define LSM6DS0_INT_GEN_SRC_G 0x14
#define LSM6DS0_OUT_TEMP_L 0x15
#define LSM6DS0_OUT_TEMP_H 0x16
#define LSM6DS0_OUT_X_L_G 0x18
#define LSM6DS0_OUT_X_H_G 0x19
#define LSM6DS0_OUT_Y_L_G 0x1a
#define LSM6DS0_OUT_Y_H_G 0x1b
#define LSM6DS0_OUT_Z_L_G 0x1c
#define LSM6DS0_OUT_Z_H_G 0x1d
#define LSM6DS0_CTRL_REG4 0x1e
#define LSM6DS0_CTRL_REG5_XL 0x1f
#define LSM6DS0_CTRL_REG6_XL 0x20
#define LSM6DS0_CTRL_REG7_XL 0x21
#define LSM6DS0_CTRL_REG8 0x22
#define LSM6DS0_CTRL_REG9 0x23
#define LSM6DS0_CTRL_REG10 0x24
#define LSM6DS0_INT_GEN_SRC_XL 0x26
#define LSM6DS0_STATUS_REG 0x27
#define LSM6DS0_OUT_X_L_XL 0x28
#define LSM6DS0_OUT_X_H_XL 0x29
#define LSM6DS0_OUT_Y_L_XL 0x2a
#define LSM6DS0_OUT_Y_H_XL 0x2b
#define LSM6DS0_OUT_Z_L_XL 0x2c
#define LSM6DS0_OUT_Z_H_XL 0x2d
#define LSM6DS0_FIFO_CTRL 0x2e
#define LSM6DS0_FIFO_SRC 0x2f
#define LSM6DS0_INT_GEN_CFG_G 0x30
#define LSM6DS0_INT_GEN_THS_XH_G 0x31
#define LSM6DS0_INT_GEN_THS_XL_G 0x32
#define LSM6DS0_INT_GEN_THS_YH_G 0x33
#define LSM6DS0_INT_GEN_THS_YL_G 0x34
#define LSM6DS0_INT_GEN_THS_ZH_G 0x35
#define LSM6DS0_INT_GEN_THS_ZL_G 0x36
#define LSM6DS0_INT_GEN_DUR_G 0x37
#define LSM6DS0_DPS_SEL_245 (0 << 3)
#define LSM6DS0_DPS_SEL_500 (1 << 3)
#define LSM6DS0_DPS_SEL_1000 (2 << 3)
#define LSM6DS0_DPS_SEL_2000 (3 << 3)
#define LSM6DS0_GSEL_2G (0 << 3)
#define LSM6DS0_GSEL_4G (2 << 3)
#define LSM6DS0_GSEL_8G (3 << 3)
#define LSM6DS0_GSEL_ALL (3 << 3)
#define LSM6DS0_RANGE_MASK (3 << 3)
#define LSM6DS0_ODR_PD (0 << 5)
#define LSM6DS0_ODR_10HZ (1 << 5)
#define LSM6DS0_ODR_15HZ (1 << 5)
#define LSM6DS0_ODR_50HZ (2 << 5)
#define LSM6DS0_ODR_59HZ (2 << 5)
#define LSM6DS0_ODR_119HZ (3 << 5)
#define LSM6DS0_ODR_238HZ (4 << 5)
#define LSM6DS0_ODR_476HZ (5 << 5)
#define LSM6DS0_ODR_982HZ (6 << 5)
#define LSM6DS0_ODR_ALL (7 << 5)
#define LSM6DS0_ODR_952HZ (6 << 5)
#define LSM6DS0_ODR_MASK (7 << 5)
/*
* Register : STATUS_REG
* Address : 0X27
*/
enum lsm6ds0_status {
LSM6DS0_STS_DOWN = 0x00,
LSM6DS0_STS_XLDA_UP = 0x01,
LSM6DS0_STS_GDA_UP = 0x02,
};
#define LSM6DS0_STS_XLDA_MASK 0x01
#define LSM6DS0_STS_GDA_MASK 0x02
/*
* Register : CTRL_REG8
* Address : 0X22
* Bit Group Name: BDU
*/
enum lsm6ds0_bdu {
LSM6DS0_BDU_DISABLE = 0x00,
LSM6DS0_BDU_ENABLE = 0x40,
};
/* Sensor resolution in number of bits. This sensor has fixed resolution. */
#define LSM6DS0_RESOLUTION 16
struct lsm6ds0_data {
struct mutex accel_mutex;
/* Current range of accelerometer. */
int sensor_range;
/* Current output data rate of accelerometer. */
int sensor_datarate;
/* Device address. */
int accel_addr;
};
extern const struct accelgyro_info accel_lsm6ds0;
extern const struct accelgyro_drv lsm6ds0_drv;
#endif /* __CROS_EC_ACCEL_LSM6DS0_H */

93
include/accelgyro.h
View File

@@ -6,104 +6,91 @@
#ifndef __CROS_EC_ACCELGYRO_H
#define __CROS_EC_ACCELGYRO_H
#include "motion_sense.h"
/* Header file for accelerometer / gyro drivers. */
/* Number of counts from accelerometer that represents 1G acceleration. */
#define ACCEL_G 1024
enum accelgyro_chip_t {
CHIP_TEST,
CHIP_KXCJ9,
CHIP_LSM6DS0,
};
enum accelgyro_sensor_t {
SENSOR_ACCELEROMETER,
SENSOR_GYRO,
};
struct accelgyro_info {
enum accelgyro_chip_t chip_type;
enum accelgyro_sensor_t sensor_type;
struct accelgyro_drv {
/**
* Initialize accelerometers.
* @param drv_data Pointer to sensor data.
* @i2c_addr i2c slave device address
* @s Pointer to sensor data pointer. Sensor data will be
* allocated on success.
* @return EC_SUCCESS if successful, non-zero if error.
*/
int (*init)(void *drv_data,
int i2c_addr);
int (*init)(const struct motion_sensor_t *s);
/**
* Read all three accelerations of an accelerometer. Note that all
* three accelerations come back in counts, where ACCEL_G can be used
* to convert counts to engineering units.
* @param drv_data Pointer to sensor data.
* @param x_acc Pointer to store X-axis acceleration (in counts).
* @param y_acc Pointer to store Y-axis acceleration (in counts).
* @param z_acc Pointer to store Z-axis acceleration (in counts).
* @s Pointer to sensor data.
* @x_acc Pointer to store X-axis acceleration (in counts).
* @y_acc Pointer to store Y-axis acceleration (in counts).
* @z_acc Pointer to store Z-axis acceleration (in counts).
* @return EC_SUCCESS if successful, non-zero if error.
*/
int (*read)(void *drv_data,
int * const x_acc,
int * const y_acc,
int * const z_acc);
int (*read)(const struct motion_sensor_t *s,
int *x_acc,
int *y_acc,
int *z_acc);
/**
* Setter and getter methods for the sensor range. The sensor range
* defines the maximum value that can be returned from read(). As the
* range increases, the resolution gets worse.
* @param drv_data Pointer to sensor data.
* @param range Range (Units are +/- G's for accel, +/- deg/s for gyro)
* @param rnd Rounding flag. If true, it rounds up to nearest valid
* @s Pointer to sensor data.
* @range Range (Units are +/- G's for accel, +/- deg/s for gyro)
* @rnd Rounding flag. If true, it rounds up to nearest valid
* value. Otherwise, it rounds down.
* @return EC_SUCCESS if successful, non-zero if error.
*/
int (*set_range)(void *drv_data,
const int range,
const int rnd);
int (*get_range)(void *drv_data,
int * const range);
int (*set_range)(const struct motion_sensor_t *s,
int range,
int rnd);
int (*get_range)(const struct motion_sensor_t *s,
int *range);
/**
* Setter and getter methods for the sensor resolution.
* @param drv_data Pointer to sensor data.
* @param range Resolution (Units are number of bits)
* @s Pointer to sensor data.
* @range Resolution (Units are number of bits)
* param rnd Rounding flag. If true, it rounds up to nearest valid
* value. Otherwise, it rounds down.
* @return EC_SUCCESS if successful, non-zero if error.
*/
int (*set_resolution)(void *drv_data,
const int res,
const int rnd);
int (*get_resolution)(void *drv_data,
int * const res);
int (*set_resolution)(const struct motion_sensor_t *s,
int res,
int rnd);
int (*get_resolution)(const struct motion_sensor_t *s,
int *res);
/**
* Setter and getter methods for the sensor output data range. As the
* ODR increases, the LPF roll-off frequency also increases.
* @param drv_data Pointer to sensor data.
* @param rate Output data rate (units are mHz)
* @param rnd Rounding flag. If true, it rounds up to nearest valid
* @s Pointer to sensor data.
* @rate Output data rate (units are Hz)
* @rnd Rounding flag. If true, it rounds up to nearest valid
* value. Otherwise, it rounds down.
* @return EC_SUCCESS if successful, non-zero if error.
*/
int (*set_datarate)(void *drv_data,
const int rate,
const int rnd);
int (*get_datarate)(void *drv_data,
int * const rate);
int (*set_data_rate)(const struct motion_sensor_t *s,
int rate,
int rnd);
int (*get_data_rate)(const struct motion_sensor_t *s,
int *rate);
#ifdef CONFIG_ACCEL_INTERRUPTS
/**
* Setup a one-time accel interrupt. If the threshold is low enough, the
* interrupt may trigger due simply to noise and not any real motion.
* If the threshold is 0, the interrupt will fire immediately.
* @param drv_data Pointer to sensor data.
* @param threshold Threshold for interrupt in units of counts.
* @s Pointer to sensor data.
* @threshold Threshold for interrupt in units of counts.
*/
int (*set_interrupt)(void *drv_data,
int (*set_interrupt)(const struct motion_sensor_t *s,
unsigned int threshold);
#endif
};

8
include/math_util.h
View File

@@ -40,11 +40,11 @@ float cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2);
* Rotate vector v by rotation matrix R.
*
* @param v Vector to be rotated.
* @param R Pointer to rotation matrix.
* @param res Pointer to the resultant vector.
* @param R Rotation matrix.
* @param res Resultant vector.
*/
void rotate(const vector_3_t v, const matrix_3x3_t (* const R),
vector_3_t *res);
void rotate(const vector_3_t v, const matrix_3x3_t R,
vector_3_t res);
#ifdef CONFIG_ACCEL_CALIBRATE

39
include/motion_sense.h
View File

@@ -93,20 +93,51 @@ void accel_int_lid(enum gpio_signal signal);
void accel_int_base(enum gpio_signal signal);
enum sensor_location_t {
LOCATION_BASE,
LOCATION_LID,
LOCATION_BASE = 0,
LOCATION_LID = 1,
};
enum sensor_type_t {
SENSOR_ACCELEROMETER = 0x1,
SENSOR_GYRO = 0x2,
};
enum sensor_chip_t {
SENSOR_CHIP_KXCJ9 = 0,
SENSOR_CHIP_LSM6DS0 = 1,
};
enum sensor_state {
SENSOR_NOT_INITIALIZED = 0,
SENSOR_INITIALIZED = 1,
SENSOR_INIT_ERROR = 2
};
enum sensor_power {
SENSOR_POWER_OFF = 0,
SENSOR_POWER_ON = 1
};
struct motion_sensor_t {
/* RO fields */
char *name;
enum sensor_chip_t chip;
enum sensor_type_t type;
enum sensor_location_t location;
const struct accelgyro_info *drv;
const struct accelgyro_drv *drv;
struct mutex *mutex;
void *drv_data;
uint8_t i2c_addr;
/* RW fields */
enum sensor_state state;
enum sensor_power power;
vector_3_t raw_xyz;
vector_3_t xyz;
};
/* Defined at board level. */
extern const struct motion_sensor_t motion_sensors[];
extern struct motion_sensor_t motion_sensors[];
extern const unsigned int motion_sensor_count;
#endif /* __CROS_EC_MOTION_SENSE_H */

5
test/math_util.c
View File

@@ -6,7 +6,7 @@
*/
#include <math.h>
#include <stdio.h>
#include "math_util.h"
#include "motion_sense.h"
#include "test_util.h"
@@ -14,7 +14,7 @@
/*****************************************************************************/
/* Need to define motion sensor globals just to compile. */
const struct motion_sensor_t motion_sensors[] = {};
struct motion_sensor_t motion_sensors[] = {};
const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);
/*****************************************************************************/
@@ -41,7 +41,6 @@ static int test_acos(void)
return EC_SUCCESS;
}
void run_test(void)
{
test_reset();

121
test/motion_sense.c
View File

@@ -9,6 +9,7 @@
#include "accelgyro.h"
#include "common.h"
#include "hooks.h"
#include "host_command.h"
#include "motion_sense.h"
#include "task.h"
@@ -16,105 +17,82 @@
#include "timer.h"
#include "util.h"
/* Mock acceleration values for motion sense task to read in. */
int mock_x_acc[ACCEL_COUNT], mock_y_acc[ACCEL_COUNT], mock_z_acc[ACCEL_COUNT];
/* For vector_3_t, define which coordinates are in which location. */
enum {
X, Y, Z
};
/*****************************************************************************/
/* Mock functions */
static int accel_init(void *drv_data, int i2c_addr)
static int accel_init(const struct motion_sensor_t *s)
{
return EC_SUCCESS;
}
static int accel_read_base(void *drv_data, int *x_acc, int *y_acc, int *z_acc)
static int accel_read(const struct motion_sensor_t *s,
int *x_acc, int *y_acc, int *z_acc)
{
/* Return the mock values. */
*x_acc = mock_x_acc[ACCEL_BASE];
*y_acc = mock_y_acc[ACCEL_BASE];
*z_acc = mock_z_acc[ACCEL_BASE];
*x_acc = s->xyz[X];
*y_acc = s->xyz[Y];
*z_acc = s->xyz[Z];
return EC_SUCCESS;
}
static int accel_read_lid(void *drv_data, int *x_acc, int *y_acc, int *z_acc)
{
/* Return the mock values. */
*x_acc = mock_x_acc[ACCEL_LID];
*y_acc = mock_y_acc[ACCEL_LID];
*z_acc = mock_z_acc[ACCEL_LID];
return EC_SUCCESS;
}
static int accel_set_range(void *drv_data,
static int accel_set_range(const struct motion_sensor_t *s,
const int range,
const int rnd)
{
return EC_SUCCESS;
}
static int accel_get_range(void *drv_data,
static int accel_get_range(const struct motion_sensor_t *s,
int * const range)
{
return EC_SUCCESS;
}
static int accel_set_resolution(void *drv_data,
static int accel_set_resolution(const struct motion_sensor_t *s,
const int res,
const int rnd)
{
return EC_SUCCESS;
}
static int accel_get_resolution(void *drv_data,
static int accel_get_resolution(const struct motion_sensor_t *s,
int * const res)
{
return EC_SUCCESS;
}
static int accel_set_datarate(void *drv_data,
static int accel_set_data_rate(const struct motion_sensor_t *s,
const int rate,
const int rnd)
{
return EC_SUCCESS;
}
static int accel_get_datarate(void *drv_data,
static int accel_get_data_rate(const struct motion_sensor_t *s,
int * const rate)
{
return EC_SUCCESS;
}
const struct accelgyro_info test_motion_sense_base = {
.chip_type = CHIP_TEST,
.sensor_type = SENSOR_ACCELEROMETER,
const struct accelgyro_drv test_motion_sense = {
.init = accel_init,
.read = accel_read_base,
.read = accel_read,
.set_range = accel_set_range,
.get_range = accel_get_range,
.set_resolution = accel_set_resolution,
.get_resolution = accel_get_resolution,
.set_datarate = accel_set_datarate,
.get_datarate = accel_get_datarate,
.set_data_rate = accel_set_data_rate,
.get_data_rate = accel_get_data_rate,
};
const struct accelgyro_info test_motion_sense_lid = {
.chip_type = CHIP_TEST,
.sensor_type = SENSOR_ACCELEROMETER,
.init = accel_init,
.read = accel_read_lid,
.set_range = accel_set_range,
.get_range = accel_get_range,
.set_resolution = accel_set_resolution,
.get_resolution = accel_get_resolution,
.set_datarate = accel_set_datarate,
.get_datarate = accel_get_datarate,
};
const struct motion_sensor_t motion_sensors[] = {
{"test base sensor", LOCATION_BASE, &test_motion_sense_base, NULL, 0},
{"test lid sensor", LOCATION_LID, &test_motion_sense_lid, NULL, 0},
struct motion_sensor_t motion_sensors[] = {
{"base", SENSOR_CHIP_LSM6DS0, SENSOR_ACCELEROMETER, LOCATION_BASE,
&test_motion_sense, NULL, NULL, 0},
{"lid", SENSOR_CHIP_KXCJ9, SENSOR_ACCELEROMETER, LOCATION_LID,
&test_motion_sense, NULL, NULL, 0},
};
const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);
@@ -125,16 +103,21 @@ static int test_lid_angle(void)
uint8_t *lpc_status = host_get_memmap(EC_MEMMAP_ACC_STATUS);
uint8_t sample;
struct motion_sensor_t *base = &motion_sensors[0];
struct motion_sensor_t *lid = &motion_sensors[1];
hook_notify(HOOK_CHIPSET_STARTUP);
/*
* Set the base accelerometer as if it were sitting flat on a desk
* and set the lid to closed.
*/
mock_x_acc[ACCEL_BASE] = 0;
mock_y_acc[ACCEL_BASE] = 0;
mock_z_acc[ACCEL_BASE] = 1000;
mock_x_acc[ACCEL_LID] = 0;
mock_y_acc[ACCEL_LID] = 0;
mock_z_acc[ACCEL_LID] = 1000;
base->xyz[X] = 0;
base->xyz[Y] = 0;
base->xyz[Z] = 1000;
lid->xyz[X] = 0;
lid->xyz[Y] = 0;
lid->xyz[Z] = 1000;
sample = *lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK;
task_wake(TASK_ID_MOTIONSENSE);
while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample)
@@ -142,9 +125,9 @@ static int test_lid_angle(void)
TEST_ASSERT(motion_get_lid_angle() == 0);
/* Set lid open to 90 degrees. */
mock_x_acc[ACCEL_LID] = -1000;
mock_y_acc[ACCEL_LID] = 0;
mock_z_acc[ACCEL_LID] = 0;
lid->xyz[X] = -1000;
lid->xyz[Y] = 0;
lid->xyz[Z] = 0;
sample = *lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK;
task_wake(TASK_ID_MOTIONSENSE);
while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample)
@@ -152,9 +135,9 @@ static int test_lid_angle(void)
TEST_ASSERT(motion_get_lid_angle() == 90);
/* Set lid open to 225. */
mock_x_acc[ACCEL_LID] = 500;
mock_y_acc[ACCEL_LID] = 0;
mock_z_acc[ACCEL_LID] = -500;
lid->xyz[X] = 500;
lid->xyz[Y] = 0;
lid->xyz[Z] = -500;
sample = *lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK;
task_wake(TASK_ID_MOTIONSENSE);
while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample)
@@ -165,9 +148,9 @@ static int test_lid_angle(void)
* Align base with hinge and make sure it returns unreliable for angle.
* In this test it doesn't matter what the lid acceleration vector is.
*/
mock_x_acc[ACCEL_BASE] = 0;
mock_y_acc[ACCEL_BASE] = 1000;
mock_z_acc[ACCEL_BASE] = 0;
base->xyz[X] = 0;
base->xyz[Y] = 1000;
base->xyz[Z] = 0;
sample = *lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK;
task_wake(TASK_ID_MOTIONSENSE);
while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample)
@@ -178,12 +161,12 @@ static int test_lid_angle(void)
* Use all three axes and set lid to negative base and make sure
* angle is 180.
*/
mock_x_acc[ACCEL_BASE] = 500;
mock_y_acc[ACCEL_BASE] = 400;
mock_z_acc[ACCEL_BASE] = 300;
mock_x_acc[ACCEL_LID] = -500;
mock_y_acc[ACCEL_LID] = -400;
mock_z_acc[ACCEL_LID] = -300;
base->xyz[X] = 500;
base->xyz[Y] = 400;
base->xyz[Z] = 300;
lid->xyz[X] = -500;
lid->xyz[Y] = -400;
lid->xyz[Z] = -300;
sample = *lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK;
task_wake(TASK_ID_MOTIONSENSE);
while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample)