You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 
 

235 lines
7.4 KiB

/*
* Copyright (C) 2014 Freie Universität Berlin
*
* This file is subject to the terms and conditions of the GNU Lesser
* General Public License v2.1. See the file LICENSE in the top level
* directory for more details.
*/
/**
* @ingroup driver_lsm303dlhc
* @{
*
* @file
* @brief Device driver implementation for the LSM303DLHC 3D accelerometer/magnetometer.
*
* @author Thomas Eichinger <thomas.eichinger@fu-berlin.de>
* @author Peter Kietzmann <peter.kietzmann@haw-hamburg.de>
*
* @}
*/
#include "lsm303dlhc.h"
#include "lsm303dlhc-internal.h"
#define ENABLE_DEBUG (0)
#include "debug.h"
int lsm303dlhc_init(lsm303dlhc_t *dev, i2c_t i2c, gpio_t acc_pin, gpio_t mag_pin,
uint8_t acc_address,
lsm303dlhc_acc_sample_rate_t acc_sample_rate,
lsm303dlhc_acc_scale_t acc_scale,
uint8_t mag_address,
lsm303dlhc_mag_sample_rate_t mag_sample_rate,
lsm303dlhc_mag_gain_t mag_gain)
{
int res;
uint8_t tmp;
dev->i2c = i2c;
dev->acc_address = acc_address;
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);
i2c_init_master(i2c, I2C_SPEED_NORMAL);
DEBUG("lsm303dlhc reboot...");
res = i2c_write_reg(dev->i2c, dev->acc_address,
LSM303DLHC_REG_CTRL5_A, LSM303DLHC_REG_CTRL5_A_BOOT);
/* Release the bus for other threads. */
i2c_release(dev->i2c);
DEBUG("[OK]\n");
/* configure accelerometer */
/* enable all three axis and set sample rate */
tmp = (LSM303DLHC_CTRL1_A_XEN
| LSM303DLHC_CTRL1_A_YEN
| LSM303DLHC_CTRL1_A_ZEN
| acc_sample_rate);
i2c_acquire(dev->i2c);
res += i2c_write_reg(dev->i2c, dev->acc_address,
LSM303DLHC_REG_CTRL1_A, tmp);
/* update on read, MSB @ low address, scale and high-resolution */
tmp = (acc_scale | LSM303DLHC_CTRL4_A_HR);
res += i2c_write_reg(dev->i2c, dev->acc_address,
LSM303DLHC_REG_CTRL4_A, tmp);
/* no interrupt generation */
res += i2c_write_reg(dev->i2c, dev->acc_address,
LSM303DLHC_REG_CTRL3_A, LSM303DLHC_CTRL3_A_I1_NONE);
/* configure acc data ready pin */
gpio_init(acc_pin, GPIO_IN);
/* configure magnetometer and temperature */
/* enable temperature output and set sample rate */
tmp = LSM303DLHC_TEMP_EN | mag_sample_rate;
res += i2c_write_reg(dev->i2c, dev->mag_address,
LSM303DLHC_REG_CRA_M, tmp);
/* configure z-axis gain */
res += i2c_write_reg(dev->i2c, dev->mag_address,
LSM303DLHC_REG_CRB_M, mag_gain);
/* set continuous mode */
res += i2c_write_reg(dev->i2c, dev->mag_address,
LSM303DLHC_REG_MR_M, LSM303DLHC_MAG_MODE_CONTINUOUS);
i2c_release(dev->i2c);
/* configure mag data ready pin */
gpio_init(mag_pin, GPIO_IN);
return (res < 7) ? -1 : 0;
}
int lsm303dlhc_read_acc(lsm303dlhc_t *dev, lsm303dlhc_3d_data_t *data)
{
int res;
uint8_t tmp;
i2c_acquire(dev->i2c);
i2c_read_reg(dev->i2c, dev->acc_address, LSM303DLHC_REG_STATUS_A, &tmp);
DEBUG("lsm303dlhc status: %x\n", tmp);
DEBUG("lsm303dlhc: wait for acc values ... ");
res = i2c_read_reg(dev->i2c, dev->acc_address,
LSM303DLHC_REG_OUT_X_L_A, &tmp);
data->x_axis = tmp;
res += i2c_read_reg(dev->i2c, dev->acc_address,
LSM303DLHC_REG_OUT_X_H_A, &tmp);
data->x_axis |= tmp<<8;
res += i2c_read_reg(dev->i2c, dev->acc_address,
LSM303DLHC_REG_OUT_Y_L_A, &tmp);
data->y_axis = tmp;
res += i2c_read_reg(dev->i2c, dev->acc_address,
LSM303DLHC_REG_OUT_Y_H_A, &tmp);
data->y_axis |= tmp<<8;
res += i2c_read_reg(dev->i2c, dev->acc_address,
LSM303DLHC_REG_OUT_Z_L_A, &tmp);
data->z_axis = tmp;
res += i2c_read_reg(dev->i2c, dev->acc_address,
LSM303DLHC_REG_OUT_Z_H_A, &tmp);
data->z_axis |= tmp<<8;
i2c_release(dev->i2c);
DEBUG("read ... ");
data->x_axis = data->x_axis>>4;
data->y_axis = data->y_axis>>4;
data->z_axis = data->z_axis>>4;
if (res < 6) {
DEBUG("[!!failed!!]\n");
return -1;
}
DEBUG("[done]\n");
return 0;
}
int lsm303dlhc_read_mag(lsm303dlhc_t *dev, lsm303dlhc_3d_data_t *data)
{
int res;
DEBUG("lsm303dlhc: wait for mag values... ");
while (gpio_read(dev->mag_pin) == 0){}
DEBUG("read ... ");
i2c_acquire(dev->i2c);
res = i2c_read_regs(dev->i2c, dev->mag_address,
LSM303DLHC_REG_OUT_X_H_M, data, 6);
i2c_release(dev->i2c);
if (res < 6) {
DEBUG("[!!failed!!]\n");
return -1;
}
DEBUG("[done]\n");
/* interchange y and z axis and fix endiness */
int16_t tmp = data->y_axis;
data->x_axis = ((data->x_axis<<8)|((data->x_axis>>8)&0xff));
data->y_axis = ((data->z_axis<<8)|((data->z_axis>>8)&0xff));
data->z_axis = ((tmp<<8)|((tmp>>8)&0xff));
/* compensate z-axis sensitivity */
/* gain is currently hardcoded to LSM303DLHC_GAIN_5 */
data->z_axis = ((data->z_axis * 400) / 355);
return 0;
}
int lsm303dlhc_read_temp(lsm303dlhc_t *dev, int16_t *value)
{
int res;
i2c_acquire(dev->i2c);
res = i2c_read_regs(dev->i2c, dev->mag_address, LSM303DLHC_REG_TEMP_OUT_H, value, 2);
i2c_release(dev->i2c);
if (res < 2) {
return -1;
}
*value = (((*value) >> 8) & 0xff) | (*value << 8);
DEBUG("LSM303DLHC: raw temp: %i\n", *value);
return 0;
}
int lsm303dlhc_disable(lsm303dlhc_t *dev)
{
int res;
i2c_acquire(dev->i2c);
res = i2c_write_reg(dev->i2c, dev->acc_address,
LSM303DLHC_REG_CTRL1_A, LSM303DLHC_CTRL1_A_POWEROFF);
res += i2c_write_reg(dev->i2c, dev->mag_address,
LSM303DLHC_REG_MR_M, LSM303DLHC_MAG_MODE_SLEEP);
res += i2c_write_reg(dev->i2c, dev->acc_address,
LSM303DLHC_REG_CRA_M, LSM303DLHC_TEMP_DIS);
i2c_release(dev->i2c);
return (res < 3) ? -1 : 0;
}
int lsm303dlhc_enable(lsm303dlhc_t *dev)
{
int res;
uint8_t tmp = (LSM303DLHC_CTRL1_A_XEN
| LSM303DLHC_CTRL1_A_YEN
| LSM303DLHC_CTRL1_A_ZEN
| LSM303DLHC_CTRL1_A_N1344HZ_L5376HZ);
i2c_acquire(dev->i2c);
res = i2c_write_reg(dev->i2c, dev->acc_address, LSM303DLHC_REG_CTRL1_A, tmp);
tmp = (LSM303DLHC_CTRL4_A_BDU| LSM303DLHC_CTRL4_A_SCALE_2G | LSM303DLHC_CTRL4_A_HR);
res += i2c_write_reg(dev->i2c, dev->acc_address, LSM303DLHC_REG_CTRL4_A, tmp);
res += i2c_write_reg(dev->i2c, dev->acc_address, LSM303DLHC_REG_CTRL3_A, LSM303DLHC_CTRL3_A_I1_DRDY1);
gpio_init(dev->acc_pin, GPIO_IN);
tmp = LSM303DLHC_TEMP_EN | LSM303DLHC_TEMP_SAMPLE_75HZ;
res += i2c_write_reg(dev->i2c, dev->mag_address, LSM303DLHC_REG_CRA_M, tmp);
res += i2c_write_reg(dev->i2c, dev->mag_address,
LSM303DLHC_REG_CRB_M, LSM303DLHC_GAIN_5);
res += i2c_write_reg(dev->i2c, dev->mag_address,
LSM303DLHC_REG_MR_M, LSM303DLHC_MAG_MODE_CONTINUOUS);
i2c_release(dev->i2c);
gpio_init(dev->mag_pin, GPIO_IN);
return (res < 6) ? -1 : 0;
}