diff options
Diffstat (limited to 'driver/accelgyro_lsm6dsm.c')
| -rw-r--r-- | driver/accelgyro_lsm6dsm.c | 71 |
1 files changed, 42 insertions, 29 deletions
diff --git a/driver/accelgyro_lsm6dsm.c b/driver/accelgyro_lsm6dsm.c index 30650eebe7..cb36de8900 100644 --- a/driver/accelgyro_lsm6dsm.c +++ b/driver/accelgyro_lsm6dsm.c @@ -71,7 +71,7 @@ static int config_interrupt(const struct motion_sensor_t *s) * @s: Motion sensor pointer: must be MOTIONSENSE_TYPE_ACCEL. * @fmode: BYPASS or CONTINUOUS */ -static int fifo_disable(const struct motion_sensor_t *s) +int accelgyro_fifo_disable(const struct motion_sensor_t *s) { return st_raw_write8(s->port, s->addr, LSM6DSM_FIFO_CTRL5_ADDR, 0x00); } @@ -93,13 +93,14 @@ static void fifo_reset_pattern(struct lsm6dsm_data *private) * Configure FIFO decimator to have every time the right pattern * with acc/gyro */ -static int fifo_enable(const struct motion_sensor_t *accel) +int accelgyro_fifo_enable(const struct motion_sensor_t *s) { int err, i, rate; uint8_t decimator[FIFO_DEV_NUM] = { 0 }; unsigned int min_odr = LSM6DSM_ODR_MAX_VAL; unsigned int max_odr = 0; - struct lsm6dsm_data *private = accel->drv_data; + uint8_t odr_reg_val; + struct lsm6dsm_data *private = s->drv_data; /* In FIFO sensors are mapped in a different way. */ uint8_t agm_maps[] = { MOTIONSENSE_TYPE_GYRO, @@ -111,7 +112,7 @@ static int fifo_enable(const struct motion_sensor_t *accel) /* Search for min and max odr values for acc, gyro. */ for (i = FIFO_DEV_GYRO; i < FIFO_DEV_NUM; i++) { /* Check if sensor enabled with ODR. */ - rate = st_get_data_rate(accel + agm_maps[i]); + rate = st_get_data_rate(s + agm_maps[i]); if (rate > 0) { min_odr = MIN(min_odr, rate); max_odr = MAX(max_odr, rate); @@ -123,10 +124,16 @@ static int fifo_enable(const struct motion_sensor_t *accel) return EC_SUCCESS; } + /* FIFO ODR must be set before the decimation factors */ + odr_reg_val = LSM6DSM_ODR_TO_REG(max_odr) << + LSM6DSM_FIFO_CTRL5_ODR_OFF; + err = st_raw_write8(s->port, s->addr, LSM6DSM_FIFO_CTRL5_ADDR, + odr_reg_val); + /* Scan all sensors configuration to calculate FIFO decimator. */ private->config.total_samples_in_pattern = 0; for (i = FIFO_DEV_GYRO; i < FIFO_DEV_NUM; i++) { - rate = st_get_data_rate(accel + agm_maps[i]); + rate = st_get_data_rate(s + agm_maps[i]); if (rate > 0) { private->config.samples_in_pattern[i] = rate / min_odr; decimator[i] = LSM6DSM_FIFO_DECIMATOR(max_odr / rate); @@ -137,17 +144,19 @@ static int fifo_enable(const struct motion_sensor_t *accel) private->config.samples_in_pattern[i] = 0; } } - st_raw_write8(accel->port, accel->addr, LSM6DSM_FIFO_CTRL3_ADDR, + st_raw_write8(s->port, s->addr, LSM6DSM_FIFO_CTRL3_ADDR, (decimator[FIFO_DEV_GYRO] << LSM6DSM_FIFO_DEC_G_OFF) | (decimator[FIFO_DEV_ACCEL] << LSM6DSM_FIFO_DEC_XL_OFF)); #ifdef CONFIG_MAG_LSM6DSM_LIS2MDL - st_raw_write8(accel->port, accel->addr, LSM6DSM_FIFO_CTRL4_ADDR, + st_raw_write8(s->port, s->addr, LSM6DSM_FIFO_CTRL4_ADDR, decimator[FIFO_DEV_MAG]); #endif /* CONFIG_MAG_LSM6DSM_LIS2MDL */ - err = st_raw_write8(accel->port, accel->addr, LSM6DSM_FIFO_CTRL5_ADDR, - (LSM6DSM_ODR_TO_REG(max_odr) << - LSM6DSM_FIFO_CTRL5_ODR_OFF) | - LSM6DSM_FIFO_MODE_CONTINUOUS_VAL); + /* + * After ODR and decimation values are set, continuous mode can be + * enabled + */ + err = st_raw_write8(s->port, s->addr, LSM6DSM_FIFO_CTRL5_ADDR, + odr_reg_val | LSM6DSM_FIFO_MODE_CONTINUOUS_VAL); if (err != EC_SUCCESS) return err; fifo_reset_pattern(private); @@ -252,6 +261,16 @@ static int load_fifo(struct motion_sensor_t *s, const struct fstatus *fsts) left *= sizeof(uint16_t); left = (left / OUT_XYZ_SIZE) * OUT_XYZ_SIZE; + /* + * TODO(b/122912601): phaser360: Investigate Standard Deviation error + * during CtsSensorTests + * - track number of samples to throw out after ODR changes + * Accel discard: "should" be 0 for freq <= 26, 1 until 1666 Hz (table + * 17) + * Gyro discard: 12.5 Hz - 2, 26-833 Hz - 3 (table 19) + * - check "pattern" register versus where code thinks it is parsing + */ + /* Push all data on upper side. */ do { /* Fit len to pre-allocated static buffer. */ @@ -279,22 +298,6 @@ static int load_fifo(struct motion_sensor_t *s, const struct fstatus *fsts) return EC_SUCCESS; } - -/** - * accelgyro_config_fifo - update mode and ODR for FIFO decimator - */ -int accelgyro_config_fifo(const struct motion_sensor_t *accel) -{ - int err; - - /* Changing in ODR must stop FIFO. */ - err = fifo_disable(accel); - if (err != EC_SUCCESS) - return err; - - - return fifo_enable(accel); -} #endif /* CONFIG_ACCEL_FIFO */ /** @@ -437,6 +440,14 @@ static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd) struct stprivate_data *data = s->drv_data; uint8_t ctrl_reg, reg_val = 0; +#ifdef CONFIG_ACCEL_FIFO + /* FIFO must be disabled before setting any ODR values */ + ret = accelgyro_fifo_disable(LSM6DSM_MAIN_SENSOR(s)); + if (ret != EC_SUCCESS) { + CPRINTS("Failed to disable FIFO. Error: %d", ret); + return ret; + } +#endif ctrl_reg = LSM6DSM_ODR_REG(s->type); if (rate > 0) { reg_val = LSM6DSM_ODR_TO_REG(rate); @@ -457,7 +468,9 @@ static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd) if (ret == EC_SUCCESS) { data->base.odr = normalized_rate; #ifdef CONFIG_ACCEL_FIFO - accelgyro_config_fifo(LSM6DSM_MAIN_SENSOR(s)); + ret = accelgyro_fifo_enable(LSM6DSM_MAIN_SENSOR(s)); + if (ret != EC_SUCCESS) + CPRINTS("Failed to enable FIFO. Error: %d", ret); #endif } @@ -569,7 +582,7 @@ static int init(const struct motion_sensor_t *s) goto err_unlock; #ifdef CONFIG_ACCEL_FIFO - ret = fifo_disable(s); + ret = accelgyro_fifo_disable(s); if (ret != EC_SUCCESS) goto err_unlock; #endif /* CONFIG_ACCEL_FIFO */ |