diff options
Diffstat (limited to 'driver')
| -rw-r--r-- | driver/accelgyro_lsm6ds0.c | 74 | ||||
| -rw-r--r-- | driver/accelgyro_lsm6ds0.h | 5 |
2 files changed, 47 insertions, 32 deletions
diff --git a/driver/accelgyro_lsm6ds0.c b/driver/accelgyro_lsm6ds0.c index 47f243eb44..e7a3a2698b 100644 --- a/driver/accelgyro_lsm6ds0.c +++ b/driver/accelgyro_lsm6ds0.c @@ -169,7 +169,7 @@ static int set_range(const struct motion_sensor_t *s, int ret, ctrl_val, range_tbl_size; uint8_t ctrl_reg, reg_val; const struct accel_param_pair *ranges; - struct motion_data_t *data = (struct motion_data_t *)s->drv_data; + struct lsm6ds0_data *data = s->drv_data; ctrl_reg = get_ctrl_reg(s->type); ranges = get_range_table(s->type, &range_tbl_size); @@ -191,7 +191,7 @@ static int set_range(const struct motion_sensor_t *s, /* Now that we have set the range, update the driver's value. */ if (ret == EC_SUCCESS) - data->range = get_engineering_val(reg_val, ranges, + data->base.range = get_engineering_val(reg_val, ranges, range_tbl_size); accel_cleanup: @@ -202,9 +202,9 @@ accel_cleanup: static int get_range(const struct motion_sensor_t *s, int *range) { - struct motion_data_t *data = (struct motion_data_t *)s->drv_data; + struct lsm6ds0_data *data = s->drv_data; - *range = data->range; + *range = data->base.range; return EC_SUCCESS; } @@ -230,7 +230,7 @@ static int set_data_rate(const struct motion_sensor_t *s, int ret, val, odr_tbl_size; uint8_t ctrl_reg, reg_val; const struct accel_param_pair *data_rates; - struct motion_data_t *data = s->drv_data; + struct lsm6ds0_data *data = s->drv_data; ctrl_reg = get_ctrl_reg(s->type); data_rates = get_odr_table(s->type, &odr_tbl_size); @@ -251,7 +251,7 @@ static int set_data_rate(const struct motion_sensor_t *s, /* Now that we have set the odr, update the driver's value. */ if (ret == EC_SUCCESS) - data->odr = get_engineering_val(reg_val, data_rates, + data->base.odr = get_engineering_val(reg_val, data_rates, odr_tbl_size); /* CTRL_REG3_G 12h @@ -280,9 +280,33 @@ accel_cleanup: static int get_data_rate(const struct motion_sensor_t *s, int *rate) { - struct motion_data_t *data = s->drv_data; + struct lsm6ds0_data *data = s->drv_data; - *rate = data->odr; + *rate = data->base.odr; + return EC_SUCCESS; +} + +static int set_offset(const struct motion_sensor_t *s, + const int16_t *offset, + int16_t temp) +{ + /* temperature is ignored */ + struct lsm6ds0_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 lsm6ds0_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; } @@ -316,9 +340,10 @@ static int is_data_ready(const struct motion_sensor_t *s, int *ready) static int read(const struct motion_sensor_t *s, vector_3_t v) { - uint8_t data[6]; + uint8_t raw[6]; uint8_t xyz_reg; - int ret, tmp = 0, range = 0; + int ret, range, i, tmp = 0; + struct lsm6ds0_data *data = s->drv_data; ret = is_data_ready(s, &tmp); if (ret != EC_SUCCESS) @@ -341,7 +366,7 @@ static int read(const struct motion_sensor_t *s, vector_3_t v) /* 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); + &xyz_reg, 1, raw, 6, I2C_XFER_SINGLE); i2c_lock(I2C_PORT_ACCEL, 0); if (ret != EC_SUCCESS) { @@ -350,27 +375,10 @@ static int read(const struct motion_sensor_t *s, vector_3_t v) return ret; } - v[0] = ((int16_t)((data[1] << 8) | data[0])); - v[1] = ((int16_t)((data[3] << 8) | data[2])); - v[2] = ((int16_t)((data[5] << 8) | data[4])); - - ret = get_range(s, &range); - if (ret) - return EC_ERROR_UNKNOWN; - - v[0] *= range; - v[1] *= range; - v[2] *= range; - - /* normalize the accel scale: 1G = 1024 */ - if (MOTIONSENSE_TYPE_ACCEL == s->type) { - v[0] >>= 5; - v[1] >>= 5; - v[2] >>= 5; - } else { - v[0] >>= 8; - v[1] >>= 8; - v[2] >>= 8; + get_range(s, &range); + for (i = X; i <= Z; i++) { + v[i] = ((int16_t)((raw[i * 2 + 1] << 8) | raw[i * 2])); + v[i] += (data->offset[i] << 5) / range; } return EC_SUCCESS; @@ -456,6 +464,8 @@ const struct accelgyro_drv lsm6ds0_drv = { .get_resolution = get_resolution, .set_data_rate = set_data_rate, .get_data_rate = get_data_rate, + .set_offset = set_offset, + .get_offset = get_offset, #ifdef CONFIG_ACCEL_INTERRUPTS .set_interrupt = set_interrupt, #endif diff --git a/driver/accelgyro_lsm6ds0.h b/driver/accelgyro_lsm6ds0.h index 93e21f55de..82b675db09 100644 --- a/driver/accelgyro_lsm6ds0.h +++ b/driver/accelgyro_lsm6ds0.h @@ -8,6 +8,7 @@ #ifndef __CROS_EC_ACCELGYRO_LSM6DS0_H #define __CROS_EC_ACCELGYRO_LSM6DS0_H +#include "motion_sense.h" #include "task.h" /* @@ -117,5 +118,9 @@ enum lsm6ds0_bdu { #define LSM6DS0_RESOLUTION 16 extern const struct accelgyro_drv lsm6ds0_drv; +struct lsm6ds0_data { + struct motion_data_t base; + int16_t offset[3]; +}; #endif /* __CROS_EC_ACCELGYRO_LSM6DS0_H */ |