diff options
| -rw-r--r-- | common/build.mk | 1 | ||||
| -rw-r--r-- | driver/accelgyro_lsm6dsm.c | 355 | ||||
| -rw-r--r-- | driver/accelgyro_lsm6dsm.h | 180 | ||||
| -rw-r--r-- | driver/build.mk | 1 | ||||
| -rw-r--r-- | include/config.h | 1 |
5 files changed, 538 insertions, 0 deletions
diff --git a/common/build.mk b/common/build.mk index d835badad9..6a3ac51b61 100644 --- a/common/build.mk +++ b/common/build.mk @@ -12,6 +12,7 @@ common-y+=version.o printf.o queue.o queue_policies.o common-$(CONFIG_ACCELGYRO_BMA255)+=math_util.o common-$(CONFIG_ACCELGYRO_BMI160)+=math_util.o common-$(CONFIG_ACCELGYRO_LSM6DS0)+=math_util.o +common-$(CONFIG_ACCELGYRO_LSM6DSM)+=math_util.o common-$(CONFIG_ACCEL_KXCJ9)+=math_util.o common-$(CONFIG_ACCEL_KX022)+=math_util.o common-$(CONFIG_ADC)+=adc.o diff --git a/driver/accelgyro_lsm6dsm.c b/driver/accelgyro_lsm6dsm.c new file mode 100644 index 0000000000..146ea4987f --- /dev/null +++ b/driver/accelgyro_lsm6dsm.c @@ -0,0 +1,355 @@ +/* Copyright (c) 2016 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. + */ + +/** + * LSM6DSM accelerometer and gyro module for Chrome EC + * 3D digital accelerometer & 3D digital gyroscope + */ + +#include "accelgyro.h" +#include "common.h" +#include "console.h" +#include "driver/accelgyro_lsm6dsm.h" +#include "hooks.h" +#include "i2c.h" +#include "math_util.h" +#include "task.h" +#include "util.h" + +#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args) + +struct lsm6dsm_data lsm6dsm_a_data; +struct lsm6dsm_data lsm6dsm_g_data; + +/** + * @return output base register for sensor + */ +static inline int get_xyz_reg(enum motionsensor_type type) +{ + return LSM6DSM_ACCEL_OUT_X_L_ADDR - + (LSM6DSM_ACCEL_OUT_X_L_ADDR - LSM6DSM_GYRO_OUT_X_L_ADDR) * type; +} + +/** + * Read single register + */ +static inline int raw_read8(const int port, const int addr, const int reg, + int *data_ptr) +{ + return i2c_read8(port, addr, reg, data_ptr); +} + +/** + * Write single register + */ +static inline int raw_write8(const int port, const int addr, const int reg, + int data) +{ + return i2c_write8(port, addr, reg, data); +} + + /** + * write_data_with_mask - Write register with mask + * @s: Motion sensor pointer + * @reg: Device register + * @mask: The mask to search + * @data: Data pointer + */ +static int write_data_with_mask(const struct motion_sensor_t *s, int reg, + uint8_t mask, uint8_t data) +{ + int err; + int new_data = 0x00, old_data = 0x00; + + err = raw_read8(s->port, s->addr, reg, &old_data); + if (err != EC_SUCCESS) + return err; + + new_data = ((old_data & (~mask)) | ((data << __builtin_ctz(mask)) & mask)); + + if (new_data == old_data) + return EC_SUCCESS; + + return raw_write8(s->port, s->addr, reg, new_data); +} + +/** + * set_range - set full scale range + * @s: Motion sensor pointer + * @range: Range + * @rnd: Round up/down flag + */ +static int set_range(const struct motion_sensor_t *s, int range, int rnd) +{ + int err; + uint8_t ctrl_reg, reg_val; + struct lsm6dsm_data *data = s->drv_data; + int newrange = range; + + ctrl_reg = LSM6DSM_RANGE_REG(s->type); + if (s->type == MOTIONSENSE_TYPE_ACCEL) { + /* Adjust and check rounded value for acc */ + if (rnd && (newrange < LSM6DSM_ACCEL_NORMALIZE_FS(newrange))) + newrange <<= 1; + if (newrange > LSM6DSM_ACCEL_FS_MAX_VAL) + newrange = LSM6DSM_ACCEL_FS_MAX_VAL; + + reg_val = LSM6DSM_ACCEL_FS_REG(newrange); + } else { + /* Adjust and check rounded value for gyro */ + if (rnd && (newrange < LSM6DSM_GYRO_NORMALIZE_FS(newrange))) + newrange <<= 1; + if (newrange > LSM6DSM_GYRO_FS_MAX_VAL) + newrange = LSM6DSM_GYRO_FS_MAX_VAL; + + reg_val = LSM6DSM_GYRO_FS_REG(newrange); + } + + mutex_lock(s->mutex); + err = write_data_with_mask(s, ctrl_reg, LSM6DSM_RANGE_MASK, reg_val); + if (err == EC_SUCCESS) + /* Save internally gain for speed optimization in read polling data */ + data->base.range = (s->type == MOTIONSENSE_TYPE_ACCEL ? + LSM6DSM_ACCEL_FS_GAIN(newrange) : LSM6DSM_GYRO_FS_GAIN(newrange)); + mutex_unlock(s->mutex); + return EC_SUCCESS; +} + +static int get_range(const struct motion_sensor_t *s) +{ + struct lsm6dsm_data *data = s->drv_data; + + if (MOTIONSENSE_TYPE_ACCEL == s->type) + return LSM6DSM_ACCEL_GAIN_FS(data->base.range); + + return LSM6DSM_GYRO_GAIN_FS(data->base.range); +} + +static int set_resolution(const struct motion_sensor_t *s, int res, int rnd) +{ + /* Only one resolution, LSM6DSM_RESOLUTION, so nothing to do. */ + return EC_SUCCESS; +} + +static int get_resolution(const struct motion_sensor_t *s) +{ + /* Only one resolution, LSM6DSM_RESOLUTION, so nothing to do. */ + return LSM6DSM_RESOLUTION; +} + +static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd) +{ + int ret, normalized_rate; + struct lsm6dsm_data *data = s->drv_data; + uint8_t ctrl_reg, reg_val; + + reg_val = LSM6DSM_ODR_TO_REG(rate); + ctrl_reg = LSM6DSM_ODR_REG(s->type); + normalized_rate = LSM6DSM_ODR_TO_NORMALIZE(rate); + + if (rate == 0) { + /* Power Off device */ + ret = write_data_with_mask(s, ctrl_reg, LSM6DSM_ODR_MASK, + LSM6DSM_ODR_POWER_OFF_VAL); + return ret; + } + + if (rnd && (normalized_rate < rate)) { + reg_val++; + normalized_rate <<= 1; + } + + /* Adjust rounded value for acc and gyro because ODR are shared */ + if (reg_val > LSM6DSM_ODR_833HZ_VAL) { + reg_val = LSM6DSM_ODR_833HZ_VAL; + normalized_rate = LSM6DSM_ODR_MAX_VAL; + } else if (reg_val < LSM6DSM_ODR_13HZ_VAL) { + reg_val = LSM6DSM_ODR_13HZ_VAL; + normalized_rate = LSM6DSM_ODR_MIN_VAL; + } + + /* + * Lock accel resource to prevent another task from attempting + * to write accel parameters until we are done + */ + mutex_lock(s->mutex); + ret = write_data_with_mask(s, ctrl_reg, LSM6DSM_ODR_MASK, reg_val); + if (ret == EC_SUCCESS) + data->base.odr = normalized_rate; + + mutex_unlock(s->mutex); + return ret; +} + +static int get_data_rate(const struct motion_sensor_t *s) +{ + struct lsm6dsm_data *data = s->drv_data; + + return data->base.odr; +} + +static int set_offset(const struct motion_sensor_t *s, + const int16_t *offset, int16_t temp) +{ + struct lsm6dsm_data *data = s->drv_data; + + data->offset[X] = offset[X]; + data->offset[Y] = offset[Y]; + data->offset[Z] = offset[Z]; + return EC_SUCCESS; +} + +static int get_offset(const struct motion_sensor_t *s, + int16_t *offset, + int16_t *temp) +{ + struct lsm6dsm_data *data = s->drv_data; + + offset[X] = data->offset[X]; + offset[Y] = data->offset[Y]; + offset[Z] = data->offset[Z]; + *temp = EC_MOTION_SENSE_INVALID_CALIB_TEMP; + return EC_SUCCESS; +} + +static int is_data_ready(const struct motion_sensor_t *s, int *ready) +{ + int ret, tmp; + + ret = raw_read8(s->port, s->addr, LSM6DSM_STATUS_REG, &tmp); + if (ret != EC_SUCCESS) { + CPRINTF("[%T %s type:0x%X RS Error]", s->name, s->type); + return ret; + } + + if (MOTIONSENSE_TYPE_ACCEL == s->type) + *ready = (LSM6DSM_STS_XLDA_UP == (tmp & LSM6DSM_STS_XLDA_MASK)); + else + *ready = (LSM6DSM_STS_GDA_UP == (tmp & LSM6DSM_STS_GDA_MASK)); + + return EC_SUCCESS; +} + +/* + * TODO: Implement FIFO support + * + * Is not very efficient to collect the data in read: better have an interrupt + * and collect the FIFO, even if it has one item: we don't have to check if the + * sensor is ready (minimize I2C access) + */ +static int read(const struct motion_sensor_t *s, vector_3_t v) +{ + uint8_t raw[LSM6DSM_OUT_XYZ_SIZE]; + uint8_t xyz_reg; + int ret, i, range, tmp = 0; + struct lsm6dsm_data *data = s->drv_data; + + ret = is_data_ready(s, &tmp); + if (ret != EC_SUCCESS) + return ret; + + /* + * 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) { + if (v != s->raw_xyz) + memcpy(v, s->raw_xyz, sizeof(s->raw_xyz)); + return EC_SUCCESS; + } + + xyz_reg = get_xyz_reg(s->type); + + /* Read data bytes starting at xyz_reg */ + i2c_lock(s->port, 1); + ret = i2c_xfer(s->port, s->addr, &xyz_reg, 1, raw, + LSM6DSM_OUT_XYZ_SIZE, I2C_XFER_SINGLE); + i2c_lock(s->port, 0); + + if (ret != EC_SUCCESS) { + CPRINTF("[%T %s type:0x%X RD XYZ Error]", + s->name, s->type); + return ret; + } + + for (i = X; i <= Z; i++) { + v[i] = ((int16_t)((raw[i * 2 + 1] << 8) | raw[i * 2])); + /* On range we seved gain related to FS */ + v[i] = v[i] * data->base.range; + } + + /* Apply rotation matrix */ + rotate(v, *s->rot_standard_ref, v); + + /* Apply offset in the device coordinates */ + range = get_range(s); + for (i = X; i <= Z; i++) + v[i] += (data->offset[i] << 5) / range; + + return EC_SUCCESS; +} + +static int init(const struct motion_sensor_t *s) +{ + int ret = 0, tmp; + + ret = raw_read8(s->port, s->addr, LSM6DSM_WHO_AM_I_REG, &tmp); + if (ret != EC_SUCCESS) + return EC_ERROR_UNKNOWN; + + if (tmp != LSM6DSM_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 so reset it + * lsm6dsm supports both accel & gyro features + * Board will see two virtual sensor devices: accel & gyro + * Requirement: Accel need be init before gyro + */ + if (s->type == MOTIONSENSE_TYPE_ACCEL) { + mutex_lock(s->mutex); + + /* Software reset */ + ret = write_data_with_mask(s, LSM6DSM_RESET_ADDR, + LSM6DSM_RESET_MASK, LSM6DSM_EN_BIT); + if (ret != EC_SUCCESS) + goto err_unlock; + + /* Output data not updated until have been read */ + ret = write_data_with_mask(s, LSM6DSM_BDU_ADDR, + LSM6DSM_BDU_MASK, LSM6DSM_EN_BIT); + if (ret != EC_SUCCESS) + goto err_unlock; + + mutex_unlock(s->mutex); + } + + ret = set_range(s, s->default_range, 1); + + CPRINTF("[%T %s: MS Done Init type:0x%X range:%d]\n", + s->name, s->type, get_range(s)); + return ret; + +err_unlock: + mutex_unlock(s->mutex); + + return EC_ERROR_UNKNOWN; +} + +const struct accelgyro_drv lsm6dsm_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, + .set_offset = set_offset, + .get_offset = get_offset, + .perform_calib = NULL, +}; diff --git a/driver/accelgyro_lsm6dsm.h b/driver/accelgyro_lsm6dsm.h new file mode 100644 index 0000000000..ba1bf51510 --- /dev/null +++ b/driver/accelgyro_lsm6dsm.h @@ -0,0 +1,180 @@ +/* Copyright (c) 2016 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. + */ + +/* LSM6DSM Accel and Gyro driver for Chrome EC */ + +#ifndef __CROS_EC_ACCELGYRO_LSM6DSM_H +#define __CROS_EC_ACCELGYRO_LSM6DSM_H + +#include "accelgyro.h" + +#define LSM6DSM_I2C_ADDR(__x) (__x << 1) + +/* + * 7-bit address is 110101Xb. Where 'X' is determined + * by the voltage on the ADDR pin. + */ +#define LSM6DSM_ADDR0 LSM6DSM_I2C_ADDR(0x6a) +#define LSM6DSM_ADDR1 LSM6DSM_I2C_ADDR(0x6b) + +/* Who Am I */ +#define LSM6DSM_WHO_AM_I_REG 0x0f +#define LSM6DSM_WHO_AM_I 0x6a + +#define LSM6DSM_OUT_XYZ_SIZE 6 + +/* Sensor Software Reset Bit */ +#define LSM6DSM_RESET_ADDR 0x12 +#define LSM6DSM_RESET_MASK 0x01 + +/* COMMON DEFINE FOR ACCEL-GYRO SENSORS */ +#define LSM6DSM_EN_BIT 0x01 +#define LSM6DSM_DIS_BIT 0x00 + +#define LSM6DSM_LIR_ADDR 0x58 +#define LSM6DSM_LIR_MASK 0x01 + +#define LSM6DSM_BDU_ADDR 0x12 +#define LSM6DSM_BDU_MASK 0x40 + +#define LSM6DSM_INT2_ON_INT1_ADDR 0x13 +#define LSM6DSM_INT2_ON_INT1_MASK 0x20 + +#define LSM6DSM_GYRO_OUT_X_L_ADDR 0x22 +#define LSM6DSM_ACCEL_OUT_X_L_ADDR 0x28 + +#define LSM6DSM_CTRL1_ADDR 0x10 +#define LSM6DSM_CTRL2_ADDR 0x11 +#define LSM6DSM_CTRL3_ADDR 0x12 +#define LSM6DSM_CTRL6_ADDR 0x15 +#define LSM6DSM_CTRL7_ADDR 0x16 + +#define LSM6DSM_STATUS_REG 0x1e + +/* Output data rate registers and masks */ +#define LSM6DSM_ODR_REG(_sensor) \ + (LSM6DSM_CTRL1_ADDR + _sensor) +#define LSM6DSM_ODR_MASK 0xf0 + +/* Common Acc/Gyro data rate */ +enum lsm6dsm_odr { + LSM6DSM_ODR_POWER_OFF_VAL = 0x00, + LSM6DSM_ODR_13HZ_VAL, + LSM6DSM_ODR_26HZ_VAL, + LSM6DSM_ODR_52HZ_VAL, + LSM6DSM_ODR_104HZ_VAL, + LSM6DSM_ODR_208HZ_VAL, + LSM6DSM_ODR_416HZ_VAL, + LSM6DSM_ODR_833HZ_VAL, + LSM6DSM_ODR_LIST_NUM +}; + +/* Absolute maximum rate for acc and gyro sensors */ +#define LSM6DSM_ODR_MIN_VAL 13000 +#define LSM6DSM_ODR_MAX_VAL 833000 + +/* ODR reg value from selected data rate in mHz */ +#define LSM6DSM_ODR_TO_REG(_odr) \ + (31 - __builtin_clz(_odr / LSM6DSM_ODR_MIN_VAL)) + +/* normalized ODR value from selected data rate in mHz */ +#define LSM6DSM_ODR_TO_NORMALIZE(_odr) \ + (LSM6DSM_ODR_MIN_VAL * (_odr / LSM6DSM_ODR_MIN_VAL)) + +/* Full Scale range value for Accel */ +#define LSM6DSM_FS_LIST_NUM 4 + +#define LSM6DSM_ACCEL_FS_ADDR 0x10 +#define LSM6DSM_ACCEL_FS_MASK 0x0c + +#define LSM6DSM_ACCEL_FS_2G_VAL 0x00 +#define LSM6DSM_ACCEL_FS_4G_VAL 0x02 +#define LSM6DSM_ACCEL_FS_8G_VAL 0x03 +#define LSM6DSM_ACCEL_FS_16G_VAL 0x01 + +#define LSM6DSM_ACCEL_FS_2G_GAIN 61 +#define LSM6DSM_ACCEL_FS_4G_GAIN 122 +#define LSM6DSM_ACCEL_FS_8G_GAIN 244 +#define LSM6DSM_ACCEL_FS_16G_GAIN 488 + +#define LSM6DSM_ACCEL_FS_MAX_VAL 16 + +/* Accel Gain value from selected Full Scale */ +#define LSM6DSM_ACCEL_FS_GAIN(_fs) \ + (_fs == 16 ? LSM6DSM_ACCEL_FS_16G_GAIN : \ + LSM6DSM_ACCEL_FS_2G_GAIN << (31 - __builtin_clz(_fs / 2))) + +/* Accel FS Full Scale value from Gain */ +#define LSM6DSM_ACCEL_GAIN_FS(_gain) \ + (1 << (32 - __builtin_clz(_gain / LSM6DSM_ACCEL_FS_2G_GAIN))) + +/* Accel Reg value from Full Scale */ +#define LSM6DSM_ACCEL_FS_REG(_fs) \ + (_fs == 2 ? LSM6DSM_ACCEL_FS_2G_VAL : \ + _fs == 16 ? LSM6DSM_ACCEL_FS_16G_VAL : \ + (32 - __builtin_clz(_fs / 2))) + +/* Accel normalized FS value from Full Scale */ +#define LSM6DSM_ACCEL_NORMALIZE_FS(_fs) \ + (1 << (32 - __builtin_clz(_fs / 2))) + +/* Full Scale range value for Gyro */ +#define LSM6DSM_GYRO_FS_ADDR 0x11 +#define LSM6DSM_GYRO_FS_MASK 0x0c + +#define LSM6DSM_GYRO_FS_245_VAL 0x00 +#define LSM6DSM_GYRO_FS_500_VAL 0x01 +#define LSM6DSM_GYRO_FS_1000_VAL 0x02 +#define LSM6DSM_GYRO_FS_2000_VAL 0x03 + +#define LSM6DSM_GYRO_FS_245_GAIN 8750 +#define LSM6DSM_GYRO_FS_500_GAIN 17500 +#define LSM6DSM_GYRO_FS_1000_GAIN 35000 +#define LSM6DSM_GYRO_FS_2000_GAIN 70000 + +#define LSM6DSM_GYRO_FS_MAX_VAL 20000 + +/* Gyro FS Gain value from selected Full Scale */ +#define LSM6DSM_GYRO_FS_GAIN(_fs) \ + (LSM6DSM_GYRO_FS_245_GAIN << (31 - __builtin_clz(_fs / 245))) + +/* Gyro FS Full Scale value from Gain */ +#define LSM6DSM_GYRO_GAIN_FS(_gain) \ + (_gain == LSM6DSM_GYRO_FS_245_GAIN ? 245 : \ + 500 << (30 - __builtin_clz(_gain / LSM6DSM_GYRO_FS_245_GAIN))) + +/* Gyro Reg value from Full Scale */ +#define LSM6DSM_GYRO_FS_REG(_fs) \ + ((31 - __builtin_clz(_fs / 245))) + +/* Gyro normalized FS value from Full Scale: for Gyro Gains are not multiple */ +#define LSM6DSM_GYRO_NORMALIZE_FS(_fs) \ + (_fs == 245 ? 245 : 500 << (31 - __builtin_clz(_fs / 500))) + +/* FS register address/mask for Acc/Gyro sensors */ +#define LSM6DSM_RANGE_REG(_sensor) (LSM6DSM_ACCEL_FS_ADDR + (_sensor)) +#define LSM6DSM_RANGE_MASK 0x0c + +/* Status register bitmask for Acc/Gyro data ready */ +enum lsm6dsm_status { + LSM6DSM_STS_DOWN = 0x00, + LSM6DSM_STS_XLDA_UP = 0x01, + LSM6DSM_STS_GDA_UP = 0x02 +}; + +#define LSM6DSM_STS_XLDA_MASK 0x01 +#define LSM6DSM_STS_GDA_MASK 0x02 + +/* Sensor resolution in number of bits. This sensor has fixed resolution. */ +#define LSM6DSM_RESOLUTION 16 + +extern const struct accelgyro_drv lsm6dsm_drv; + +struct lsm6dsm_data { + struct accelgyro_saved_data_t base; + int16_t offset[3]; +}; + +#endif /* __CROS_EC_ACCELGYRO_LSM6DSM_H */ diff --git a/driver/build.mk b/driver/build.mk index 0a6d109adc..43f68502ea 100644 --- a/driver/build.mk +++ b/driver/build.mk @@ -13,6 +13,7 @@ driver-$(CONFIG_ACCEL_KX022)+=accel_kionix.o driver-$(CONFIG_ACCELGYRO_LSM6DS0)+=accelgyro_lsm6ds0.o driver-$(CONFIG_ACCELGYRO_BMI160)+=accelgyro_bmi160.o driver-$(CONFIG_MAG_BMI160_BMM150)+=mag_bmm150.o +driver-$(CONFIG_ACCELGYRO_LSM6DSM)+=accelgyro_lsm6dsm.o # Gyrometers driver-$(CONFIG_GYRO_L3GD20H)+=gyro_l3gd20h.o diff --git a/include/config.h b/include/config.h index f56239d303..8690a7cf03 100644 --- a/include/config.h +++ b/include/config.h @@ -56,6 +56,7 @@ #undef CONFIG_ACCEL_KX022 #undef CONFIG_ACCELGYRO_LSM6DS0 #undef CONFIG_ACCELGYRO_BMI160 +#undef CONFIG_ACCELGYRO_LSM6DSM /* Specify barometer attached */ #undef CONFIG_BARO_BMP280 |