diff options
| -rw-r--r-- | driver/accel_lis2dw12.c | 26 | ||||
| -rw-r--r-- | driver/accel_lis2dw12.h | 8 |
2 files changed, 24 insertions, 10 deletions
diff --git a/driver/accel_lis2dw12.c b/driver/accel_lis2dw12.c index 66edd8723e..f51f83b5ce 100644 --- a/driver/accel_lis2dw12.c +++ b/driver/accel_lis2dw12.c @@ -356,6 +356,28 @@ static int get_range(const struct motion_sensor_t *s) return data->base.range; } +/** + * ODR reg value from selected data rate in mHz. + */ +static uint8_t odr_to_reg(int odr) +{ + if (odr <= LIS2DW12_ODR_MIN_VAL) + return LIS2DW12_ODR_12HZ_VAL; + + return (__fls(odr / LIS2DW12_ODR_MIN_VAL) + LIS2DW12_ODR_12HZ_VAL); +} + +/** + * Normalized ODR value from selected data rate in mHz. + */ +static int odr_to_normalize(int odr) +{ + if (odr <= LIS2DW12_ODR_MIN_VAL) + return LIS2DW12_ODR_MIN_VAL; + + return (LIS2DW12_ODR_MIN_VAL << (__fls(odr / LIS2DW12_ODR_MIN_VAL))); +} + static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd) { int ret, normalized_rate; @@ -381,8 +403,8 @@ static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd) goto unlock_rate; } - reg_val = LIS2DW12_ODR_TO_REG(rate); - normalized_rate = LIS2DW12_ODR_TO_NORMALIZE(rate); + reg_val = odr_to_reg(rate); + normalized_rate = odr_to_normalize(rate); if (rnd && (normalized_rate < rate)) { reg_val++; diff --git a/driver/accel_lis2dw12.h b/driver/accel_lis2dw12.h index a4d183de78..225af1b748 100644 --- a/driver/accel_lis2dw12.h +++ b/driver/accel_lis2dw12.h @@ -178,14 +178,6 @@ enum lis2dw12_odr { #define LIS2DW12_ODR_MIN_VAL 12500 #define LIS2DW12_ODR_MAX_VAL 1600000 -/* ODR reg value from selected data rate in mHz. */ -#define LIS2DW12_ODR_TO_REG(_odr) \ - (__fls(_odr / LIS2DW12_ODR_MIN_VAL) + LIS2DW12_ODR_12HZ_VAL) - -/* Normalized ODR value from selected data rate in mHz. */ -#define LIS2DW12_ODR_TO_NORMALIZE(_odr) \ - (LIS2DW12_ODR_MIN_VAL << (__fls(_odr / LIS2DW12_ODR_MIN_VAL))) - /* Full scale range registers. */ #define LIS2DW12_FS_ADDR LIS2DW12_CTRL6_ADDR #define LIS2DW12_FS_MASK 0x30 |