diff --git a/drivers/include/lsm303dlhc.h b/drivers/include/lsm303dlhc.h index 07613c68f..be7e5bd18 100644 --- a/drivers/include/lsm303dlhc.h +++ b/drivers/include/lsm303dlhc.h @@ -101,11 +101,13 @@ typedef struct { * @brief Device descriptor for LSM303DLHC sensors */ typedef struct { - i2c_t i2c; /**< I2C device */ - uint8_t acc_address; /**< accelerometer's I2C address */ - uint8_t mag_address; /**< magnetometer's I2C address */ - gpio_t acc_pin; /**< accelerometer's data ready pin */ - gpio_t mag_pin; /**< magnetometer's data ready pin */ + i2c_t i2c; /**< I2C device */ + uint8_t acc_address; /**< accelerometer's I2C address */ + uint8_t mag_address; /**< magnetometer's I2C address */ + gpio_t acc_pin; /**< accelerometer's data ready pin */ + gpio_t mag_pin; /**< magnetometer's data ready pin */ + lsm303dlhc_acc_scale_t acc_scale; /**< accelerometer scale factor */ + lsm303dlhc_mag_gain_t mag_gain; /**< magnetometer gain */ } lsm303dlhc_t; /** @@ -159,7 +161,7 @@ int lsm303dlhc_init(lsm303dlhc_t *dev, i2c_t i2c, gpio_t acc_pin, gpio_t mag_pin * +- 2g | 1*10^-3 * +- 4g | 2*10^-3 * +- 8g | 4*10^-3 - * +-16g | 12*10^-3 + * +-16g | 8*10^-3 * * @param[in] dev device descriptor of an LSM303DLHC device * @param[out] data the measured accelerometer data diff --git a/drivers/lsm303dlhc/lsm303dlhc.c b/drivers/lsm303dlhc/lsm303dlhc.c index 7c149364c..e79a8a79d 100644 --- a/drivers/lsm303dlhc/lsm303dlhc.c +++ b/drivers/lsm303dlhc/lsm303dlhc.c @@ -41,6 +41,8 @@ int lsm303dlhc_init(lsm303dlhc_t *dev, i2c_t i2c, gpio_t acc_pin, gpio_t mag_pin dev->mag_address = mag_address; dev->acc_pin = acc_pin; dev->mag_pin = mag_pin; + dev->acc_scale = acc_scale; + dev->mag_gain = mag_gain; /* Acquire exclusive access to the bus. */ i2c_acquire(dev->i2c); diff --git a/drivers/lsm303dlhc/lsm303dlhc_saul.c b/drivers/lsm303dlhc/lsm303dlhc_saul.c index a05660c18..3af60c22d 100644 --- a/drivers/lsm303dlhc/lsm303dlhc_saul.c +++ b/drivers/lsm303dlhc/lsm303dlhc_saul.c @@ -27,8 +27,15 @@ static int read_acc(void *dev, phydat_t *res) { lsm303dlhc_t *d = (lsm303dlhc_t *)dev; lsm303dlhc_read_acc(d, (lsm303dlhc_3d_data_t *)res); + + /* normalize result */ + int fac = (1 << (d->acc_scale >> 4)); + for (int i = 0; i < 3; i++) { + res->val[i] *= fac; + } + res->unit = UNIT_G; - res->scale = 0; + res->scale = -3; return 3; } @@ -36,8 +43,27 @@ static int read_mag(void *dev, phydat_t *res) { lsm303dlhc_t *d = (lsm303dlhc_t *)dev; lsm303dlhc_read_mag(d, (lsm303dlhc_3d_data_t *)res); + + /* normalize results */ + int gain; + switch (d->mag_gain) { + case LSM303DLHC_MAG_GAIN_1100_980_GAUSS: gain = 1100; break; + case LSM303DLHC_MAG_GAIN_855_760_GAUSS: gain = 855; break; + case LSM303DLHC_MAG_GAIN_670_600_GAUSS: gain = 670; break; + case LSM303DLHC_MAG_GAIN_450_400_GAUSS: gain = 450; break; + case LSM303DLHC_MAG_GAIN_400_355_GAUSS: gain = 400; break; + case LSM303DLHC_MAG_GAIN_330_295_GAUSS: gain = 330; break; + case LSM303DLHC_MAG_GAIN_230_205_GAUSS: gain = 230; break; + default: gain = 1000; break; + } + for (int i = 0; i < 3; i++) { + int32_t tmp = res->val[i] * 1000; + tmp /= gain; + res->val[i] = (int16_t)tmp; + } + res->unit = UNIT_GS; - res->scale = 0; + res->scale = -3; return 3; }