diff options
Diffstat (limited to 'drivers/iio/magnetometer/yamaha-yas530.c')
| -rw-r--r-- | drivers/iio/magnetometer/yamaha-yas530.c | 13 |
1 files changed, 7 insertions, 6 deletions
diff --git a/drivers/iio/magnetometer/yamaha-yas530.c b/drivers/iio/magnetometer/yamaha-yas530.c index 65011a8598d3..c55a38650c0d 100644 --- a/drivers/iio/magnetometer/yamaha-yas530.c +++ b/drivers/iio/magnetometer/yamaha-yas530.c @@ -372,6 +372,7 @@ static int yas537_measure(struct yas5xx *yas5xx, u16 *t, u16 *x, u16 *y1, u16 *y u8 data[8]; u16 xy1y2[3]; s32 h[3], s[3]; + int half_range = BIT(13); int i, ret; mutex_lock(&yas5xx->lock); @@ -406,13 +407,13 @@ static int yas537_measure(struct yas5xx *yas5xx, u16 *t, u16 *x, u16 *y1, u16 *y /* The second version of YAS537 needs to include calibration coefficients */ if (yas5xx->version == YAS537_VERSION_1) { for (i = 0; i < 3; i++) - s[i] = xy1y2[i] - BIT(13); - h[0] = (c->k * (128 * s[0] + c->a2 * s[1] + c->a3 * s[2])) / BIT(13); - h[1] = (c->k * (c->a4 * s[0] + c->a5 * s[1] + c->a6 * s[2])) / BIT(13); - h[2] = (c->k * (c->a7 * s[0] + c->a8 * s[1] + c->a9 * s[2])) / BIT(13); + s[i] = xy1y2[i] - half_range; + h[0] = (c->k * (128 * s[0] + c->a2 * s[1] + c->a3 * s[2])) / half_range; + h[1] = (c->k * (c->a4 * s[0] + c->a5 * s[1] + c->a6 * s[2])) / half_range; + h[2] = (c->k * (c->a7 * s[0] + c->a8 * s[1] + c->a9 * s[2])) / half_range; for (i = 0; i < 3; i++) { - clamp_val(h[i], -BIT(13), BIT(13) - 1); - xy1y2[i] = h[i] + BIT(13); + h[i] = clamp(h[i], -half_range, half_range - 1); + xy1y2[i] = h[i] + half_range; } } |