diff options
| -rw-r--r-- | board/fleex/board.c | 2 | ||||
| -rw-r--r-- | board/meep/board.c | 2 | ||||
| -rw-r--r-- | board/phaser/board.c | 2 | ||||
| -rw-r--r-- | board/yorp/board.c | 2 | ||||
| -rw-r--r-- | driver/accelgyro_lsm6dsm.c | 20 | ||||
| -rw-r--r-- | driver/accelgyro_lsm6dsm.h | 38 |
6 files changed, 26 insertions, 40 deletions
diff --git a/board/fleex/board.c b/board/fleex/board.c index 879c4fc959..eba235f85b 100644 --- a/board/fleex/board.c +++ b/board/fleex/board.c @@ -192,7 +192,7 @@ struct motion_sensor_t motion_sensors[] = { .drv_data = &lsm6dsm_g_data, .port = I2C_PORT_SENSOR, .addr = LSM6DSM_ADDR0, - .default_range = 1000, /* dps */ + .default_range = 1000 | ROUND_UP_FLAG, /* dps */ .rot_standard_ref = &base_standard_ref, .min_frequency = LSM6DSM_ODR_MIN_VAL, .max_frequency = LSM6DSM_ODR_MAX_VAL, diff --git a/board/meep/board.c b/board/meep/board.c index 591e8758b9..df754ac8dd 100644 --- a/board/meep/board.c +++ b/board/meep/board.c @@ -202,7 +202,7 @@ struct motion_sensor_t motion_sensors[] = { .drv_data = &lsm6dsm_g_data, .port = I2C_PORT_SENSOR, .addr = LSM6DSM_ADDR0, - .default_range = 1000, /* dps */ + .default_range = 1000 | ROUND_UP_FLAG, /* dps */ .rot_standard_ref = &base_standard_ref, .min_frequency = LSM6DSM_ODR_MIN_VAL, .max_frequency = LSM6DSM_ODR_MAX_VAL, diff --git a/board/phaser/board.c b/board/phaser/board.c index ee3c8601cb..1efa2c41cd 100644 --- a/board/phaser/board.c +++ b/board/phaser/board.c @@ -179,7 +179,7 @@ struct motion_sensor_t motion_sensors[] = { .drv_data = &lsm6dsm_g_data, .port = I2C_PORT_SENSOR, .addr = LSM6DSM_ADDR0, - .default_range = 1000, /* dps */ + .default_range = 1000 | ROUND_UP_FLAG, /* dps */ .rot_standard_ref = &standard_rot_ref, .min_frequency = LSM6DSM_ODR_MIN_VAL, .max_frequency = LSM6DSM_ODR_MAX_VAL, diff --git a/board/yorp/board.c b/board/yorp/board.c index 0cfeccbd2e..7c3bc7adae 100644 --- a/board/yorp/board.c +++ b/board/yorp/board.c @@ -175,7 +175,7 @@ struct motion_sensor_t motion_sensors[] = { .drv_data = &lsm6dsm_g_data, .port = I2C_PORT_SENSOR, .addr = LSM6DSM_ADDR0, - .default_range = 1000, /* dps */ + .default_range = 1000 | ROUND_UP_FLAG, /* dps */ .rot_standard_ref = &base_standard_ref, .min_frequency = LSM6DSM_ODR_MIN_VAL, .max_frequency = LSM6DSM_ODR_MAX_VAL, diff --git a/driver/accelgyro_lsm6dsm.c b/driver/accelgyro_lsm6dsm.c index 7f9ac5b983..30650eebe7 100644 --- a/driver/accelgyro_lsm6dsm.c +++ b/driver/accelgyro_lsm6dsm.c @@ -375,22 +375,20 @@ 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. */ - if (rnd && (newrange < LSM6DSM_GYRO_NORMALIZE_FS(newrange))) - newrange *= 2; - - if (newrange > LSM6DSM_GYRO_FS_MAX_VAL) - newrange = LSM6DSM_GYRO_FS_MAX_VAL; + reg_val = LSM6DSM_GYRO_FS_REG(range); + if (rnd && (range > LSM6DSM_GYRO_NORMALIZE_FS(reg_val))) + reg_val++; - reg_val = LSM6DSM_GYRO_FS_REG(newrange); + if (reg_val > LSM6DSM_GYRO_FS_MAX_REG_VAL) + reg_val = LSM6DSM_GYRO_FS_MAX_REG_VAL; + newrange = LSM6DSM_GYRO_NORMALIZE_FS(reg_val); } mutex_lock(s->mutex); err = st_write_data_with_mask(s, ctrl_reg, LSM6DSM_RANGE_MASK, reg_val); if (err == EC_SUCCESS) /* Save internally gain for speed optimization. */ - data->base.range = (s->type == MOTIONSENSE_TYPE_ACCEL ? - newrange : - LSM6DSM_GYRO_FS_GAIN(newrange)); + data->base.range = newrange; mutex_unlock(s->mutex); return EC_SUCCESS; @@ -414,9 +412,7 @@ static int get_range(const struct motion_sensor_t *s) */ struct stprivate_data *data = s->drv_data; - if (s->type == MOTIONSENSE_TYPE_ACCEL) - return data->base.range; - return LSM6DSM_GYRO_GAIN_FS(data->base.range); + return data->base.range; } /** diff --git a/driver/accelgyro_lsm6dsm.h b/driver/accelgyro_lsm6dsm.h index 72c21fa5f3..0be055271e 100644 --- a/driver/accelgyro_lsm6dsm.h +++ b/driver/accelgyro_lsm6dsm.h @@ -195,34 +195,24 @@ struct fstatus { #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 << __fls(_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))) +/* Supported gyroscope ranges: + * name(dps) | register | gain(udps/LSB) | actual value(dps) + * 250 | 0 | 8750 | 286.72 + * 500 | 1 | 17500 | 573.44 + * 1000 | 2 | 35000 | 1146.88 + * 2000 | 3 | 70000 | 2293.76 + */ +#define LSM6DSM_GYRO_FS_MIN_VAL_MDPS ((8750 << 15) / 1000) +#define LSM6DSM_GYRO_FS_MAX_REG_VAL 3 -/* Gyro Reg value from Full Scale */ +/* Gyro Reg value for Full Scale selection */ #define LSM6DSM_GYRO_FS_REG(_fs) \ - __fls(_fs / 245) + __fls(MAX(1, (_fs * 1000) / LSM6DSM_GYRO_FS_MIN_VAL_MDPS)) -/* Gyro normalized FS value from Full Scale: for Gyro Gains are not multiple */ -#define LSM6DSM_GYRO_NORMALIZE_FS(_fs) \ - (_fs == 245 ? 245 : 500 << __fls(_fs / 500)) +/* Gyro normalized FS value from Full Scale register */ +#define LSM6DSM_GYRO_NORMALIZE_FS(_reg) \ + ((LSM6DSM_GYRO_FS_MIN_VAL_MDPS << (_reg)) / 1000) /* FS register address/mask for Acc/Gyro sensors */ #define LSM6DSM_RANGE_REG(_sensor) (LSM6DSM_ACCEL_FS_ADDR + (_sensor)) |