michaelh/zephyr
1
0
Fork 0
Code Releases Activity

sensors: lis2dh: fix raw value scaling

Browse source 
All values were scaled by 8 instead of 12 for 16g compile-time selected
range. This resulted with values around 6.5 m/s^2. Scaling for runtime
configurable ranges was broken for all except 2g range option due to
totally broken lis2dh_range_to_reg_val().

Fix wrong scaling for 16g compile-time and all runtime selectable ranges
by reworking code that scales raw value. While doing this, change
lis2dh->scale type from 16 to 32 bits. This allows to slightly increase
final result precision by using the fact that raw values have maximum 12
bits precision, allowing us to multiply lis2dh->scale by (1 << 4)
compared to previous implementation.

Fix bug #19872.

Signed-off-by: Marcin Niestroj <m.niestroj@grinn-global.com>
This commit is contained in:
Marcin Niestroj 2019-10-21 23:50:09 +02:00 • committed by Maureen Helm
commit 59648bd1a5
2 changed files with 27 additions and 18 deletions
Download patch file Download diff file Expand all files Collapse all files

42
drivers/sensor/lis2dh/lis2dh.c
View file

@ -13,6 +13,20 @@
LOG_MODULE_REGISTER(lis2dh, CONFIG_SENSOR_LOG_LEVEL);
#include "lis2dh.h"
#define ACCEL_SCALE(sensitivity) \
((SENSOR_G * (sensitivity) >> 14) / 100)
/*
* Use values for low-power mode in DS "Mechanical (Sensor) characteristics",
* multiplied by 100.
*/
static const u32_t lis2dh_reg_val_to_scale[] = {
ACCEL_SCALE(1600),
ACCEL_SCALE(3200),
ACCEL_SCALE(6400),
ACCEL_SCALE(19200),
};
#if defined(DT_ST_LIS2DH_BUS_SPI)
int lis2dh_spi_access(struct lis2dh_data *ctx, u8_t cmd,
void *data, size_t length)
@ -63,18 +77,18 @@ int lis2dh_reg_field_update(struct device *dev, u8_t reg_addr,
(old_val & ~mask) | ((val << pos) & mask));
}
static void lis2dh_convert(s16_t raw_val, u16_t scale,
static void lis2dh_convert(s16_t raw_val, u32_t scale,
struct sensor_value *val)
{
s32_t converted_val;
/*
* maximum converted value we can get is: max(raw_val) * max(scale)
* max(raw_val) = +/- 2^15
* max(scale) = 4785
* max(converted_val) = 156794880 which is less than 2^31
* max(raw_val >> 4) = +/- 2^11
* max(scale) = 114921
* max(converted_val) = 235358208 which is less than 2^31
*/
converted_val = raw_val * scale;
converted_val = (raw_val >> 4) * scale;
val->val1 = converted_val / 1000000;
val->val2 = converted_val % 1000000;
}
@ -210,20 +224,16 @@ static int lis2dh_acc_odr_set(struct device *dev, u16_t freq)
#endif
#ifdef CONFIG_LIS2DH_ACCEL_RANGE_RUNTIME
static const union {
u32_t word_le32;
u8_t fs_values[4];
} lis2dh_acc_range_map = { .fs_values = {2, 4, 8, 16} };
#define LIS2DH_RANGE_IDX_TO_VALUE(idx) (1 << ((idx) + 1))
#define LIS2DH_NUM_RANGES 4
static int lis2dh_range_to_reg_val(u16_t range)
{
int i;
u32_t range_map;
range_map = sys_le32_to_cpu(lis2dh_acc_range_map.word_le32);
for (i = 0; range_map; i++, range_map >>= 1) {
if (range == (range_map & 0xff)) {
for (i = 0; i < LIS2DH_NUM_RANGES; i++) {
if (range == LIS2DH_RANGE_IDX_TO_VALUE(i)) {
return i;
}
}
@ -241,7 +251,7 @@ static int lis2dh_acc_range_set(struct device *dev, s32_t range)
return fs;
}
lis2dh->scale = LIS2DH_ACCEL_SCALE(range);
lis2dh->scale = lis2dh_reg_val_to_scale[fs];
return lis2dh_reg_field_update(dev, LIS2DH_REG_CTRL4,
LIS2DH_FS_SHIFT,
@ -340,7 +350,7 @@ int lis2dh_init(struct device *dev)
}
/* set full scale range and store it for later conversion */
lis2dh->scale = LIS2DH_ACCEL_SCALE(1 << (LIS2DH_FS_IDX + 1));
lis2dh->scale = lis2dh_reg_val_to_scale[LIS2DH_FS_IDX];
status = lis2dh_reg_write_byte(dev, LIS2DH_REG_CTRL4,
LIS2DH_FS_BITS | LIS2DH_HR_BIT);
if (status < 0) {

3
drivers/sensor/lis2dh/lis2dh.h
View file

@ -117,7 +117,6 @@
#define LIS2DH_FS_SELECT(fs) ((fs) << LIS2DH_FS_SHIFT)
#define LIS2DH_FS_BITS (LIS2DH_FS_SELECT(LIS2DH_FS_IDX))
#define LIS2DH_ACCEL_SCALE(range_g) ((SENSOR_G * 2 * (range_g)) / 65636LL)
#if defined(CONFIG_LIS2DH_OPER_MODE_HIGH_RES)
#define LIS2DH_HR_BIT BIT(3)
#else
@ -201,7 +200,7 @@ struct lis2dh_data {
#endif
union lis2dh_sample sample;
/* current scaling factor, in micro m/s^2 / lsb */
u16_t scale;
u32_t scale;
#ifdef CONFIG_LIS2DH_TRIGGER
struct device *gpio_int1;