diff options
| -rw-r--r-- | driver/accel_lis2dh.c | 7 | ||||
| -rw-r--r-- | driver/accelgyro_lsm6dsm.c | 228 | ||||
| -rw-r--r-- | driver/accelgyro_lsm6dsm.h | 42 | ||||
| -rw-r--r-- | driver/build.mk | 2 | ||||
| -rw-r--r-- | driver/stm_mems_common.c | 69 | ||||
| -rw-r--r-- | driver/stm_mems_common.h | 29 |
6 files changed, 150 insertions, 227 deletions
diff --git a/driver/accel_lis2dh.c b/driver/accel_lis2dh.c index 017621ff6a..e937205755 100644 --- a/driver/accel_lis2dh.c +++ b/driver/accel_lis2dh.c @@ -19,6 +19,9 @@ #include "driver/accel_lis2dh.h" #include "driver/stm_mems_common.h" +#define CPUTS(outstr) cputs(CC_ACCEL, outstr) +#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args) + #ifdef CONFIG_ACCEL_FIFO /** * enable_fifo - Enable/Disable FIFO in LIS2DH @@ -317,10 +320,6 @@ static int read(const struct motion_sensor_t *s, vector_3_t v) /* Transform from LSB to real data with rotation and gain */ st_normalize(s, v, raw); - /* apply offset in the device coordinates */ - for (i = X; i <= Z; i++) - v[i] += (data->offset[i] << 5) / data->base.range; - return EC_SUCCESS; } diff --git a/driver/accelgyro_lsm6dsm.c b/driver/accelgyro_lsm6dsm.c index c685615f9f..11aceb5347 100644 --- a/driver/accelgyro_lsm6dsm.c +++ b/driver/accelgyro_lsm6dsm.c @@ -4,21 +4,19 @@ */ /** - * LSM6DSM accelerometer and gyro module for Chrome EC + * LSM6DSx (x is L or M) accelerometer and gyro module for Chrome EC * 3D digital accelerometer & 3D digital gyroscope + * This driver supports both devices LSM6DSM and LSM6DSL */ -#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 CPUTS(outstr) cputs(CC_ACCEL, outstr) #define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args) +#define CPRINTS(format, args...) cprints(CC_ACCEL, format, ## args) /** * @return output base register for sensor @@ -30,64 +28,22 @@ static inline int get_xyz_reg(enum motionsensor_type 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 + * Note: Range is sensitivity/gain for speed purpose */ 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; + struct stprivate_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 */ + /* Adjust and check rounded value for acc. */ if (rnd && (newrange < LSM6DSM_ACCEL_NORMALIZE_FS(newrange))) newrange <<= 1; if (newrange > LSM6DSM_ACCEL_FS_MAX_VAL) @@ -95,7 +51,7 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd) reg_val = LSM6DSM_ACCEL_FS_REG(newrange); } else { - /* Adjust and check rounded value for gyro */ + /* Adjust and check rounded value for gyro. */ if (rnd && (newrange < LSM6DSM_GYRO_NORMALIZE_FS(newrange))) newrange <<= 1; if (newrange > LSM6DSM_GYRO_FS_MAX_VAL) @@ -105,18 +61,26 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd) } mutex_lock(s->mutex); - err = write_data_with_mask(s, ctrl_reg, LSM6DSM_RANGE_MASK, reg_val); + err = st_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 */ + /* Save internally gain for speed optimization. */ data->base.range = (s->type == MOTIONSENSE_TYPE_ACCEL ? - LSM6DSM_ACCEL_FS_GAIN(newrange) : LSM6DSM_GYRO_FS_GAIN(newrange)); + LSM6DSM_ACCEL_FS_GAIN(newrange) : + LSM6DSM_GYRO_FS_GAIN(newrange)); mutex_unlock(s->mutex); + return EC_SUCCESS; } +/** + * get_range - get full scale range + * @s: Motion sensor pointer + * + * For mag range is fixed to LIS2MDL_RANGE by hardware + */ static int get_range(const struct motion_sensor_t *s) { - struct lsm6dsm_data *data = s->drv_data; + struct stprivate_data *data = s->drv_data; if (MOTIONSENSE_TYPE_ACCEL == s->type) return LSM6DSM_ACCEL_GAIN_FS(data->base.range); @@ -124,85 +88,61 @@ static int get_range(const struct motion_sensor_t *s) return LSM6DSM_GYRO_GAIN_FS(data->base.range); } -static int get_resolution(const struct motion_sensor_t *s) -{ - /* Only one resolution, LSM6DSM_RESOLUTION, so nothing to do. */ - return LSM6DSM_RESOLUTION; -} - +/** + * set_data_rate + * @s: Motion sensor pointer + * @range: Rate (mHz) + * @rnd: Round up/down flag + * + * For mag in cascade with lsm6dsm/l we use acc trigger and FIFO decimator + */ 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; + int ret, normalized_rate = LSM6DSM_ODR_MIN_VAL; + struct stprivate_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); + /* Power off acc or gyro. */ + mutex_lock(s->mutex); + + ret = st_write_data_with_mask(s, ctrl_reg, LSM6DSM_ODR_MASK, + LSM6DSM_ODR_0HZ_VAL); + if (ret == EC_SUCCESS) + data->base.odr = LSM6DSM_ODR_0HZ_VAL; + + mutex_unlock(s->mutex); + return ret; } + reg_val = LSM6DSM_ODR_TO_REG(rate); + normalized_rate = LSM6DSM_ODR_TO_NORMALIZE(rate); + 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; + /* Adjust rounded value for acc and gyro because ODR are shared. */ + if (reg_val > LSM6DSM_ODR_416HZ_VAL) { + reg_val = LSM6DSM_ODR_416HZ_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); + ret = st_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; + return ret; } static int is_data_ready(const struct motion_sensor_t *s, int *ready) @@ -228,14 +168,13 @@ static int is_data_ready(const struct motion_sensor_t *s, int *ready) * * 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) + * 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 raw[OUT_XYZ_SIZE]; uint8_t xyz_reg; - int ret, i, range, tmp = 0; - struct lsm6dsm_data *data = s->drv_data; + int ret, tmp = 0; ret = is_data_ready(s, &tmp); if (ret != EC_SUCCESS) @@ -254,31 +193,13 @@ static int read(const struct motion_sensor_t *s, vector_3_t v) 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); + /* Read data bytes starting at xyz_reg. */ + ret = st_raw_read_n_noinc(s->port, s->addr, xyz_reg, raw, OUT_XYZ_SIZE); + if (ret != EC_SUCCESS) 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; + /* Apply precision, sensitivity and rotation vector. */ + st_normalize(s, v, raw); return EC_SUCCESS; } @@ -286,6 +207,7 @@ static int read(const struct motion_sensor_t *s, vector_3_t v) static int init(const struct motion_sensor_t *s) { int ret = 0, tmp; + struct stprivate_data *data = s->drv_data; ret = raw_read8(s->port, s->addr, LSM6DSM_WHO_AM_I_REG, &tmp); if (ret != EC_SUCCESS) @@ -295,36 +217,42 @@ static int init(const struct motion_sensor_t *s) 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 + * This sensor can be powered through an EC reboot, so the state of the + * sensor is unknown here so reset it + * LSM6DSM/L supports both accel & gyro features * Board will see two virtual sensor devices: accel & gyro - * Requirement: Accel need be init before gyro + * Requirement: Accel need be init before gyro and mag */ 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); + /* Software reset. */ + ret = st_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); + /* Output data not updated until have been read. */ + ret = st_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); } + /* Set default resolution common to acc and gyro. */ + data->resol = LSM6DSM_RESOLUTION; return sensor_init_done(s); err_unlock: mutex_unlock(s->mutex); - return EC_ERROR_UNKNOWN; + return ret; } const struct accelgyro_drv lsm6dsm_drv = { @@ -332,9 +260,9 @@ const struct accelgyro_drv lsm6dsm_drv = { .read = read, .set_range = set_range, .get_range = get_range, - .get_resolution = get_resolution, + .get_resolution = st_get_resolution, .set_data_rate = set_data_rate, - .get_data_rate = get_data_rate, - .set_offset = set_offset, - .get_offset = get_offset, + .get_data_rate = st_get_data_rate, + .set_offset = st_set_offset, + .get_offset = st_get_offset, }; diff --git a/driver/accelgyro_lsm6dsm.h b/driver/accelgyro_lsm6dsm.h index bf40741140..1c1dea606d 100644 --- a/driver/accelgyro_lsm6dsm.h +++ b/driver/accelgyro_lsm6dsm.h @@ -1,29 +1,27 @@ -/* Copyright (c) 2016 The Chromium OS Authors. All rights reserved. +/* Copyright 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 */ +/* LSM6DSM (also LSM6DSL) Accel and Gyro driver for Chrome EC */ #ifndef __CROS_EC_ACCELGYRO_LSM6DSM_H #define __CROS_EC_ACCELGYRO_LSM6DSM_H -#include "accelgyro.h" +#include "stm_mems_common.h" #define LSM6DSM_I2C_ADDR(__x) (__x << 1) /* - * 7-bit address is 110101Xb. Where 'X' is determined - * by the voltage on the ADDR pin. + * 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) +#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 +#define LSM6DSM_WHO_AM_I 0x6a /* Sensor Software Reset Bit */ #define LSM6DSM_RESET_ADDR 0x12 @@ -33,15 +31,9 @@ #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 @@ -60,20 +52,19 @@ /* Common Acc/Gyro data rate */ enum lsm6dsm_odr { - LSM6DSM_ODR_POWER_OFF_VAL = 0x00, + LSM6DSM_ODR_0HZ_VAL = 0, 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 +#define LSM6DSM_ODR_MAX_VAL 416000 /* ODR reg value from selected data rate in mHz */ #define LSM6DSM_ODR_TO_REG(_odr) \ @@ -83,7 +74,7 @@ enum lsm6dsm_odr { #define LSM6DSM_ODR_TO_NORMALIZE(_odr) \ (LSM6DSM_ODR_MIN_VAL << __fls(_odr/LSM6DSM_ODR_MIN_VAL)) -/* Full Scale range value for Accel */ +/* Full Scale range value and gain for Acc */ #define LSM6DSM_FS_LIST_NUM 4 #define LSM6DSM_ACCEL_FS_ADDR 0x10 @@ -120,7 +111,7 @@ enum lsm6dsm_odr { #define LSM6DSM_ACCEL_NORMALIZE_FS(_fs) \ (1 << (32 - __builtin_clz(_fs / 2))) -/* Full Scale range value for Gyro */ +/* Full Scale range value and gain for Gyro */ #define LSM6DSM_GYRO_FS_ADDR 0x11 #define LSM6DSM_GYRO_FS_MASK 0x0c @@ -154,7 +145,7 @@ enum lsm6dsm_odr { (_fs == 245 ? 245 : 500 << __fls(_fs / 500)) /* FS register address/mask for Acc/Gyro sensors */ -#define LSM6DSM_RANGE_REG(_sensor) (LSM6DSM_ACCEL_FS_ADDR + (_sensor)) +#define LSM6DSM_RANGE_REG(_sensor) (LSM6DSM_ACCEL_FS_ADDR + (_sensor)) #define LSM6DSM_RANGE_MASK 0x0c /* Status register bitmask for Acc/Gyro data ready */ @@ -167,14 +158,9 @@ enum lsm6dsm_status { #define LSM6DSM_STS_XLDA_MASK 0x01 #define LSM6DSM_STS_GDA_MASK 0x02 -/* Sensor resolution in number of bits. This sensor has fixed resolution. */ +/* Sensor resolution in number of bits: fixed 16 bit */ #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 4fa26af667..23eb0a2d65 100644 --- a/driver/build.mk +++ b/driver/build.mk @@ -13,7 +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 +driver-$(CONFIG_ACCELGYRO_LSM6DSM)+=accelgyro_lsm6dsm.o stm_mems_common.o driver-$(CONFIG_ACCEL_LIS2DH)+=accel_lis2dh.o stm_mems_common.o driver-$(CONFIG_SYNC)+=sync.o diff --git a/driver/stm_mems_common.c b/driver/stm_mems_common.c index afffba2d25..c2939ae714 100644 --- a/driver/stm_mems_common.c +++ b/driver/stm_mems_common.c @@ -27,10 +27,7 @@ int raw_write8(const int port, const int addr, const int reg, int data) } /** - * Read n bytes for read - * NOTE: Some chip use MSB for auto-increments in SUB address - * MSB must be set for autoincrement in multi read when auto_inc - * is set + * st_raw_read_n - Read n bytes for read */ int st_raw_read_n(const int port, const int addr, const uint8_t reg, uint8_t *data_ptr, const int len) @@ -46,8 +43,24 @@ int st_raw_read_n(const int port, const int addr, const uint8_t reg, return rv; } +/** + * st_raw_read_n_noinc - Read n bytes for read (no auto inc address) + */ +int st_raw_read_n_noinc(const int port, const int addr, const uint8_t reg, + uint8_t *data_ptr, const int len) +{ + int rv = -EC_ERROR_PARAM1; + + /* TODO: Implement SPI interface support */ + i2c_lock(port, 1); + rv = i2c_xfer(port, addr, ®, 1, data_ptr, len, I2C_XFER_SINGLE); + i2c_lock(port, 0); + + return rv; +} + /** - * write_data_with_mask - Write register with mask + * st_write_data_with_mask - Write register with mask * @s: Motion sensor pointer * @reg: Device register * @mask: The mask to search @@ -72,11 +85,9 @@ int st_write_data_with_mask(const struct motion_sensor_t *s, int reg, return raw_write8(s->port, s->addr, reg, new_data); } - /** - * get_resolution - Get bit resolution +/** + * st_get_resolution - Get bit resolution * @s: Motion sensor pointer - * - * TODO: must support multiple resolution */ int st_get_resolution(const struct motion_sensor_t *s) { @@ -86,7 +97,7 @@ int st_get_resolution(const struct motion_sensor_t *s) } /** - * set_offset - Set data offset + * st_set_offset - Set data offset * @s: Motion sensor pointer * @offset: offset vector * @temp: Temp @@ -103,7 +114,7 @@ int st_set_offset(const struct motion_sensor_t *s, } /** - * get_offset - Get data offset + * st_get_offset - Get data offset * @s: Motion sensor pointer * @offset: offset vector * @temp: Temp @@ -121,7 +132,7 @@ int st_get_offset(const struct motion_sensor_t *s, } /** - * get_data_rate - Get data rate (ODR) + * st_get_data_rate - Get data rate (ODR) * @s: Motion sensor pointer */ int st_get_data_rate(const struct motion_sensor_t *s) @@ -132,34 +143,30 @@ int st_get_data_rate(const struct motion_sensor_t *s) } /** - * normalize - Apply LSB data sens. and rotation based on sensor resolution + * st_normalize - Apply LSB data sens. and rotation based on sensor resolution * @s: Motion sensor pointer * @v: output vector * @data: LSB raw data */ void st_normalize(const struct motion_sensor_t *s, vector_3_t v, uint8_t *data) { - int i; + int i, range; struct stprivate_data *drvdata = s->drv_data; - /* Adjust data to sensor Sensitivity and Precision: - * - devices with 16 bits resolution has gain in ug/LSB - * - devices with 8/10 bits resolution has gain in mg/LSB - */ - for (i = 0; i < 3; i++) { - switch (drvdata->resol) { - case 10: - v[i] = ((int16_t)((data[i * 2 + 1] << 8) | - data[i * 2]) >> 6); - v[i] = v[i] * drvdata->base.range; - break; - case 16: - v[i] = ((int16_t)(data[i * 2 + 1] << 8) | - data[i * 2]); - v[i] = (v[i] * drvdata->base.range) / 1000; - break; - } + for (i = X; i <= Z; i++) { + v[i] = (int16_t)((data[i * 2 + 1] << 8) | + data[i * 2]) >> (16 - drvdata->resol); } rotate(v, *s->rot_standard_ref, v); + + /* apply offset in the device coordinates */ + range = s->drv->get_range(s); + for (i = X; i <= Z; i++) + /* + * TODO(gwendal): only work for accel: + * offsest is in 1/1024th g/LSB, so shift left and divide by + * range to normalize the value. + */ + v[i] += (drvdata->offset[i] << 5) / range; } diff --git a/driver/stm_mems_common.h b/driver/stm_mems_common.h index 848c3d4825..b708812d63 100644 --- a/driver/stm_mems_common.h +++ b/driver/stm_mems_common.h @@ -14,14 +14,13 @@ #include "console.h" #include "i2c.h" -/* Common debug funcions */ -#define CPRINTF(format, args...) cprintf(CC_ACCEL, format "\n", ## args) - /* X, Y, Z axis data len */ #define OUT_XYZ_SIZE 6 +#define ST_NORMALIZE_RATE(_fS) (1 << __fls(_fs)) + #ifdef CONFIG_ACCEL_FIFO -#define FIFO_BUFFER_NUM_PATTERN 16 +#define FIFO_BUFFER_NUM_PATTERN 32 /* Define number of data to be read from FIFO each time * It must be a multiple of OUT_XYZ_SIZE. * In case of LSM6DSM FIFO contains pattern depending ODR @@ -45,13 +44,19 @@ extern inline int raw_write8(const int port, const int addr, const int reg, int data); /** - * Read n bytes for read + * st_raw_read_n - Read n bytes for read */ int st_raw_read_n(const int port, const int addr, const uint8_t reg, uint8_t *data_ptr, const int len); +/** + * st_raw_read_n_noinc - Read n bytes for read (no auto inc address) + */ +int st_raw_read_n_noinc(const int port, const int addr, const uint8_t reg, + uint8_t *data_ptr, const int len); + /** - * write_data_with_mask - Write register with mask + * st_write_data_with_mask - Write register with mask * @s: Motion sensor pointer * @reg: Device register * @mask: The mask to search @@ -61,15 +66,13 @@ int st_write_data_with_mask(const struct motion_sensor_t *s, int reg, uint8_t mask, uint8_t data); /** - * get_resolution - Get bit resolution + * st_get_resolution - Get bit resolution * @s: Motion sensor pointer - * - * TODO: must support multiple resolution */ int st_get_resolution(const struct motion_sensor_t *s); /** - * set_offset - Set data offset + * st_set_offset - Set data offset * @s: Motion sensor pointer * @offset: offset vector * @temp: Temp @@ -78,7 +81,7 @@ int st_set_offset(const struct motion_sensor_t *s, const int16_t *offset, int16_t temp); /** - * get_offset - Get data offset + * st_get_offset - Get data offset * @s: Motion sensor pointer * @offset: offset vector * @temp: Temp @@ -87,13 +90,13 @@ int st_get_offset(const struct motion_sensor_t *s, int16_t *offset, int16_t *temp); /** - * get_data_rate - Get data rate (ODR) + * st_get_data_rate - Get data rate (ODR) * @s: Motion sensor pointer */ int st_get_data_rate(const struct motion_sensor_t *s); /** - * normalize - Apply to LSB data sensitivity and rotation + * st_normalize - Apply to LSB data sensitivity and rotation * @s: Motion sensor pointer * @v: vector * @data: LSB raw data |