summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--driver/accelgyro_lsm6dsm.c71
-rw-r--r--driver/accelgyro_lsm6dsm.h3
-rw-r--r--driver/sensorhub_lsm6dsm.c9
3 files changed, 52 insertions, 31 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 */
diff --git a/driver/accelgyro_lsm6dsm.h b/driver/accelgyro_lsm6dsm.h
index 0be055271e..3336326b90 100644
--- a/driver/accelgyro_lsm6dsm.h
+++ b/driver/accelgyro_lsm6dsm.h
@@ -273,7 +273,8 @@ struct lsm6dsm_data {
#define LSM6DSM_MAIN_SENSOR(_s) ((_s) - (_s)->type)
#ifdef CONFIG_ACCEL_FIFO
-int accelgyro_config_fifo(const struct motion_sensor_t *accel);
+int accelgyro_fifo_disable(const struct motion_sensor_t *s);
+int accelgyro_fifo_enable(const struct motion_sensor_t *s);
#endif /* CONFIG_ACCEL_FIFO */
#endif /* __CROS_EC_ACCELGYRO_LSM6DSM_H */
diff --git a/driver/sensorhub_lsm6dsm.c b/driver/sensorhub_lsm6dsm.c
index 04776b0557..c9989b8c0b 100644
--- a/driver/sensorhub_lsm6dsm.c
+++ b/driver/sensorhub_lsm6dsm.c
@@ -200,7 +200,14 @@ int sensorhub_set_ext_data_rate(const struct motion_sensor_t *s,
xl_rate = st_get_data_rate(s);
*ret_rate = MIN(rate, xl_rate);
#ifdef CONFIG_ACCEL_FIFO
- ret = accelgyro_config_fifo(s);
+ ret = accelgyro_fifo_disable(s);
+ if (ret != EC_SUCCESS)
+ return ret;
+ /*
+ * NOTE: the ODR on the pass-through sensor does not currently change.
+ * If it does, that step needs to be performed here
+ */
+ ret = accelgyro_fifo_enable(s);
#endif
return ret;
}
View Lorry Controller Status