From 73d81494905730daa5c650ab1f5ae5d5aad5484f Mon Sep 17 00:00:00 2001 From: Vincent Dupont Date: Thu, 16 Mar 2017 16:44:04 +0100 Subject: [PATCH] drivers: add lsm6dsl imu driver --- drivers/Makefile.dep | 5 + drivers/Makefile.include | 3 + drivers/include/lsm6dsl.h | 148 ++++++++++++++ drivers/lsm6dsl/Makefile | 1 + drivers/lsm6dsl/include/lsm6dsl_internal.h | 157 +++++++++++++++ drivers/lsm6dsl/include/lsm6dsl_params.h | 95 +++++++++ drivers/lsm6dsl/lsm6dsl.c | 218 +++++++++++++++++++++ drivers/lsm6dsl/lsm6dsl_saul.c | 57 ++++++ sys/auto_init/auto_init.c | 4 + sys/auto_init/saul/auto_init_lsm6dsl.c | 77 ++++++++ 10 files changed, 765 insertions(+) create mode 100644 drivers/include/lsm6dsl.h create mode 100644 drivers/lsm6dsl/Makefile create mode 100644 drivers/lsm6dsl/include/lsm6dsl_internal.h create mode 100644 drivers/lsm6dsl/include/lsm6dsl_params.h create mode 100644 drivers/lsm6dsl/lsm6dsl.c create mode 100644 drivers/lsm6dsl/lsm6dsl_saul.c create mode 100644 sys/auto_init/saul/auto_init_lsm6dsl.c diff --git a/drivers/Makefile.dep b/drivers/Makefile.dep index 5c083df737..4813af1e70 100644 --- a/drivers/Makefile.dep +++ b/drivers/Makefile.dep @@ -227,3 +227,8 @@ ifneq (,$(filter mtd_spi_nor,$(USEMODULE))) USEMODULE += mtd FEATURES_REQUIRED += periph_spi endif + +ifneq (,$(filter lsm6dsl,$(USEMODULE))) + FEATURES_REQUIRED += periph_i2c + USEMODULE += xtimer +endif diff --git a/drivers/Makefile.include b/drivers/Makefile.include index cc2c4d4a1e..5ad17a4c32 100644 --- a/drivers/Makefile.include +++ b/drivers/Makefile.include @@ -118,3 +118,6 @@ endif ifneq (,$(filter dynamixel,$(USEMODULE))) USEMODULE_INCLUDES += $(RIOTBASE)/drivers/dynamixel/include endif +ifneq (,$(filter lsm6dsl,$(USEMODULE))) + USEMODULE_INCLUDES += $(RIOTBASE)/drivers/lsm6dsl/include +endif diff --git a/drivers/include/lsm6dsl.h b/drivers/include/lsm6dsl.h new file mode 100644 index 0000000000..0752660099 --- /dev/null +++ b/drivers/include/lsm6dsl.h @@ -0,0 +1,148 @@ +/* + * Copyright (C) 2017 OTA keys S.A. + * + * 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. + * + */ + +/** + * @defgroup drivers_lsm6dsl LSM36DSL 3D accelerometer/gyroscope + * @ingroup drivers_sensors + * @brief Device driver for the LSM36DSL 3D accelerometer/gyroscope + * + * @{ + * @file + * @brief Device driver interface for the LSM36DSL 3D accelerometer/gyroscope. + * + * @author Vincent Dupont + */ + +#ifndef LSM6DSL_H +#define LSM6DSL_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "periph/i2c.h" + +/** Data rate */ +enum { + LSM6DSL_DATA_RATE_POWER_DOWN = 0x0, + LSM6DSL_DATA_RATE_1_6HZ = 0xB, + LSM6DSL_DATA_RATE_12_5HZ = 0x1, + LSM6DSL_DATA_RATE_26HZ = 0x2, + LSM6DSL_DATA_RATE_52HZ = 0x3, + LSM6DSL_DATA_RATE_104HZ = 0x4, + LSM6DSL_DATA_RATE_208HZ = 0x5, + LSM6DSL_DATA_RATE_416HZ = 0x6, + LSM6DSL_DATA_RATE_833HZ = 0x7, + LSM6DSL_DATA_RATE_1_66KHZ = 0x8, + LSM6DSL_DATA_RATE_3_33KHZ = 0x9, + LSM6DSL_DATA_RATE_6_66KHZ = 0xa, +}; + +/** Decimation */ +enum { + LSM6DSL_DECIMATION_NOT_IN_FIFO = 0, + LSM6DSL_DECIMATION_NO = 1, + LSM6DSL_DECIMATION_2 = 2, + LSM6DSL_DECIMATION_3 = 3, + LSM6DSL_DECIMATION_4 = 4, + LSM6DSL_DECIMATION_8 = 5, + LSM6DSL_DECIMATION_16 = 6, + LSM6DSL_DECIMATION_32 = 7, +}; + +/** Accelerometer full scale */ +enum { + LSM6DSL_ACC_FS_2G = 0, + LSM6DSL_ACC_FS_4G = 2, + LSM6DSL_ACC_FS_8G = 3, + LSM6DSL_ACC_FS_16G = 1, +}; + +/** Gyroscope full scale */ +enum { + LSM6DSL_GYRO_FS_245DPS = 0, + LSM6DSL_GYRO_FS_500DPS = 1, + LSM6DSL_GYRO_FS_1000DPS = 2, + LSM6DSL_GYRO_FS_2000DPS = 3, +}; + +/** LSM6DSL driver parameters */ +typedef struct { + i2c_t i2c; /**< i2c bus */ + uint8_t addr; /**< i2c address */ + uint8_t acc_odr; /**< accelerometer output data rate */ + uint8_t gyro_odr; /**< gyroscope output data rate */ + uint8_t acc_fs; /**< accelerometer full scale */ + uint8_t gyro_fs; /**< gyroscope full scale */ + uint8_t acc_decimation; /**< accelerometer decimation */ + uint8_t gyro_decimation; /**< gyroscope decimation */ +} lsm6dsl_params_t; + +/** LSM6DSL device descriptor */ +typedef struct { + lsm6dsl_params_t params; /**< driver parameters */ +} lsm6dsl_t; + +/** 3D output data */ +typedef struct { + int16_t x; /**< X axis */ + int16_t y; /**< Y axis */ + int16_t z; /**< Z axis */ +} lsm6dsl_3d_data_t; + +/** + * @brief Initialize a LSM6DSL device + * + * @param[in] dev device to initialize + * @param[in] params driver parameters + * + * @return 0 on success + * @return < 0 on error + */ +int lsm6dsl_init(lsm6dsl_t *dev, const lsm6dsl_params_t *params); + +/** + * @brief Read accelerometer data + * + * @param[in] dev device to read + * @param[out] data accelerometer values + * + * @return 0 on success + * @return < 0 on error + */ +int lsm6dsl_read_acc(lsm6dsl_t *dev, lsm6dsl_3d_data_t *data); + +/** + * @brief Read gyroscope data + * + * @param[in] dev device to read + * @param[out] data gyroscope values + * + * @return 0 on success + * @return < 0 on error + */ +int lsm6dsl_read_gyro(lsm6dsl_t *dev, lsm6dsl_3d_data_t *data); + +/** + * @brief Read temperature data + * + * @param[in] dev device to read + * @param[out] data temperature value + * + * @return 0 on success + * @return < 0 on error + */ +int lsm6dsl_read_temp(lsm6dsl_t *dev, int16_t *data); + +#ifdef __cplusplus +} +#endif + +#endif /* LSM6DSL_H */ +/** @} */ diff --git a/drivers/lsm6dsl/Makefile b/drivers/lsm6dsl/Makefile new file mode 100644 index 0000000000..48422e909a --- /dev/null +++ b/drivers/lsm6dsl/Makefile @@ -0,0 +1 @@ +include $(RIOTBASE)/Makefile.base diff --git a/drivers/lsm6dsl/include/lsm6dsl_internal.h b/drivers/lsm6dsl/include/lsm6dsl_internal.h new file mode 100644 index 0000000000..6fa9655fd4 --- /dev/null +++ b/drivers/lsm6dsl/include/lsm6dsl_internal.h @@ -0,0 +1,157 @@ +/* + * Copyright (C) 2017 OTA keys S.A. + * + * 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. + * + */ + +/** + * @addtogroup drivers_lsm6dsl + * + * @{ + * @file + * @brief Internal configuration for LSM36DSL devices + * + * @author Vincent Dupont + */ + +#ifndef LSM6DSL_INTERNAL_H +#define LSM6DSL_INTERNAL_H + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @name LSM6DSL registers + * @{ + */ +#define LSM6DSL_REG_FUNC_CFG_ACCESS (0x01) +#define LSM6DSL_REG_SENSOR_SYNC_TIME_FRAME (0x04) +#define LSM6DSL_REG_SENSOR_SYC_RES_RATIO (0x05) +#define LSM6DSL_REG_FIFO_CTRL1 (0x06) +#define LSM6DSL_REG_FIFO_CTRL2 (0x07) +#define LSM6DSL_REG_FIFO_CTRL3 (0x08) +#define LSM6DSL_REG_FIFO_CTRL4 (0x09) +#define LSM6DSL_REG_FIFO_CTRL5 (0x0A) +#define LSM6DSL_REG_DRDY_PULSE_CFG_G (0x0B) +#define LSM6DSL_REG_INT1_CTRL (0x0D) +#define LSM6DSL_REG_INT2_CTRL (0x0E) +#define LSM6DSL_REG_WHO_AM_I (0x0F) +#define LSM6DSL_REG_CTRL1_XL (0x10) +#define LSM6DSL_REG_CTRL2_G (0x11) +#define LSM6DSL_REG_CTRL3_C (0x12) +#define LSM6DSL_REG_CTRL4_C (0x13) +#define LSM6DSL_REG_CTRL5_C (0x14) +#define LSM6DSL_REG_CTRL6_C (0x15) +#define LSM6DSL_REG_CTRL7_G (0x16) +#define LSM6DSL_REG_CTRL8_XL (0x17) +#define LSM6DSL_REG_CTRL9_XL (0x18) +#define LSM6DSL_REG_CTRL10_C (0x19) +#define LSM6DSL_REG_MASTER_CONFIG (0x1A) +#define LSM6DSL_REG_WAKE_UP_SRC (0x1B) +#define LSM6DSL_REG_TAP_SRC (0x1C) +#define LSM6DSL_REG_D6D_SRC (0x1D) +#define LSM6DSL_REG_STATUS_REG (0x1E) +#define LSM6DSL_REG_OUT_TEMP_L (0x20) +#define LSM6DSL_REG_OUT_TEMP_H (0x21) +#define LSM6DSL_REG_OUTX_L_G (0x22) +#define LSM6DSL_REG_OUTX_H_G (0x23) +#define LSM6DSL_REG_OUTY_L_G (0x24) +#define LSM6DSL_REG_OUTY_H_G (0x25) +#define LSM6DSL_REG_OUTZ_L_G (0x26) +#define LSM6DSL_REG_OUTZ_H_G (0x27) +#define LSM6DSL_REG_OUTX_L_XL (0x28) +#define LSM6DSL_REG_OUTX_H_XL (0x29) +#define LSM6DSL_REG_OUTY_L_XL (0x2A) +#define LSM6DSL_REG_OUTY_H_XL (0x2B) +#define LSM6DSL_REG_OUTZ_L_XL (0x2C) +#define LSM6DSL_REG_OUTZ_H_XL (0x2D) +#define LSM6DSL_REG_SENSORHUB1_REG (0x2E) +#define LSM6DSL_REG_SENSORHUB2_REG (0x2F) +#define LSM6DSL_REG_SENSORHUB3_REG (0x30) +#define LSM6DSL_REG_SENSORHUB4_REG (0x31) +#define LSM6DSL_REG_SENSORHUB5_REG (0x32) +#define LSM6DSL_REG_SENSORHUB6_REG (0x33) +#define LSM6DSL_REG_SENSORHUB7_REG (0x34) +#define LSM6DSL_REG_SENSORHUB8_REG (0x35) +#define LSM6DSL_REG_SENSORHUB9_REG (0x36) +#define LSM6DSL_REG_SENSORHUB10_REG (0x37) +#define LSM6DSL_REG_SENSORHUB11_REG (0x38) +#define LSM6DSL_REG_SENSORHUB12_REG (0x39) +#define LSM6DSL_REG_FIFO_STATUS1 (0x3A) +#define LSM6DSL_REG_FIFO_STATUS2 (0x3B) +#define LSM6DSL_REG_FIFO_STATUS3 (0x3C) +#define LSM6DSL_REG_FIFO_STATUS4 (0x3D) +#define LSM6DSL_REG_FIFO_DATA_OUT_L (0x3E) +#define LSM6DSL_REG_FIFO_DATA_OUT_H (0x3F) +#define LSM6DSL_REG_TIMESTAMP0_REG (0x40) +#define LSM6DSL_REG_TIMESTAMP1_REG (0x41) +#define LSM6DSL_REG_TIMESTAMP2_REG (0x42) +#define LSM6DSL_REG_STEP_TIMESTAMP_L (0x49) +#define LSM6DSL_REG_STEP_TIMESTAMP_H (0x4A) +#define LSM6DSL_REG_STEP_COUNTER_L (0x4B) +#define LSM6DSL_REG_STEP_COUNTER_H (0x4C) +#define LSM6DSL_REG_SENSORHUB13_REG (0x4D) +#define LSM6DSL_REG_SENSORHUB14_REG (0x4E) +#define LSM6DSL_REG_SENSORHUB15_REG (0x4F) +#define LSM6DSL_REG_SENSORHUB16_REG (0x50) +#define LSM6DSL_REG_SENSORHUB17_REG (0x51) +#define LSM6DSL_REG_SENSORHUB18_REG (0x52) +#define LSM6DSL_REG_FUNC_SRC_1 (0x53) +#define LSM6DSL_REG_FUNC_SRC_2 (0x54) +#define LSM6DSL_REG_WRIST_TILT_IA (0x55) +#define LSM6DSL_REG_TAP_CFG (0x58) +#define LSM6DSL_REG_TAP_THS_6D (0x59) +#define LSM6DSL_REG_INT_DUR2 (0x5A) +#define LSM6DSL_REG_WAKE_UP_THS (0x5B) +#define LSM6DSL_REG_WAKE_UP_DUR (0x5C) +#define LSM6DSL_REG_FREE_FALL (0x5D) +#define LSM6DSL_REG_MD1_CFG (0x5E) +#define LSM6DSL_REG_MD2_CFG (0x5F) +#define LSM6DSL_REG_MASTER_CMD_CODE (0x60) +#define LSM6DSL_REG_SENS_SYNC_SPI_ERR_CODE (0x61) +#define LSM6DSL_REG_OUT_MAG_RAW_X_L (0x66) +#define LSM6DSL_REG_OUT_MAG_RAW_X_H (0x67) +#define LSM6DSL_REG_OUT_MAG_RAW_Y_L (0x68) +#define LSM6DSL_REG_OUT_MAG_RAW_Y_H (0x69) +#define LSM6DSL_REG_OUT_MAG_RAW_Z_L (0x6A) +#define LSM6DSL_REG_OUT_MAG_RAW_Z_H (0x6B) +#define LSM6DSL_REG_X_OFS_USR (0x73) +#define LSM6DSL_REG_Y_OFS_USR (0x74) +#define LSM6DSL_REG_Z_OFS_USR (0x75) +/** @} */ + +/** WHO_AM_I value */ +#define LSM6DSL_WHO_AM_I (0b01101010) + +/** + * @name CTRL_x registers + * @{ + */ +#define LSM6DSL_CTRL_ODR_SHIFT (4) +#define LSM6DSL_CTRL_ODR_MASK (0xF0) +#define LSM6DSL_CTRL_FS_SHIFT (2) +#define LSM6DSL_CTRL_FS_MASK (0x0C) + +#define LSM6DSL_CTRL3_C_BOOT (0x80) +/** @} */ + +/** + * @name FIFO_CTRL_x registers + * @{ + */ +#define LSM6DSL_FIFO_CTRL5_CONTINUOUS_MODE (0x6) +#define LSM6DSL_FIFO_CTRL5_FIFO_ODR_SHIFT (3) + +#define LSM6DSL_FIFO_CTRL3_GYRO_DEC_SHIFT (3) +/** @} */ + +#ifdef __cplusplus +} +#endif + +#endif /* LSM6DS_LINTERNAL_H */ +/** @} */ diff --git a/drivers/lsm6dsl/include/lsm6dsl_params.h b/drivers/lsm6dsl/include/lsm6dsl_params.h new file mode 100644 index 0000000000..71df0cbc28 --- /dev/null +++ b/drivers/lsm6dsl/include/lsm6dsl_params.h @@ -0,0 +1,95 @@ +/* + * Copyright (C) 2017 OTA keys S.A. + * + * 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. + * + */ + +/** + * @addtogroup drivers_lsm6dsl + * + * @{ + * @file + * @brief Default configuration for LSM36DSL devices + * + * @author Vincent Dupont + */ + +#ifndef LSM6DSL_PARAMS_H +#define LSM6DSL_PARAMS_H + +#include "board.h" +#include "lsm6dsl.h" +#include "saul_reg.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Set default configuration parameters + * @{ + */ +#ifndef LSM6DSL_PARAM_I2C +#define LSM6DSL_PARAM_I2C I2C_DEV(0) +#endif +#ifndef LSM6DSL_PARAM_ADDR +#define LSM6DSL_PARAM_ADDR (0x6B) /* (0x6A) */ +#endif +#ifndef LSM6DSL_PARAM_ACC_ODR +#define LSM6DSL_PARAM_ACC_ODR (LSM6DSL_DATA_RATE_52HZ) +#endif +#ifndef LSM6DSL_PARAM_GYRO_ODR +#define LSM6DSL_PARAM_GYRO_ODR (LSM6DSL_DATA_RATE_52HZ) +#endif +#ifndef LSM6DSL_PARAM_ACC_FS +#define LSM6DSL_PARAM_ACC_FS (LSM6DSL_ACC_FS_2G) +#endif +#ifndef LSM6DSL_PARAM_GYRO_FS +#define LSM6DSL_PARAM_GYRO_FS (LSM6DSL_GYRO_FS_245DPS) +#endif +#ifndef LSM6DSL_PARAM_ACC_FIFO_DEC +#define LSM6DSL_PARAM_ACC_FIFO_DEC (LSM6DSL_DECIMATION_NO) +#endif +#ifndef LSM6DSL_PARAM_GYRO_FIFO_DEC +#define LSM6DSL_PARAM_GYRO_FIFO_DEC (LSM6DSL_DECIMATION_NO) +#endif + +#define LSM6DSL_PARAMS_DEFAULT { .i2c = LSM6DSL_PARAM_I2C, \ + .addr = LSM6DSL_PARAM_ADDR, \ + .acc_odr = LSM6DSL_PARAM_ACC_ODR, \ + .gyro_odr = LSM6DSL_PARAM_GYRO_ODR, \ + .acc_fs = LSM6DSL_PARAM_ACC_FS, \ + .gyro_fs = LSM6DSL_PARAM_GYRO_FS, \ + .acc_decimation = LSM6DSL_PARAM_ACC_FIFO_DEC, \ + .gyro_decimation = LSM6DSL_PARAM_GYRO_FIFO_DEC } +/** @} */ + +/** + * @brief Allocate some memory to store the actual configuration + */ +static const lsm6dsl_params_t lsm6dsl_params[] = +{ +#ifdef LSM6DSL_PARAMS_CUSTOM + LSM6DSL_PARAMS_CUSTOM, +#else + LSM6DSL_PARAMS_DEFAULT, +#endif +}; + +/** + * @brief Additional meta information to keep in the SAUL registry + */ +static const saul_reg_info_t lsm6dsl_saul_info[] = +{ + { .name = "lsm6dsl" } +}; + +#ifdef __cplusplus +} +#endif + +#endif /* LSM6DSL_PARAMS_H */ +/** @} */ diff --git a/drivers/lsm6dsl/lsm6dsl.c b/drivers/lsm6dsl/lsm6dsl.c new file mode 100644 index 0000000000..f1f181a96c --- /dev/null +++ b/drivers/lsm6dsl/lsm6dsl.c @@ -0,0 +1,218 @@ +/* + * Copyright (C) 2017 OTA keys S.A. + * + * 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. + * + */ + +/** + * @file + * @brief Device driver implementation for the LSM36DSL 3D accelerometer/gyroscope. + * + * @author Vincent Dupont + */ + +#include "xtimer.h" + +#include "lsm6dsl.h" +#include "lsm6dsl_internal.h" + +#define ENABLE_DEBUG (1) +#include "debug.h" + +#define MAX(a, b) ((a) > (b) ? (a) : (b)) + +int lsm6dsl_init(lsm6dsl_t *dev, const lsm6dsl_params_t *params) +{ + uint8_t tmp; + int res; + + assert(dev && params); + + dev->params = *params; + + i2c_acquire(dev->params.i2c); + i2c_init_master(dev->params.i2c, I2C_SPEED_NORMAL); + + /* Reboot */ + i2c_write_reg(dev->params.i2c, dev->params.addr, + LSM6DSL_REG_CTRL3_C, LSM6DSL_CTRL3_C_BOOT); + + xtimer_usleep(5000); + + res = i2c_read_reg(dev->params.i2c, dev->params.addr, LSM6DSL_REG_WHO_AM_I, &tmp); + if ((res != 1) || (tmp != LSM6DSL_WHO_AM_I)) { + i2c_release(dev->params.i2c); + DEBUG("[!!failed!!] WHO_AM_I\n"); + return -1; + } + + /* Set acc odr / full scale */ + res = i2c_write_reg(dev->params.i2c, dev->params.addr, LSM6DSL_REG_CTRL1_XL, + ((dev->params.acc_odr << LSM6DSL_CTRL_ODR_SHIFT) | + (dev->params.acc_fs << LSM6DSL_CTRL_FS_SHIFT))); + /* Set gyro odr / full scale */ + res += i2c_write_reg(dev->params.i2c, dev->params.addr, LSM6DSL_REG_CTRL2_G, + ((dev->params.gyro_odr << LSM6DSL_CTRL_ODR_SHIFT) | + (dev->params.gyro_fs << LSM6DSL_CTRL_FS_SHIFT))); + + /* Set continuous mode */ + uint8_t fifo_odr = MAX(dev->params.acc_odr, dev->params.gyro_odr); + res += i2c_write_reg(dev->params.i2c, dev->params.addr, LSM6DSL_REG_FIFO_CTRL5, + (fifo_odr << LSM6DSL_FIFO_CTRL5_FIFO_ODR_SHIFT) | + LSM6DSL_FIFO_CTRL5_CONTINUOUS_MODE); + res += i2c_write_reg(dev->params.i2c, dev->params.addr, LSM6DSL_REG_FIFO_CTRL3, + (dev->params.gyro_decimation << LSM6DSL_FIFO_CTRL3_GYRO_DEC_SHIFT) | + dev->params.acc_decimation); + + i2c_release(dev->params.i2c); + + if (res < 4) { + DEBUG("[!!failed!!] config\n"); + return -1; + } + return 0; +} + +int lsm6dsl_read_acc(lsm6dsl_t *dev, lsm6dsl_3d_data_t *data) +{ + int res; + uint8_t tmp; + + i2c_acquire(dev->params.i2c); + i2c_read_reg(dev->params.i2c, dev->params.addr, LSM6DSL_REG_STATUS_REG, &tmp); + DEBUG("lsm6dsl status: %x\n", tmp); + + res = i2c_read_reg(dev->params.i2c, dev->params.addr, + LSM6DSL_REG_OUTX_L_XL, &tmp); + data->x = tmp; + res += i2c_read_reg(dev->params.i2c, dev->params.addr, + LSM6DSL_REG_OUTX_H_XL, &tmp); + data->x |= tmp << 8; + res += i2c_read_reg(dev->params.i2c, dev->params.addr, + LSM6DSL_REG_OUTY_L_XL, &tmp); + data->y = tmp; + res += i2c_read_reg(dev->params.i2c, dev->params.addr, + LSM6DSL_REG_OUTY_H_XL, &tmp); + data->y |= tmp << 8; + res += i2c_read_reg(dev->params.i2c, dev->params.addr, + LSM6DSL_REG_OUTZ_L_XL, &tmp); + data->z = tmp; + res += i2c_read_reg(dev->params.i2c, dev->params.addr, + LSM6DSL_REG_OUTZ_H_XL, &tmp); + data->z |= tmp << 8; + i2c_release(dev->params.i2c); + + if (res < 6) { + DEBUG("[!!failed!!]\n"); + return -1; + } + DEBUG("[done]\n"); + + float range; + switch (dev->params.acc_fs) { + case LSM6DSL_ACC_FS_2G: + range = 2000.0; + break; + case LSM6DSL_ACC_FS_4G: + range = 4000.0; + break; + case LSM6DSL_ACC_FS_8G: + range = 8000.0; + break; + case LSM6DSL_ACC_FS_16G: + range = 16000.0; + break; + default: + return -1; + } + + data->x = (data->x * range) / INT16_MAX; + data->y = (data->y * range) / INT16_MAX; + data->z = (data->z * range) / INT16_MAX; + + return 0; +} + +int lsm6dsl_read_gyro(lsm6dsl_t *dev, lsm6dsl_3d_data_t *data) +{ + int res; + uint8_t tmp; + + i2c_acquire(dev->params.i2c); + i2c_read_reg(dev->params.i2c, dev->params.addr, LSM6DSL_REG_STATUS_REG, &tmp); + DEBUG("lsm6dsl status: %x\n", tmp); + + res = i2c_read_reg(dev->params.i2c, dev->params.addr, + LSM6DSL_REG_OUTX_L_G, &tmp); + data->x = tmp; + res += i2c_read_reg(dev->params.i2c, dev->params.addr, + LSM6DSL_REG_OUTX_H_G, &tmp); + data->x |= tmp << 8; + res += i2c_read_reg(dev->params.i2c, dev->params.addr, + LSM6DSL_REG_OUTY_L_G, &tmp); + data->y = tmp; + res += i2c_read_reg(dev->params.i2c, dev->params.addr, + LSM6DSL_REG_OUTY_H_G, &tmp); + data->y |= tmp << 8; + res += i2c_read_reg(dev->params.i2c, dev->params.addr, + LSM6DSL_REG_OUTZ_L_G, &tmp); + data->z = tmp; + res += i2c_read_reg(dev->params.i2c, dev->params.addr, + LSM6DSL_REG_OUTZ_H_G, &tmp); + data->z |= tmp << 8; + i2c_release(dev->params.i2c); + + if (res < 6) { + DEBUG("[!!failed!!]\n"); + return -1; + } + DEBUG("[done]\n"); + + float range; + switch (dev->params.acc_fs) { + case LSM6DSL_GYRO_FS_245DPS: + range = 2450.0; + break; + case LSM6DSL_GYRO_FS_500DPS: + range = 5000.0; + break; + case LSM6DSL_GYRO_FS_1000DPS: + range = 10000.0; + break; + case LSM6DSL_GYRO_FS_2000DPS: + range = 20000.0; + break; + default: + return -1; + } + + data->x = (data->x * range) / INT16_MAX; + data->y = (data->y * range) / INT16_MAX; + data->z = (data->z * range) / INT16_MAX; + + return 0; +} + +int lsm6dsl_read_temp(lsm6dsl_t *dev, int16_t *data) +{ + int res; + uint8_t tmp; + + i2c_acquire(dev->params.i2c); + res = i2c_read_reg(dev->params.i2c, dev->params.addr, LSM6DSL_REG_OUT_TEMP_L, &tmp); + *data = tmp; + + res += i2c_read_reg(dev->params.i2c, dev->params.addr, LSM6DSL_REG_OUT_TEMP_H, &tmp); + *data |= tmp << 8; + + i2c_release(dev->params.i2c); + + if (res < 2) { + return -1; + } + + return 0; +} diff --git a/drivers/lsm6dsl/lsm6dsl_saul.c b/drivers/lsm6dsl/lsm6dsl_saul.c new file mode 100644 index 0000000000..0b921c8c35 --- /dev/null +++ b/drivers/lsm6dsl/lsm6dsl_saul.c @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2017 OTA keys S.A. + * + * 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. + * + */ + +/** + * @file + * @brief SAUL implementation for the LSM36DSL 3D accelerometer/gyroscope. + * + * @author Vincent Dupont + */ + +#include "lsm6dsl.h" +#include "saul.h" + +static int read_acc(void *dev, phydat_t *res) +{ + int ret = lsm6dsl_read_acc(dev, (lsm6dsl_3d_data_t *)res); + if (ret < 0) { + return -ECANCELED; + } + + res->scale = -3; + res->unit = UNIT_G; + + return 3; +} + +static int read_gyro(void *dev, phydat_t *res) +{ + int ret = lsm6dsl_read_gyro(dev, (lsm6dsl_3d_data_t *)res); + if (ret < 0) { + return -ECANCELED; + } + + res->scale = -1; + res->unit = UNIT_DPS; + + return 3; +} + +const saul_driver_t lsm6dsl_saul_acc_driver = { + .read = read_acc, + .write = saul_notsup, + .type = SAUL_SENSE_ACCEL, +}; + + +const saul_driver_t lsm6dsl_saul_gyro_driver = { + .read = read_gyro, + .write = saul_notsup, + .type = SAUL_SENSE_GYRO, +}; diff --git a/sys/auto_init/auto_init.c b/sys/auto_init/auto_init.c index be60b26303..4ff488f2b0 100644 --- a/sys/auto_init/auto_init.c +++ b/sys/auto_init/auto_init.c @@ -323,6 +323,10 @@ void auto_init(void) extern void auto_init_adxl345(void); auto_init_adxl345(); #endif +#ifdef MODULE_LSM6DSL + extern void auto_init_lsm6dsl(void); + auto_init_lsm6dsl(); +#endif #endif /* MODULE_AUTO_INIT_SAUL */ diff --git a/sys/auto_init/saul/auto_init_lsm6dsl.c b/sys/auto_init/saul/auto_init_lsm6dsl.c new file mode 100644 index 0000000000..d35beb6c35 --- /dev/null +++ b/sys/auto_init/saul/auto_init_lsm6dsl.c @@ -0,0 +1,77 @@ +/* + * Copyright (C) 2017 OTA keys S.A. + * + * 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 auto_init_saul + * @{ + * + * @file + * @brief Auto initialization of LSM6DSL accelerometer/gyroscope sensors + * + * @author Vincent Dupont