diff options
-rw-r--r-- | common/motion_sense.c | 162 | ||||
-rw-r--r-- | driver/accel_kxcj9.c | 223 | ||||
-rw-r--r-- | driver/accelgyro_lsm6ds0.c | 285 | ||||
-rw-r--r-- | driver/accelgyro_lsm6ds0.h | 45 |
4 files changed, 492 insertions, 223 deletions
diff --git a/common/motion_sense.c b/common/motion_sense.c index d48b578cd1..90ed1ecd6d 100644 --- a/common/motion_sense.c +++ b/common/motion_sense.c @@ -543,3 +543,165 @@ 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) +{ + char *e; + int id, data, round = 1; + + if (argc < 2 || argc > 4) + return EC_ERROR_PARAM_COUNT; + + /* First argument is sensor id. */ + id = strtoi(argv[1], &e, 0); + if (*e || id < 0 || id > ACCEL_COUNT) + return EC_ERROR_PARAM1; + + if (argc >= 3) { + /* Second argument is data to write. */ + data = strtoi(argv[2], &e, 0); + if (*e) + return EC_ERROR_PARAM2; + + if (argc == 4) { + /* Third argument is rounding flag. */ + round = strtoi(argv[3], &e, 0); + if (*e) + return EC_ERROR_PARAM3; + } + + /* + * Write new range, if it returns invalid arg, then return + * a parameter error. + */ + if (accel_set_range(id, data, round) == EC_ERROR_INVAL) + return EC_ERROR_PARAM2; + } else { + accel_get_range(id, &data); + ccprintf("Range for sensor %d: %d\n", id, data); + } + + return EC_SUCCESS; +} +DECLARE_CONSOLE_COMMAND(accelrange, command_accelrange, + "id [data [roundup]]", + "Read or write accelerometer range", NULL); + +static int command_accelresolution(int argc, char **argv) +{ + char *e; + int id, data, round = 1; + + if (argc < 2 || argc > 4) + return EC_ERROR_PARAM_COUNT; + + /* First argument is sensor id. */ + id = strtoi(argv[1], &e, 0); + if (*e || id < 0 || id > ACCEL_COUNT) + return EC_ERROR_PARAM1; + + if (argc >= 3) { + /* Second argument is data to write. */ + data = strtoi(argv[2], &e, 0); + if (*e) + return EC_ERROR_PARAM2; + + if (argc == 4) { + /* Third argument is rounding flag. */ + round = strtoi(argv[3], &e, 0); + if (*e) + return EC_ERROR_PARAM3; + } + + /* + * Write new resolution, if it returns invalid arg, then + * return a parameter error. + */ + if (accel_set_resolution(id, data, round) == EC_ERROR_INVAL) + return EC_ERROR_PARAM2; + } else { + accel_get_resolution(id, &data); + ccprintf("Resolution for sensor %d: %d\n", id, data); + } + + return EC_SUCCESS; +} +DECLARE_CONSOLE_COMMAND(accelres, command_accelresolution, + "id [data [roundup]]", + "Read or write accelerometer resolution", NULL); + +static int command_acceldatarate(int argc, char **argv) +{ + char *e; + int id, data, round = 1; + + if (argc < 2 || argc > 4) + return EC_ERROR_PARAM_COUNT; + + /* First argument is sensor id. */ + id = strtoi(argv[1], &e, 0); + if (*e || id < 0 || id > ACCEL_COUNT) + return EC_ERROR_PARAM1; + + if (argc >= 3) { + /* Second argument is data to write. */ + data = strtoi(argv[2], &e, 0); + if (*e) + return EC_ERROR_PARAM2; + + if (argc == 4) { + /* Third argument is rounding flag. */ + round = strtoi(argv[3], &e, 0); + if (*e) + return EC_ERROR_PARAM3; + } + + /* + * Write new data rate, if it returns invalid arg, then + * return a parameter error. + */ + if (accel_set_datarate(id, data, round) == EC_ERROR_INVAL) + return EC_ERROR_PARAM2; + } else { + accel_get_datarate(id, &data); + ccprintf("Data rate for sensor %d: %d\n", id, data); + } + + return EC_SUCCESS; +} +DECLARE_CONSOLE_COMMAND(accelrate, command_acceldatarate, + "id [data [roundup]]", + "Read or write accelerometer range", NULL); + +#ifdef CONFIG_ACCEL_INTERRUPTS +static int command_accelerometer_interrupt(int argc, char **argv) +{ + char *e; + int id, thresh; + + if (argc != 3) + return EC_ERROR_PARAM_COUNT; + + /* First argument is id. */ + id = strtoi(argv[1], &e, 0); + if (*e || id < 0 || id >= ACCEL_COUNT) + return EC_ERROR_PARAM1; + + /* Second argument is interrupt threshold. */ + thresh = strtoi(argv[2], &e, 0); + if (*e) + return EC_ERROR_PARAM2; + + accel_set_interrupt(id, thresh); + + return EC_SUCCESS; +} +DECLARE_CONSOLE_COMMAND(accelint, command_accelerometer_interrupt, + "id threshold", + "Write interrupt threshold", NULL); +#endif /* CONFIG_ACCEL_INTERRUPTS */ + +#endif /* CONFIG_CMD_ACCELS */ + + diff --git a/driver/accel_kxcj9.c b/driver/accel_kxcj9.c index 76aee6f1b5..ff65e50b94 100644 --- a/driver/accel_kxcj9.c +++ b/driver/accel_kxcj9.c @@ -514,226 +514,3 @@ int accel_init(const enum accel_id id) return ret; } - - - -/*****************************************************************************/ -/* Console commands */ -#ifdef CONFIG_CMD_ACCELS -static int command_read_accelerometer(int argc, char **argv) -{ - char *e; - int addr, reg, data; - - if (argc != 3) - return EC_ERROR_PARAM_COUNT; - - /* First argument is address. */ - addr = strtoi(argv[1], &e, 0); - if (*e) - return EC_ERROR_PARAM1; - - /* Second argument is register offset. */ - reg = strtoi(argv[2], &e, 0); - if (*e) - return EC_ERROR_PARAM2; - - raw_read8(addr, reg, &data); - - ccprintf("0x%02x\n", data); - - return EC_SUCCESS; -} -DECLARE_CONSOLE_COMMAND(accelread, command_read_accelerometer, - "addr reg", - "Read from accelerometer at slave address addr", NULL); - -static int command_write_accelerometer(int argc, char **argv) -{ - char *e; - int addr, reg, data; - - if (argc != 4) - return EC_ERROR_PARAM_COUNT; - - /* First argument is address. */ - addr = strtoi(argv[1], &e, 0); - if (*e) - return EC_ERROR_PARAM1; - - /* Second argument is register offset. */ - reg = strtoi(argv[2], &e, 0); - if (*e) - return EC_ERROR_PARAM2; - - /* Third argument is data. */ - data = strtoi(argv[3], &e, 0); - if (*e) - return EC_ERROR_PARAM3; - - raw_write8(addr, reg, data); - - return EC_SUCCESS; -} -DECLARE_CONSOLE_COMMAND(accelwrite, command_write_accelerometer, - "addr reg data", - "Write to accelerometer at slave address addr", NULL); - -static int command_accelrange(int argc, char **argv) -{ - char *e; - int id, data, round = 1; - - if (argc < 2 || argc > 4) - return EC_ERROR_PARAM_COUNT; - - /* First argument is sensor id. */ - id = strtoi(argv[1], &e, 0); - if (*e || id < 0 || id > ACCEL_COUNT) - return EC_ERROR_PARAM1; - - if (argc >= 3) { - /* Second argument is data to write. */ - data = strtoi(argv[2], &e, 0); - if (*e) - return EC_ERROR_PARAM2; - - if (argc == 4) { - /* Third argument is rounding flag. */ - round = strtoi(argv[3], &e, 0); - if (*e) - return EC_ERROR_PARAM3; - } - - /* - * Write new range, if it returns invalid arg, then return - * a parameter error. - */ - if (accel_set_range(id, data, round) == EC_ERROR_INVAL) - return EC_ERROR_PARAM2; - } else { - accel_get_range(id, &data); - ccprintf("Range for sensor %d: %d\n", id, data); - } - - return EC_SUCCESS; -} -DECLARE_CONSOLE_COMMAND(accelrange, command_accelrange, - "id [data [roundup]]", - "Read or write accelerometer range", NULL); - -static int command_accelresolution(int argc, char **argv) -{ - char *e; - int id, data, round = 1; - - if (argc < 2 || argc > 4) - return EC_ERROR_PARAM_COUNT; - - /* First argument is sensor id. */ - id = strtoi(argv[1], &e, 0); - if (*e || id < 0 || id > ACCEL_COUNT) - return EC_ERROR_PARAM1; - - if (argc >= 3) { - /* Second argument is data to write. */ - data = strtoi(argv[2], &e, 0); - if (*e) - return EC_ERROR_PARAM2; - - if (argc == 4) { - /* Third argument is rounding flag. */ - round = strtoi(argv[3], &e, 0); - if (*e) - return EC_ERROR_PARAM3; - } - - /* - * Write new resolution, if it returns invalid arg, then - * return a parameter error. - */ - if (accel_set_resolution(id, data, round) == EC_ERROR_INVAL) - return EC_ERROR_PARAM2; - } else { - accel_get_resolution(id, &data); - ccprintf("Resolution for sensor %d: %d\n", id, data); - } - - return EC_SUCCESS; -} -DECLARE_CONSOLE_COMMAND(accelres, command_accelresolution, - "id [data [roundup]]", - "Read or write accelerometer resolution", NULL); - -static int command_acceldatarate(int argc, char **argv) -{ - char *e; - int id, data, round = 1; - - if (argc < 2 || argc > 4) - return EC_ERROR_PARAM_COUNT; - - /* First argument is sensor id. */ - id = strtoi(argv[1], &e, 0); - if (*e || id < 0 || id > ACCEL_COUNT) - return EC_ERROR_PARAM1; - - if (argc >= 3) { - /* Second argument is data to write. */ - data = strtoi(argv[2], &e, 0); - if (*e) - return EC_ERROR_PARAM2; - - if (argc == 4) { - /* Third argument is rounding flag. */ - round = strtoi(argv[3], &e, 0); - if (*e) - return EC_ERROR_PARAM3; - } - - /* - * Write new data rate, if it returns invalid arg, then - * return a parameter error. - */ - if (accel_set_datarate(id, data, round) == EC_ERROR_INVAL) - return EC_ERROR_PARAM2; - } else { - accel_get_datarate(id, &data); - ccprintf("Data rate for sensor %d: %d\n", id, data); - } - - return EC_SUCCESS; -} -DECLARE_CONSOLE_COMMAND(accelrate, command_acceldatarate, - "id [data [roundup]]", - "Read or write accelerometer range", NULL); - -#ifdef CONFIG_ACCEL_INTERRUPTS -static int command_accelerometer_interrupt(int argc, char **argv) -{ - char *e; - int id, thresh; - - if (argc != 3) - return EC_ERROR_PARAM_COUNT; - - /* First argument is id. */ - id = strtoi(argv[1], &e, 0); - if (*e || id < 0 || id >= ACCEL_COUNT) - return EC_ERROR_PARAM1; - - /* Second argument is interrupt threshold. */ - thresh = strtoi(argv[2], &e, 0); - if (*e) - return EC_ERROR_PARAM2; - - accel_set_interrupt(id, thresh); - - return EC_SUCCESS; -} -DECLARE_CONSOLE_COMMAND(accelint, command_accelerometer_interrupt, - "id threshold", - "Write interrupt threshold", NULL); -#endif /* CONFIG_ACCEL_INTERRUPTS */ - -#endif /* CONFIG_CMD_ACCELS */ diff --git a/driver/accelgyro_lsm6ds0.c b/driver/accelgyro_lsm6ds0.c new file mode 100644 index 0000000000..8c8bd76b49 --- /dev/null +++ b/driver/accelgyro_lsm6ds0.c @@ -0,0 +1,285 @@ +/* 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. + */ + +/* LSM6DS0 accelerometer and gyro module for Chrome EC */ + +#include "accelerometer.h" +#include "common.h" +#include "console.h" +#include "driver/accelgyro_lsm6ds0.h" +#include "hooks.h" +#include "i2c.h" +#include "task.h" +#include "util.h" + +/* + * 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. */ +}; + +/* List of range values in +/-G's and their associated register values. */ +const struct accel_param_pair ranges[] = { + {2, LSM6DS0_GSEL_2G}, + {4, LSM6DS0_GSEL_4G}, + {8, LSM6DS0_GSEL_8G} +}; + +/* List of ODR values in mHz and their associated register values. */ +const struct accel_param_pair datarates[] = { + {10000, LSM6DS0_ODR_10HZ}, + {50000, LSM6DS0_ODR_50HZ}, + {119000, LSM6DS0_ODR_119HZ}, + {238000, LSM6DS0_ODR_238HZ}, + {476000, LSM6DS0_ODR_476HZ}, + {952000, LSM6DS0_ODR_982HZ} +}; + +/* Current range of each accelerometer. The value is an index into ranges[]. */ +static int sensor_range[ACCEL_COUNT] = {0, 0}; + +/* + * Current output data rate of each accelerometer. The value is an index into + * datarates[]. + */ +static int sensor_datarate[ACCEL_COUNT] = {1, 1}; + +static struct mutex accel_mutex[ACCEL_COUNT]; + +/** + * 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. + */ +static int find_param_index(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; + + if (eng_val < pairs[i+1].val) { + if (round_up) + return i + 1; + else + return i; + } + } + + return i; +} + +/** + * Read register from accelerometer. + */ +static int raw_read8(const int addr, const int reg, int *data_ptr) +{ + return i2c_read8(I2C_PORT_ACCEL, addr, reg, data_ptr); +} + +/** + * Write register from accelerometer. + */ +static int raw_write8(const int addr, const int reg, int data) +{ + return i2c_write8(I2C_PORT_ACCEL, addr, reg, data); +} + +int accel_set_range(const enum accel_id id, const int range, const int rnd) +{ + int ret, index, ctrl_reg6; + + /* Find index for interface pair matching the specified range. */ + index = find_param_index(range, rnd, ranges, ARRAY_SIZE(ranges)); + + /* + * Lock accel resource to prevent another task from attempting + * to write accel parameters until we are done. + */ + mutex_lock(&accel_mutex[id]); + + ret = raw_read8(accel_addr[id], LSM6DS0_CTRL_REG6_XL, &ctrl_reg6); + if (ret != EC_SUCCESS) + goto accel_cleanup; + + ctrl_reg6 = (ctrl_reg6 & ~LSM6DS0_GSEL_ALL) | ranges[index].reg; + ret = raw_write8(accel_addr[id], LSM6DS0_CTRL_REG6_XL, ctrl_reg6); + +accel_cleanup: + /* Unlock accel resource and save new range if written successfully. */ + mutex_unlock(&accel_mutex[id]); + if (ret == EC_SUCCESS) + sensor_range[id] = index; + + return EC_SUCCESS; +} + +int accel_get_range(const enum accel_id id, int * const range) +{ + /* Check for valid id. */ + if (id < 0 || id >= ACCEL_COUNT) + return EC_ERROR_INVAL; + + *range = ranges[sensor_range[id]].val; + return EC_SUCCESS; +} + +int accel_set_resolution(const enum accel_id id, const int res, const int rnd) +{ + /* Check for valid id. */ + if (id < 0 || id >= ACCEL_COUNT) + return EC_ERROR_INVAL; + + /* Only one resolution, LSM6DS0_RESOLUTION, so nothing to do. */ + return EC_SUCCESS; +} + +int accel_get_resolution(const enum accel_id id, int * const res) +{ + /* Check for valid id. */ + if (id < 0 || id >= ACCEL_COUNT) + return EC_ERROR_INVAL; + + *res = LSM6DS0_RESOLUTION; + return EC_SUCCESS; +} + +int accel_set_datarate(const enum accel_id id, const int rate, const int rnd) +{ + int ret, index, ctrl_reg6; + + /* Check for valid id. */ + if (id < 0 || id >= ACCEL_COUNT) + return EC_ERROR_INVAL; + + /* Find index for interface pair matching the specified range. */ + index = find_param_index(rate, rnd, datarates, ARRAY_SIZE(datarates)); + + /* + * Lock accel resource to prevent another task from attempting + * to write accel parameters until we are done. + */ + mutex_lock(&accel_mutex[id]); + + ret = raw_read8(accel_addr[id], LSM6DS0_CTRL_REG6_XL, &ctrl_reg6); + if (ret != EC_SUCCESS) + goto accel_cleanup; + + ctrl_reg6 = (ctrl_reg6 & ~LSM6DS0_ODR_ALL) | datarates[index].reg; + ret = raw_write8(accel_addr[id], LSM6DS0_CTRL_REG6_XL, ctrl_reg6); + +accel_cleanup: + /* Unlock accel resource and save new ODR if written successfully. */ + mutex_unlock(&accel_mutex[id]); + if (ret == EC_SUCCESS) + sensor_datarate[id] = index; + + return EC_SUCCESS; +} + +int accel_get_datarate(const enum accel_id id, int * const rate) +{ + /* Check for valid id. */ + if (id < 0 || id >= ACCEL_COUNT) + return EC_ERROR_INVAL; + + *rate = datarates[sensor_datarate[id]].val; + return EC_SUCCESS; +} + +#ifdef CONFIG_ACCEL_INTERRUPTS +int accel_set_interrupt(const enum accel_id id, unsigned int threshold) +{ + /* Currently unsupported. */ + return EC_ERROR_UNKNOWN; +} +#endif + +int accel_read(const enum accel_id id, int * const x_acc, int * const y_acc, + int * const z_acc) +{ + uint8_t acc[6]; + uint8_t reg = LSM6DS0_OUT_X_L_XL; + int ret, multiplier; + + /* Read 6 bytes starting at LSM6DS0_OUT_X_L_XL. */ + mutex_lock(&accel_mutex[id]); + i2c_lock(I2C_PORT_ACCEL, 1); + ret = i2c_xfer(I2C_PORT_ACCEL, accel_addr[id], ®, 1, acc, 6, + I2C_XFER_SINGLE); + i2c_lock(I2C_PORT_ACCEL, 0); + mutex_unlock(&accel_mutex[id]); + + if (ret != EC_SUCCESS) + return ret; + + /* Determine multiplier based on stored range. */ + switch (ranges[sensor_range[id]].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; + + return EC_SUCCESS; +} + +int accel_init(const enum accel_id id) +{ + int ret, ctrl_reg6; + + /* Check for valid id. */ + if (id < 0 || id >= ACCEL_COUNT) + return EC_ERROR_INVAL; + + mutex_lock(&accel_mutex[id]); + + /* + * 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(accel_addr[id], LSM6DS0_CTRL_REG8, 1); + if (ret != EC_SUCCESS) + goto accel_cleanup; + + /* Set ODR and range. */ + ctrl_reg6 = datarates[sensor_datarate[id]].reg | + ranges[sensor_range[id]].reg; + + ret = raw_write8(accel_addr[id], LSM6DS0_CTRL_REG6_XL, ctrl_reg6); + +accel_cleanup: + mutex_unlock(&accel_mutex[id]); + return ret; +} diff --git a/driver/accelgyro_lsm6ds0.h b/driver/accelgyro_lsm6ds0.h new file mode 100644 index 0000000000..6c34570b15 --- /dev/null +++ b/driver/accelgyro_lsm6ds0.h @@ -0,0 +1,45 @@ +/* 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. + */ + +/* LSM6DS0 accelerometer and gyro module for Chrome EC */ + +#ifndef __CROS_EC_ACCEL_LSM6DS0_H +#define __CROS_EC_ACCEL_LSM6DS0_H + +/* + * 7-bit address is 110101Xb. Where 'X' is determined + * by the voltage on the ADDR pin. + */ +#define LSM6DS0_ADDR0 0xd4 +#define LSM6DS0_ADDR1 0xd6 + +/* Chip specific registers. */ +#define LSM6DS0_CTRL_REG6_XL 0x20 +#define LSM6DS0_CTRL_REG8 0x22 +#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_GSEL_2G (0 << 3) +#define LSM6DS0_GSEL_4G (2 << 3) +#define LSM6DS0_GSEL_8G (3 << 3) +#define LSM6DS0_GSEL_ALL (3 << 3) + +#define LSM6DS0_ODR_10HZ (1 << 5) +#define LSM6DS0_ODR_50HZ (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) + +/* Sensor resolution in number of bits. This sensor has fixed resolution. */ +#define LSM6DS0_RESOLUTION 16 + +#endif /* __CROS_EC_ACCEL_LSM6DS0_H */ |