diff options
Diffstat (limited to 'driver/accelgyro_lsm6ds0.c')
| -rw-r--r-- | driver/accelgyro_lsm6ds0.c | 12 |
1 files changed, 7 insertions, 5 deletions
diff --git a/driver/accelgyro_lsm6ds0.c b/driver/accelgyro_lsm6ds0.c index 357710d56e..9aa6644670 100644 --- a/driver/accelgyro_lsm6ds0.c +++ b/driver/accelgyro_lsm6ds0.c @@ -375,13 +375,15 @@ static int read(const struct motion_sensor_t *s, vector_3_t v) return ret; } - get_range(s, &range); - for (i = X; i <= Z; i++) { + for (i = X; i <= Z; i++) v[i] = ((int16_t)((raw[i * 2 + 1] << 8) | raw[i * 2])); + + rotate(v, *s->rot_standard_ref, v); + + /* apply offset in the device coordinates */ + get_range(s, &range); + for (i = X; i <= Z; i++) v[i] += (data->offset[i] << 5) / range; - } - if (*s->rot_standard_ref != NULL) - rotate(v, *s->rot_standard_ref, v); return EC_SUCCESS; } |