michaelh/zephyr
1
0
Fork 0
Code Releases Activity

driver/sensor: lsm6dso: Add support

Browse source 
Add support to STM LSM6DSO 6-axis IMU sensor driver.
This driver supports communication with device though both
I2C and SPI bus and both polling and drdy trigger mode.

This driver supports also the sensorhub mode with the possibility
to connect a maximum of two external devices, typically a
magnetometer and an environmental sensor, currently selected among
following devices: lis2mdl magnetometer, lps22hh or lps22hb
pressure and temperature sensors, HTS221 humidity sensor.

Signed-off-by: Armando Visconti <armando.visconti@st.com>
This commit is contained in:
Armando Visconti 2019-02-18 15:17:54 +01:00 • committed by Maureen Helm
commit 72ae24d8d5
15 changed files with 2558 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

1
drivers/sensor/CMakeLists.txt
View file

@ -36,6 +36,7 @@ add_subdirectory_ifdef(CONFIG_LPS25HB lps25hb)
add_subdirectory_ifdef(CONFIG_LSM303DLHC_MAGN lsm303dlhc_magn) add_subdirectory_ifdef(CONFIG_LSM303DLHC_MAGN lsm303dlhc_magn)
add_subdirectory_ifdef(CONFIG_LSM6DS0 lsm6ds0) add_subdirectory_ifdef(CONFIG_LSM6DS0 lsm6ds0)
add_subdirectory_ifdef(CONFIG_LSM6DSL lsm6dsl) add_subdirectory_ifdef(CONFIG_LSM6DSL lsm6dsl)
add_subdirectory_ifdef(CONFIG_LSM6DSO lsm6dso)
add_subdirectory_ifdef(CONFIG_LSM9DS0_GYRO lsm9ds0_gyro) add_subdirectory_ifdef(CONFIG_LSM9DS0_GYRO lsm9ds0_gyro)
add_subdirectory_ifdef(CONFIG_LSM9DS0_MFD lsm9ds0_mfd) add_subdirectory_ifdef(CONFIG_LSM9DS0_MFD lsm9ds0_mfd)
add_subdirectory_ifdef(CONFIG_MAX30101 max30101) add_subdirectory_ifdef(CONFIG_MAX30101 max30101)

2
drivers/sensor/Kconfig
View file

@ -97,6 +97,8 @@ source "drivers/sensor/lsm6ds0/Kconfig"
source "drivers/sensor/lsm6dsl/Kconfig" source "drivers/sensor/lsm6dsl/Kconfig"
source "drivers/sensor/lsm6dso/Kconfig"
source "drivers/sensor/lsm9ds0_gyro/Kconfig" source "drivers/sensor/lsm9ds0_gyro/Kconfig"
source "drivers/sensor/lsm9ds0_mfd/Kconfig" source "drivers/sensor/lsm9ds0_mfd/Kconfig"

13
drivers/sensor/lsm6dso/CMakeLists.txt Normal file
View file

@ -0,0 +1,13 @@
# ST Microelectronics LSM6DSO 6-axis IMU sensor driver
#
# Copyright (c) 2019 STMicroelectronics
#
# SPDX-License-Identifier: Apache-2.0
#
zephyr_library()
zephyr_library_sources_ifdef(CONFIG_LSM6DSO lsm6dso.c)
zephyr_library_sources_ifdef(CONFIG_LSM6DSO lsm6dso_i2c.c)
zephyr_library_sources_ifdef(CONFIG_LSM6DSO lsm6dso_spi.c)
zephyr_library_sources_ifdef(CONFIG_LSM6DSO_SENSORHUB lsm6dso_shub.c)
zephyr_library_sources_ifdef(CONFIG_LSM6DSO_TRIGGER lsm6dso_trigger.c)

171
drivers/sensor/lsm6dso/Kconfig Normal file
View file

@ -0,0 +1,171 @@
# ST Microelectronics LSM6DSO 6-axis IMU sensor driver
#
# Copyright (c) 2019 STMicroelectronics
#
# SPDX-License-Identifier: Apache-2.0
#
menuconfig LSM6DSO
bool "LSM6DSO I2C/SPI accelerometer and gyroscope Chip"
depends on (I2C && HAS_DTS_I2C) || (SPI && HAS_DTS_SPI)
select HAS_STMEMSC
select USE_STDC_LSM6DSO
help
Enable driver for LSM6DSO accelerometer and gyroscope
sensor.
if LSM6DSO
choice LSM6DSO_TRIGGER_MODE
prompt "Trigger mode"
help
Specify the type of triggering to be used by the driver.
config LSM6DSO_TRIGGER_NONE
bool "No trigger"
config LSM6DSO_TRIGGER_GLOBAL_THREAD
bool "Use global thread"
depends on GPIO
select LSM6DSO_TRIGGER
config LSM6DSO_TRIGGER_OWN_THREAD
bool "Use own thread"
depends on GPIO
select LSM6DSO_TRIGGER
endchoice
config LSM6DSO_TRIGGER
bool
if LSM6DSO_TRIGGER
config LSM6DSO_THREAD_PRIORITY
int "Thread priority"
depends on LSM6DSO_TRIGGER_OWN_THREAD
default 10
help
Priority of thread used by the driver to handle interrupts.
config LSM6DSO_THREAD_STACK_SIZE
int "Thread stack size"
depends on LSM6DSO_TRIGGER_OWN_THREAD
default 1024
help
Stack size of thread used by the driver to handle interrupts.
choice LSM6DSO_INT_PIN
prompt "Sensor INT pin number"
default LSM6DSO_INT_PIN_1
help
The number of LSM6DSO int pin used to generate interrupt to cpu.
Supported values are int1 or int2
config LSM6DSO_INT_PIN_1
bool "int1"
config LSM6DSO_INT_PIN_2
bool "int2"
endchoice
endif # LSM6DSO_TRIGGER
config LSM6DSO_ENABLE_TEMP
bool "Enable temperature"
help
Enable/disable temperature
config LSM6DSO_SENSORHUB
bool "Enable I2C sensorhub feature"
help
Enable/disable internal sensorhub. You can enable
a maximum of two external sensors (if more than two are enabled
the system would enumerate only the first two found)
if LSM6DSO_SENSORHUB
config LSM6DSO_EXT_LIS2MDL
bool "Enable LIS2MDL as external sensor"
default y
config LSM6DSO_EXT_LPS22HH
bool "Enable LPS22HH as external sensor"
default y
config LSM6DSO_EXT_HTS221
bool "Enable HTS221 as external sensor"
config LSM6DSO_EXT_LPS22HB
bool "Enable LPS22HB as external sensor"
endif #LSM6DSO_SENSORHUB
menu "Attributes"
config LSM6DSO_GYRO_FS
int "Gyroscope full-scale range"
default 0
help
Specify the default gyroscope full-scale range.
An X value for the config represents a range of +/- X degree per
second. Valid values are:
0: Full Scale selected at runtime
125: +/- 125dps
250: +/- 250dps
500: +/- 500dps
1000: +/- 1000dps
2000: +/- 2000dps
config LSM6DSO_GYRO_ODR
int "Gyroscope Output data rate frequency"
range 0 10
default 0
help
Specify the default accelerometer output data rate expressed in
samples per second (Hz).
0: ODR selected at runtime
1: 12.5Hz
2: 26Hz
3: 52Hz
4: 104Hz
5: 208Hz
6: 416Hz
7: 833Hz
8: 1660Hz
9: 3330Hz
10: 6660Hz
config LSM6DSO_ACCEL_FS
int "Accelerometer full-scale range"
default 0
help
Specify the default accelerometer full-scale range.
An X value for the config represents a range of +/- X G. Valid values
are:
0: Full Scale selected at runtime
2: +/- 2g
4: +/- 4g
8: +/- 8g
16: +/- 16g
config LSM6DSO_ACCEL_ODR
int "Accelerometer Output data rate frequency"
range 0 10
default 0
help
Specify the default accelerometer output data rate expressed in
samples per second (Hz).
0: ODR selected at runtime
1: 12.5Hz
2: 26Hz
3: 52Hz
4: 104Hz
5: 208Hz
6: 416Hz
7: 833Hz
8: 1660Hz
9: 3330Hz
10: 6660Hz
endmenu
endif # LSM6DSO

828
drivers/sensor/lsm6dso/lsm6dso.c Normal file
View file

@ -0,0 +1,828 @@
/* ST Microelectronics LSM6DSO 6-axis IMU sensor driver
*
* Copyright (c) 2019 STMicroelectronics
*
* SPDX-License-Identifier: Apache-2.0
*
* Datasheet:
* https://www.st.com/resource/en/datasheet/lsm6dso.pdf
*/
#include <sensor.h>
#include <kernel.h>
#include <device.h>
#include <init.h>
#include <string.h>
#include <sys/byteorder.h>
#include <sys/__assert.h>
#include <logging/log.h>
#include "lsm6dso.h"
#define LOG_LEVEL CONFIG_SENSOR_LOG_LEVEL
LOG_MODULE_REGISTER(LSM6DSO);
static const u16_t lsm6dso_odr_map[] = {0, 12, 26, 52, 104, 208, 416, 833,
1660, 3330, 6660};
#if defined(LSM6DSO_ACCEL_ODR_RUNTIME) || defined(LSM6DSO_GYRO_ODR_RUNTIME)
static int lsm6dso_freq_to_odr_val(u16_t freq)
{
size_t i;
for (i = 0; i < ARRAY_SIZE(lsm6dso_odr_map); i++) {
if (freq == lsm6dso_odr_map[i]) {
return i;
}
}
return -EINVAL;
}
#endif
static int lsm6dso_odr_to_freq_val(u16_t odr)
{
/* for valid index, return value from map */
if (odr < ARRAY_SIZE(lsm6dso_odr_map)) {
return lsm6dso_odr_map[odr];
}
/* invalid index, return last entry */
return lsm6dso_odr_map[ARRAY_SIZE(lsm6dso_odr_map) - 1];
}
#ifdef LSM6DSO_ACCEL_FS_RUNTIME
static const u16_t lsm6dso_accel_fs_map[] = {2, 16, 4, 8};
static const u16_t lsm6dso_accel_fs_sens[] = {1, 8, 2, 4};
static int lsm6dso_accel_range_to_fs_val(s32_t range)
{
size_t i;
for (i = 0; i < ARRAY_SIZE(lsm6dso_accel_fs_map); i++) {
if (range == lsm6dso_accel_fs_map[i]) {
return i;
}
}
return -EINVAL;
}
#endif
#ifdef LSM6DSO_GYRO_FS_RUNTIME
static const u16_t lsm6dso_gyro_fs_map[] = {250, 500, 1000, 2000, 125};
static const u16_t lsm6dso_gyro_fs_sens[] = {2, 4, 8, 16, 1};
static int lsm6dso_gyro_range_to_fs_val(s32_t range)
{
size_t i;
for (i = 0; i < ARRAY_SIZE(lsm6dso_gyro_fs_map); i++) {
if (range == lsm6dso_gyro_fs_map[i]) {
return i;
}
}
return -EINVAL;
}
#endif
static inline int lsm6dso_reboot(struct device *dev)
{
struct lsm6dso_data *data = dev->driver_data;
if (lsm6dso_boot_set(data->ctx, 1) < 0) {
return -EIO;
}
/* Wait sensor turn-on time as per datasheet */
k_busy_wait(35 * USEC_PER_MSEC);
return 0;
}
static int lsm6dso_accel_set_fs_raw(struct device *dev, u8_t fs)
{
struct lsm6dso_data *data = dev->driver_data;
if (lsm6dso_xl_full_scale_set(data->ctx, fs) < 0) {
return -EIO;
}
data->accel_fs = fs;
return 0;
}
static int lsm6dso_accel_set_odr_raw(struct device *dev, u8_t odr)
{
struct lsm6dso_data *data = dev->driver_data;
if (lsm6dso_xl_data_rate_set(data->ctx, odr) < 0) {
return -EIO;
}
data->accel_freq = lsm6dso_odr_to_freq_val(odr);
return 0;
}
static int lsm6dso_gyro_set_fs_raw(struct device *dev, u8_t fs)
{
struct lsm6dso_data *data = dev->driver_data;
if (lsm6dso_gy_full_scale_set(data->ctx, fs) < 0) {
return -EIO;
}
return 0;
}
static int lsm6dso_gyro_set_odr_raw(struct device *dev, u8_t odr)
{
struct lsm6dso_data *data = dev->driver_data;
if (lsm6dso_gy_data_rate_set(data->ctx, odr) < 0) {
return -EIO;
}
return 0;
}
#ifdef LSM6DSO_ACCEL_ODR_RUNTIME
static int lsm6dso_accel_odr_set(struct device *dev, u16_t freq)
{
int odr;
odr = lsm6dso_freq_to_odr_val(freq);
if (odr < 0) {
return odr;
}
if (lsm6dso_accel_set_odr_raw(dev, odr) < 0) {
LOG_DBG("failed to set accelerometer sampling rate");
return -EIO;
}
return 0;
}
#endif
#ifdef LSM6DSO_ACCEL_FS_RUNTIME
static int lsm6dso_accel_range_set(struct device *dev, s32_t range)
{
int fs;
struct lsm6dso_data *data = dev->driver_data;
fs = lsm6dso_accel_range_to_fs_val(range);
if (fs < 0) {
return fs;
}
if (lsm6dso_accel_set_fs_raw(dev, fs) < 0) {
LOG_DBG("failed to set accelerometer full-scale");
return -EIO;
}
data->acc_gain = (lsm6dso_accel_fs_sens[fs] * GAIN_UNIT_XL);
return 0;
}
#endif
static int lsm6dso_accel_config(struct device *dev, enum sensor_channel chan,
enum sensor_attribute attr,
const struct sensor_value *val)
{
switch (attr) {
#ifdef LSM6DSO_ACCEL_FS_RUNTIME
case SENSOR_ATTR_FULL_SCALE:
return lsm6dso_accel_range_set(dev, sensor_ms2_to_g(val));
#endif
#ifdef LSM6DSO_ACCEL_ODR_RUNTIME
case SENSOR_ATTR_SAMPLING_FREQUENCY:
return lsm6dso_accel_odr_set(dev, val->val1);
#endif
default:
LOG_DBG("Accel attribute not supported.");
return -ENOTSUP;
}
return 0;
}
#ifdef LSM6DSO_GYRO_ODR_RUNTIME
static int lsm6dso_gyro_odr_set(struct device *dev, u16_t freq)
{
int odr;
odr = lsm6dso_freq_to_odr_val(freq);
if (odr < 0) {
return odr;
}
if (lsm6dso_gyro_set_odr_raw(dev, odr) < 0) {
LOG_DBG("failed to set gyroscope sampling rate");
return -EIO;
}
return 0;
}
#endif
#ifdef LSM6DSO_GYRO_FS_RUNTIME
static int lsm6dso_gyro_range_set(struct device *dev, s32_t range)
{
int fs;
struct lsm6dso_data *data = dev->driver_data;
fs = lsm6dso_gyro_range_to_fs_val(range);
if (fs < 0) {
return fs;
}
if (lsm6dso_gyro_set_fs_raw(dev, fs) < 0) {
LOG_DBG("failed to set gyroscope full-scale");
return -EIO;
}
data->gyro_gain = (lsm6dso_gyro_fs_sens[fs] * GAIN_UNIT_G);
return 0;
}
#endif
static int lsm6dso_gyro_config(struct device *dev, enum sensor_channel chan,
enum sensor_attribute attr,
const struct sensor_value *val)
{
switch (attr) {
#ifdef LSM6DSO_GYRO_FS_RUNTIME
case SENSOR_ATTR_FULL_SCALE:
return lsm6dso_gyro_range_set(dev, sensor_rad_to_degrees(val));
#endif
#ifdef LSM6DSO_GYRO_ODR_RUNTIME
case SENSOR_ATTR_SAMPLING_FREQUENCY:
return lsm6dso_gyro_odr_set(dev, val->val1);
#endif
default:
LOG_DBG("Gyro attribute not supported.");
return -ENOTSUP;
}
return 0;
}
static int lsm6dso_attr_set(struct device *dev, enum sensor_channel chan,
enum sensor_attribute attr,
const struct sensor_value *val)
{
switch (chan) {
case SENSOR_CHAN_ACCEL_XYZ:
return lsm6dso_accel_config(dev, chan, attr, val);
case SENSOR_CHAN_GYRO_XYZ:
return lsm6dso_gyro_config(dev, chan, attr, val);
#if defined(CONFIG_LSM6DSO_SENSORHUB)
case SENSOR_CHAN_MAGN_XYZ:
case SENSOR_CHAN_PRESS:
case SENSOR_CHAN_HUMIDITY:
return lsm6dso_shub_config(dev, chan, attr, val);
#endif /* CONFIG_LSM6DSO_SENSORHUB */
default:
LOG_WRN("attr_set() not supported on this channel.");
return -ENOTSUP;
}
return 0;
}
static int lsm6dso_sample_fetch_accel(struct device *dev)
{
struct lsm6dso_data *data = dev->driver_data;
axis3bit16_t buf;
if (lsm6dso_acceleration_raw_get(data->ctx, buf.u8bit) < 0) {
LOG_DBG("Failed to read sample");
return -EIO;
}
data->acc[0] = sys_le16_to_cpu(buf.i16bit[0]);
data->acc[1] = sys_le16_to_cpu(buf.i16bit[1]);
data->acc[2] = sys_le16_to_cpu(buf.i16bit[2]);
return 0;
}
static int lsm6dso_sample_fetch_gyro(struct device *dev)
{
struct lsm6dso_data *data = dev->driver_data;
axis3bit16_t buf;
if (lsm6dso_angular_rate_raw_get(data->ctx, buf.u8bit) < 0) {
LOG_DBG("Failed to read sample");
return -EIO;
}
data->gyro[0] = sys_le16_to_cpu(buf.i16bit[0]);
data->gyro[1] = sys_le16_to_cpu(buf.i16bit[1]);
data->gyro[2] = sys_le16_to_cpu(buf.i16bit[2]);
return 0;
}
#if defined(CONFIG_LSM6DSO_ENABLE_TEMP)
static int lsm6dso_sample_fetch_temp(struct device *dev)
{
struct lsm6dso_data *data = dev->driver_data;
axis1bit16_t buf;
if (lsm6dso_temperature_raw_get(data->ctx, buf.u8bit) < 0) {
LOG_DBG("Failed to read sample");
return -EIO;
}
data->temp_sample = sys_le16_to_cpu(buf.i16bit);
return 0;
}
#endif
#if defined(CONFIG_LSM6DSO_SENSORHUB)
static int lsm6dso_sample_fetch_shub(struct device *dev)
{
if (lsm6dso_shub_fetch_external_devs(dev) < 0) {
LOG_DBG("failed to read ext shub devices");
return -EIO;
}
return 0;
}
#endif /* CONFIG_LSM6DSO_SENSORHUB */
static int lsm6dso_sample_fetch(struct device *dev, enum sensor_channel chan)
{
switch (chan) {
case SENSOR_CHAN_ACCEL_XYZ:
lsm6dso_sample_fetch_accel(dev);
#if defined(CONFIG_LSM6DSO_SENSORHUB)
lsm6dso_sample_fetch_shub(dev);
#endif
break;
case SENSOR_CHAN_GYRO_XYZ:
lsm6dso_sample_fetch_gyro(dev);
break;
#if defined(CONFIG_LSM6DSO_ENABLE_TEMP)
case SENSOR_CHAN_DIE_TEMP:
lsm6dso_sample_fetch_temp(dev);
break;
#endif
case SENSOR_CHAN_ALL:
lsm6dso_sample_fetch_accel(dev);
lsm6dso_sample_fetch_gyro(dev);
#if defined(CONFIG_LSM6DSO_ENABLE_TEMP)
lsm6dso_sample_fetch_temp(dev);
#endif
#if defined(CONFIG_LSM6DSO_SENSORHUB)
lsm6dso_sample_fetch_shub(dev);
#endif
break;
default:
return -ENOTSUP;
}
return 0;
}
static inline void lsm6dso_accel_convert(struct sensor_value *val, int raw_val,
u32_t sensitivity)
{
s64_t dval;
/* Sensitivity is exposed in ug/LSB */
/* Convert to m/s^2 */
dval = (s64_t)(raw_val) * sensitivity * SENSOR_G_DOUBLE;
val->val1 = (s32_t)(dval / 1000000);
val->val2 = (s32_t)(dval % 1000000);
}
static inline int lsm6dso_accel_get_channel(enum sensor_channel chan,
struct sensor_value *val,
struct lsm6dso_data *data,
u32_t sensitivity)
{
u8_t i;
switch (chan) {
case SENSOR_CHAN_ACCEL_X:
lsm6dso_accel_convert(val, data->acc[0], sensitivity);
break;
case SENSOR_CHAN_ACCEL_Y:
lsm6dso_accel_convert(val, data->acc[1], sensitivity);
break;
case SENSOR_CHAN_ACCEL_Z:
lsm6dso_accel_convert(val, data->acc[2], sensitivity);
break;
case SENSOR_CHAN_ACCEL_XYZ:
for (i = 0; i < 3; i++) {
lsm6dso_accel_convert(val++, data->acc[i], sensitivity);
}
break;
default:
return -ENOTSUP;
}
return 0;
}
static int lsm6dso_accel_channel_get(enum sensor_channel chan,
struct sensor_value *val,
struct lsm6dso_data *data)
{
return lsm6dso_accel_get_channel(chan, val, data, data->acc_gain);
}
static inline void lsm6dso_gyro_convert(struct sensor_value *val, int raw_val,
u32_t sensitivity)
{
s64_t dval;
/* Sensitivity is exposed in udps/LSB */
/* Convert to rad/s */
dval = (s64_t)(raw_val) * sensitivity * SENSOR_DEG2RAD_DOUBLE;
val->val1 = (s32_t)(dval / 1000000);
val->val2 = (s32_t)(dval % 1000000);
}
static inline int lsm6dso_gyro_get_channel(enum sensor_channel chan,
struct sensor_value *val,
struct lsm6dso_data *data,
u32_t sensitivity)
{
u8_t i;
switch (chan) {
case SENSOR_CHAN_GYRO_X:
lsm6dso_gyro_convert(val, data->gyro[0], sensitivity);
break;
case SENSOR_CHAN_GYRO_Y:
lsm6dso_gyro_convert(val, data->gyro[1], sensitivity);
break;
case SENSOR_CHAN_GYRO_Z:
lsm6dso_gyro_convert(val, data->gyro[2], sensitivity);
break;
case SENSOR_CHAN_GYRO_XYZ:
for (i = 0; i < 3; i++) {
lsm6dso_gyro_convert(val++, data->gyro[i], sensitivity);
}
break;
default:
return -ENOTSUP;
}
return 0;
}
static int lsm6dso_gyro_channel_get(enum sensor_channel chan,
struct sensor_value *val,
struct lsm6dso_data *data)
{
return lsm6dso_gyro_get_channel(chan, val, data,
LSM6DSO_DEFAULT_GYRO_SENSITIVITY);
}
#if defined(CONFIG_LSM6DSO_ENABLE_TEMP)
static void lsm6dso_gyro_channel_get_temp(struct sensor_value *val,
struct lsm6dso_data *data)
{
/* val = temp_sample / 256 + 25 */
val->val1 = data->temp_sample / 256 + 25;
val->val2 = (data->temp_sample % 256) * (1000000 / 256);
}
#endif
#if defined(CONFIG_LSM6DSO_SENSORHUB)
static inline void lsm6dso_magn_convert(struct sensor_value *val, int raw_val,
u16_t sensitivity)
{
double dval;
/* Sensitivity is exposed in mgauss/LSB */
dval = (double)(raw_val * sensitivity);
val->val1 = (s32_t)dval / 1000000;
val->val2 = (s32_t)dval % 1000000;
}
static inline int lsm6dso_magn_get_channel(enum sensor_channel chan,
struct sensor_value *val,
struct lsm6dso_data *data)
{
s16_t sample[3];
int idx;
idx = lsm6dso_shub_get_idx(SENSOR_CHAN_MAGN_XYZ);
if (idx < 0) {
LOG_DBG("external magn not supported");
return -ENOTSUP;
}
sample[0] = sys_le16_to_cpu((s16_t)(data->ext_data[idx][0] |
(data->ext_data[idx][1] << 8)));
sample[1] = sys_le16_to_cpu((s16_t)(data->ext_data[idx][2] |
(data->ext_data[idx][3] << 8)));
sample[2] = sys_le16_to_cpu((s16_t)(data->ext_data[idx][4] |
(data->ext_data[idx][5] << 8)));
switch (chan) {
case SENSOR_CHAN_MAGN_X:
lsm6dso_magn_convert(val, sample[0], data->magn_gain);
break;
case SENSOR_CHAN_MAGN_Y:
lsm6dso_magn_convert(val, sample[1], data->magn_gain);
break;
case SENSOR_CHAN_MAGN_Z:
lsm6dso_magn_convert(val, sample[2], data->magn_gain);
break;
case SENSOR_CHAN_MAGN_XYZ:
lsm6dso_magn_convert(val, sample[0], data->magn_gain);
lsm6dso_magn_convert(val + 1, sample[1], data->magn_gain);
lsm6dso_magn_convert(val + 2, sample[2], data->magn_gain);
break;
default:
return -ENOTSUP;
}
return 0;
}
static inline void lsm6dso_hum_convert(struct sensor_value *val,
struct lsm6dso_data *data)
{
float rh;
s16_t raw_val;
struct hts221_data *ht = &data->hts221;
int idx;
idx = lsm6dso_shub_get_idx(SENSOR_CHAN_HUMIDITY);
if (idx < 0) {
LOG_DBG("external press/temp not supported");
return;
}
raw_val = sys_le16_to_cpu((s16_t)(data->ext_data[idx][0] |
(data->ext_data[idx][1] << 8)));
/* find relative humidty by linear interpolation */
rh = (ht->y1 - ht->y0) * raw_val + ht->x1 * ht->y0 - ht->x0 * ht->y1;
rh /= (ht->x1 - ht->x0);
/* convert humidity to integer and fractional part */
val->val1 = rh;
val->val2 = rh * 1000000;
}
static inline void lsm6dso_press_convert(struct sensor_value *val,
struct lsm6dso_data *data)
{
s32_t raw_val;
int idx;
idx = lsm6dso_shub_get_idx(SENSOR_CHAN_PRESS);
if (idx < 0) {
LOG_DBG("external press/temp not supported");
return;
}
raw_val = sys_le32_to_cpu((s32_t)(data->ext_data[idx][0] |
(data->ext_data[idx][1] << 8) |
(data->ext_data[idx][2] << 16)));
/* Pressure sensitivity is 4096 LSB/hPa */
/* Convert raw_val to val in kPa */
val->val1 = (raw_val >> 12) / 10;
val->val2 = (raw_val >> 12) % 10 * 100000 +
(((s32_t)((raw_val) & 0x0FFF) * 100000L) >> 12);
}
static inline void lsm6dso_temp_convert(struct sensor_value *val,
struct lsm6dso_data *data)
{
s16_t raw_val;
int idx;
idx = lsm6dso_shub_get_idx(SENSOR_CHAN_PRESS);
if (idx < 0) {
LOG_DBG("external press/temp not supported");
return;
}
raw_val = sys_le16_to_cpu((s16_t)(data->ext_data[idx][3] |
(data->ext_data[idx][4] << 8)));
/* Temperature sensitivity is 100 LSB/deg C */
val->val1 = raw_val / 100;
val->val2 = (s32_t)raw_val % 100 * (10000);
}
#endif
static int lsm6dso_channel_get(struct device *dev,
enum sensor_channel chan,
struct sensor_value *val)
{
struct lsm6dso_data *data = dev->driver_data;
switch (chan) {
case SENSOR_CHAN_ACCEL_X:
case SENSOR_CHAN_ACCEL_Y:
case SENSOR_CHAN_ACCEL_Z:
case SENSOR_CHAN_ACCEL_XYZ:
lsm6dso_accel_channel_get(chan, val, data);
break;
case SENSOR_CHAN_GYRO_X:
case SENSOR_CHAN_GYRO_Y:
case SENSOR_CHAN_GYRO_Z:
case SENSOR_CHAN_GYRO_XYZ:
lsm6dso_gyro_channel_get(chan, val, data);
break;
#if defined(CONFIG_LSM6DSO_ENABLE_TEMP)
case SENSOR_CHAN_DIE_TEMP:
lsm6dso_gyro_channel_get_temp(val, data);
break;
#endif
#if defined(CONFIG_LSM6DSO_SENSORHUB)
case SENSOR_CHAN_MAGN_X:
case SENSOR_CHAN_MAGN_Y:
case SENSOR_CHAN_MAGN_Z:
case SENSOR_CHAN_MAGN_XYZ:
lsm6dso_magn_get_channel(chan, val, data);
break;
case SENSOR_CHAN_HUMIDITY:
lsm6dso_hum_convert(val, data);
break;
case SENSOR_CHAN_PRESS:
lsm6dso_press_convert(val, data);
break;
case SENSOR_CHAN_AMBIENT_TEMP:
lsm6dso_temp_convert(val, data);
break;
#endif
default:
return -ENOTSUP;
}
return 0;
}
static const struct sensor_driver_api lsm6dso_api_funcs = {
.attr_set = lsm6dso_attr_set,
#if CONFIG_LSM6DSO_TRIGGER
.trigger_set = lsm6dso_trigger_set,
#endif
.sample_fetch = lsm6dso_sample_fetch,
.channel_get = lsm6dso_channel_get,
};
static int lsm6dso_init_chip(struct device *dev)
{
struct lsm6dso_data *lsm6dso = dev->driver_data;
u8_t chip_id;
if (lsm6dso_device_id_get(lsm6dso->ctx, &chip_id) < 0) {
LOG_DBG("Failed reading chip id");
return -EIO;
}
LOG_INF("chip id 0x%x", chip_id);
if (chip_id != LSM6DSO_ID) {
LOG_DBG("Invalid chip id 0x%x", chip_id);
return -EIO;
}
/* reset device */
if (lsm6dso_reset_set(lsm6dso->ctx, 1) < 0) {
return -EIO;
}
k_busy_wait(100);
if (lsm6dso_accel_set_fs_raw(dev,
LSM6DSO_DEFAULT_ACCEL_FULLSCALE) < 0) {
LOG_DBG("failed to set accelerometer full-scale");
return -EIO;
}
lsm6dso->acc_gain = LSM6DSO_DEFAULT_ACCEL_SENSITIVITY;
lsm6dso->accel_freq = lsm6dso_odr_to_freq_val(CONFIG_LSM6DSO_ACCEL_ODR);
if (lsm6dso_accel_set_odr_raw(dev, CONFIG_LSM6DSO_ACCEL_ODR) < 0) {
LOG_DBG("failed to set accelerometer sampling rate");
return -EIO;
}
if (lsm6dso_gyro_set_fs_raw(dev, LSM6DSO_DEFAULT_GYRO_FULLSCALE) < 0) {
LOG_DBG("failed to set gyroscope full-scale");
return -EIO;
}
lsm6dso->gyro_gain = LSM6DSO_DEFAULT_GYRO_SENSITIVITY;
lsm6dso->gyro_freq = lsm6dso_odr_to_freq_val(CONFIG_LSM6DSO_GYRO_ODR);
if (lsm6dso_gyro_set_odr_raw(dev, CONFIG_LSM6DSO_GYRO_ODR) < 0) {
LOG_DBG("failed to set gyroscope sampling rate");
return -EIO;
}
/* Set FIFO bypass mode */
if (lsm6dso_fifo_mode_set(lsm6dso->ctx, LSM6DSO_BYPASS_MODE) < 0) {
LOG_DBG("failed to set FIFO mode");
return -EIO;
}
if (lsm6dso_block_data_update_set(lsm6dso->ctx, 1) < 0) {
LOG_DBG("failed to set BDU mode");
return -EIO;
}
return 0;
}
static struct lsm6dso_data lsm6dso_data;
static const struct lsm6dso_config lsm6dso_config = {
.bus_name = DT_INST_0_ST_LSM6DSO_BUS_NAME,
#if defined(DT_ST_LSM6DSO_BUS_SPI)
.bus_init = lsm6dso_spi_init,
.spi_conf.frequency = DT_INST_0_ST_LSM6DSO_SPI_MAX_FREQUENCY,
.spi_conf.operation = (SPI_OP_MODE_MASTER | SPI_MODE_CPOL |
SPI_MODE_CPHA | SPI_WORD_SET(8) |
SPI_LINES_SINGLE),
.spi_conf.slave = DT_INST_0_ST_LSM6DSO_BASE_ADDRESS,
#if defined(DT_INST_0_ST_LSM6DSO_CS_GPIO_CONTROLLER)
.gpio_cs_port = DT_INST_0_ST_LSM6DSO_CS_GPIO_CONTROLLER,
.cs_gpio = DT_INST_0_ST_LSM6DSO_CS_GPIO_PIN,
.spi_conf.cs = &lsm6dso_data.cs_ctrl,
#else
.spi_conf.cs = NULL,
#endif
#elif defined(DT_ST_LSM6DSO_BUS_I2C)
.bus_init = lsm6dso_i2c_init,
.i2c_slv_addr = DT_INST_0_ST_LSM6DSO_BASE_ADDRESS,
#else
#error "BUS MACRO NOT DEFINED IN DTS"
#endif
#ifdef CONFIG_LSM6DSO_TRIGGER
.int_gpio_port = DT_INST_0_ST_LSM6DSO_IRQ_GPIOS_CONTROLLER,
.int_gpio_pin = DT_INST_0_ST_LSM6DSO_IRQ_GPIOS_PIN,
#if defined(CONFIG_LSM6DSO_INT_PIN_1)
.int_pin = 1,
#elif defined(CONFIG_LSM6DSO_INT_PIN_2)
.int_pin = 2,
#endif /* CONFIG_LSM6DSO_INT_PIN */
#endif /* CONFIG_LSM6DSO_TRIGGER */
};
static int lsm6dso_init(struct device *dev)
{
const struct lsm6dso_config * const config = dev->config->config_info;
struct lsm6dso_data *data = dev->driver_data;
data->bus = device_get_binding(config->bus_name);
if (!data->bus) {
LOG_DBG("master not found: %s",
config->bus_name);
return -EINVAL;
}
config->bus_init(dev);
#ifdef CONFIG_LSM6DSO_TRIGGER
if (lsm6dso_init_interrupt(dev) < 0) {
LOG_ERR("Failed to initialize interrupt.");
return -EIO;
}
#endif
if (lsm6dso_init_chip(dev) < 0) {
LOG_DBG("failed to initialize chip");
return -EIO;
}
#ifdef CONFIG_LSM6DSO_SENSORHUB
if (lsm6dso_shub_init(dev) < 0) {
LOG_DBG("failed to initialize external chip");
return -EIO;
}
#endif
return 0;
}
static struct lsm6dso_data lsm6dso_data;
DEVICE_AND_API_INIT(lsm6dso, DT_INST_0_ST_LSM6DSO_LABEL, lsm6dso_init,
&lsm6dso_data, &lsm6dso_config, POST_KERNEL,
CONFIG_SENSOR_INIT_PRIORITY, &lsm6dso_api_funcs);

203
drivers/sensor/lsm6dso/lsm6dso.h Normal file
View file

@ -0,0 +1,203 @@
/* ST Microelectronics LSM6DSO 6-axis IMU sensor driver
*
* Copyright (c) 2019 STMicroelectronics
*
* SPDX-License-Identifier: Apache-2.0
*
* Datasheet:
* https://www.st.com/resource/en/datasheet/lsm6dso.pdf
*/
#ifndef ZEPHYR_DRIVERS_SENSOR_LSM6DSO_LSM6DSO_H_
#define ZEPHYR_DRIVERS_SENSOR_LSM6DSO_LSM6DSO_H_
#include <sensor.h>
#include <zephyr/types.h>
#include <gpio.h>
#include <drivers/spi.h>
#include <sys/util.h>
#include "lsm6dso_reg.h"
#define LSM6DSO_EN_BIT 0x01
#define LSM6DSO_DIS_BIT 0x00
/* Accel sensor sensitivity grain is 61 ug/LSB */
#define GAIN_UNIT_XL (61LL)
/* Gyro sensor sensitivity grain is 4.375 udps/LSB */
#define GAIN_UNIT_G (4375LL)
#define SENSOR_PI_DOUBLE (SENSOR_PI / 1000000.0)
#define SENSOR_DEG2RAD_DOUBLE (SENSOR_PI_DOUBLE / 180)
#define SENSOR_G_DOUBLE (SENSOR_G / 1000000.0)
#if CONFIG_LSM6DSO_ACCEL_FS == 0
#define LSM6DSO_ACCEL_FS_RUNTIME 1
#define LSM6DSO_DEFAULT_ACCEL_FULLSCALE 0
#define LSM6DSO_DEFAULT_ACCEL_SENSITIVITY GAIN_UNIT_XL
#elif CONFIG_LSM6DSO_ACCEL_FS == 2
#define LSM6DSO_DEFAULT_ACCEL_FULLSCALE 0
#define LSM6DSO_DEFAULT_ACCEL_SENSITIVITY GAIN_UNIT_XL
#elif CONFIG_LSM6DSO_ACCEL_FS == 4
#define LSM6DSO_DEFAULT_ACCEL_FULLSCALE 2
#define LSM6DSO_DEFAULT_ACCEL_SENSITIVITY (2.0 * GAIN_UNIT_XL)
#elif CONFIG_LSM6DSO_ACCEL_FS == 8
#define LSM6DSO_DEFAULT_ACCEL_FULLSCALE 3
#define LSM6DSO_DEFAULT_ACCEL_SENSITIVITY (4.0 * GAIN_UNIT_XL)
#elif CONFIG_LSM6DSO_ACCEL_FS == 16
#define LSM6DSO_DEFAULT_ACCEL_FULLSCALE 1
#define LSM6DSO_DEFAULT_ACCEL_SENSITIVITY (8.0 * GAIN_UNIT_XL)
#endif
#if (CONFIG_LSM6DSO_ACCEL_ODR == 0)
#define LSM6DSO_ACCEL_ODR_RUNTIME 1
#endif
#define GYRO_FULLSCALE_125 4
#if CONFIG_LSM6DSO_GYRO_FS == 0
#define LSM6DSO_GYRO_FS_RUNTIME 1
#define LSM6DSO_DEFAULT_GYRO_FULLSCALE 4
#define LSM6DSO_DEFAULT_GYRO_SENSITIVITY GAIN_UNIT_G
#elif CONFIG_LSM6DSO_GYRO_FS == 125
#define LSM6DSO_DEFAULT_GYRO_FULLSCALE 4
#define LSM6DSO_DEFAULT_GYRO_SENSITIVITY GAIN_UNIT_G
#elif CONFIG_LSM6DSO_GYRO_FS == 250
#define LSM6DSO_DEFAULT_GYRO_FULLSCALE 0
#define LSM6DSO_DEFAULT_GYRO_SENSITIVITY (2.0 * GAIN_UNIT_G)
#elif CONFIG_LSM6DSO_GYRO_FS == 500
#define LSM6DSO_DEFAULT_GYRO_FULLSCALE 1
#define LSM6DSO_DEFAULT_GYRO_SENSITIVITY (4.0 * GAIN_UNIT_G)
#elif CONFIG_LSM6DSO_GYRO_FS == 1000
#define LSM6DSO_DEFAULT_GYRO_FULLSCALE 2
#define LSM6DSO_DEFAULT_GYRO_SENSITIVITY (8.0 * GAIN_UNIT_G)
#elif CONFIG_LSM6DSO_GYRO_FS == 2000
#define LSM6DSO_DEFAULT_GYRO_FULLSCALE 3
#define LSM6DSO_DEFAULT_GYRO_SENSITIVITY (16.0 * GAIN_UNIT_G)
#endif
#if (CONFIG_LSM6DSO_GYRO_ODR == 0)
#define LSM6DSO_GYRO_ODR_RUNTIME 1
#endif
struct lsm6dso_config {
char *bus_name;
int (*bus_init)(struct device *dev);
#ifdef CONFIG_LSM6DSO_TRIGGER
const char *int_gpio_port;
u8_t int_gpio_pin;
u8_t int_pin;
#endif /* CONFIG_LSM6DSO_TRIGGER */
#ifdef DT_ST_LSM6DSO_BUS_I2C
u16_t i2c_slv_addr;
#elif DT_ST_LSM6DSO_BUS_SPI
struct spi_config spi_conf;
#if defined(DT_INST_0_ST_LSM6DSO_CS_GPIO_CONTROLLER)
const char *gpio_cs_port;
u8_t cs_gpio;
#endif /* DT_INST_0_ST_LSM6DSO_CS_GPIO_CONTROLLER */
#endif /* DT_ST_LSM6DSO_BUS_I2C */
};
union samples {
u8_t raw[6];
struct {
s16_t axis[3];
};
} __aligned(2);
/* sensor data forward declaration (member definition is below) */
struct lsm6dso_data;
struct lsm6dso_tf {
int (*read_data)(struct lsm6dso_data *data, u8_t reg_addr,
u8_t *value, u8_t len);
int (*write_data)(struct lsm6dso_data *data, u8_t reg_addr,
u8_t *value, u8_t len);
int (*read_reg)(struct lsm6dso_data *data, u8_t reg_addr,
u8_t *value);
int (*write_reg)(struct lsm6dso_data *data, u8_t reg_addr,
u8_t value);
int (*update_reg)(struct lsm6dso_data *data, u8_t reg_addr,
u8_t mask, u8_t value);
};
#define LSM6DSO_SHUB_MAX_NUM_SLVS 2
struct lsm6dso_data {
struct device *bus;
s16_t acc[3];
u32_t acc_gain;
s16_t gyro[3];
u32_t gyro_gain;
#if defined(CONFIG_LSM6DSO_ENABLE_TEMP)
int temp_sample;
#endif
#if defined(CONFIG_LSM6DSO_SENSORHUB)
u8_t ext_data[2][6];
u16_t magn_gain;
struct hts221_data {
s16_t x0;
s16_t x1;
s16_t y0;
s16_t y1;
} hts221;
#endif /* CONFIG_LSM6DSO_SENSORHUB */
lsm6dso_ctx_t *ctx;
#ifdef DT_ST_LSM6DSO_BUS_I2C
lsm6dso_ctx_t ctx_i2c;
#elif DT_ST_LSM6DSO_BUS_SPI
lsm6dso_ctx_t ctx_spi;
#endif
u16_t accel_freq;
u8_t accel_fs;
u16_t gyro_freq;
u8_t gyro_fs;
#ifdef CONFIG_LSM6DSO_TRIGGER
struct device *gpio;
struct gpio_callback gpio_cb;
sensor_trigger_handler_t handler_drdy_acc;
sensor_trigger_handler_t handler_drdy_gyr;
sensor_trigger_handler_t handler_drdy_temp;
#if defined(CONFIG_LSM6DSO_TRIGGER_OWN_THREAD)
K_THREAD_STACK_MEMBER(thread_stack, CONFIG_LSM6DSO_THREAD_STACK_SIZE);
struct k_thread thread;
struct k_sem gpio_sem;
#elif defined(CONFIG_LSM6DSO_TRIGGER_GLOBAL_THREAD)
struct k_work work;
struct device *dev;
#endif
#endif /* CONFIG_LSM6DSO_TRIGGER */
#if defined(DT_INST_0_ST_LSM6DSO_CS_GPIO_CONTROLLER)
struct spi_cs_control cs_ctrl;
#endif
};
int lsm6dso_spi_init(struct device *dev);
int lsm6dso_i2c_init(struct device *dev);
#if defined(CONFIG_LSM6DSO_SENSORHUB)
int lsm6dso_shub_init(struct device *dev);
int lsm6dso_shub_fetch_external_devs(struct device *dev);
int lsm6dso_shub_get_idx(enum sensor_channel type);
int lsm6dso_shub_config(struct device *dev, enum sensor_channel chan,
enum sensor_attribute attr,
const struct sensor_value *val);
#endif /* CONFIG_LSM6DSO_SENSORHUB */
#ifdef CONFIG_LSM6DSO_TRIGGER
int lsm6dso_trigger_set(struct device *dev,
const struct sensor_trigger *trig,
sensor_trigger_handler_t handler);
int lsm6dso_init_interrupt(struct device *dev);
#endif
#endif /* ZEPHYR_DRIVERS_SENSOR_LSM6DSO_LSM6DSO_H_ */

54
drivers/sensor/lsm6dso/lsm6dso_i2c.c Normal file
View file

@ -0,0 +1,54 @@
/* ST Microelectronics LSM6DSO 6-axis IMU sensor driver
*
* Copyright (c) 2019 STMicroelectronics
*
* SPDX-License-Identifier: Apache-2.0
*
* Datasheet:
* https://www.st.com/resource/en/datasheet/lsm6dso.pdf
*/
#include <string.h>
#include <drivers/i2c.h>
#include <logging/log.h>
#include "lsm6dso.h"
#ifdef DT_ST_LSM6DSO_BUS_I2C
#define LOG_LEVEL CONFIG_SENSOR_LOG_LEVEL
LOG_MODULE_DECLARE(LSM6DSO);
static int lsm6dso_i2c_read(struct device *dev, u8_t reg_addr,
u8_t *value, u8_t len)
{
struct lsm6dso_data *data = dev->driver_data;
const struct lsm6dso_config *cfg = dev->config->config_info;
return i2c_burst_read(data->bus, cfg->i2c_slv_addr,
reg_addr, value, len);
}
static int lsm6dso_i2c_write(struct device *dev, u8_t reg_addr,
u8_t *value, u8_t len)
{
struct lsm6dso_data *data = dev->driver_data;
const struct lsm6dso_config *cfg = dev->config->config_info;
return i2c_burst_write(data->bus, cfg->i2c_slv_addr,
reg_addr, value, len);
}
int lsm6dso_i2c_init(struct device *dev)
{
struct lsm6dso_data *data = dev->driver_data;
data->ctx_i2c.read_reg = (lsm6dso_read_ptr) lsm6dso_i2c_read,
data->ctx_i2c.write_reg = (lsm6dso_write_ptr) lsm6dso_i2c_write,
data->ctx = &data->ctx_i2c;
data->ctx->handle = dev;
return 0;
}
#endif /* DT_ST_LSM6DSO_BUS_I2C */

789
drivers/sensor/lsm6dso/lsm6dso_shub.c Normal file
View file

@ -0,0 +1,789 @@
/* ST Microelectronics LSM6DSO 6-axis IMU sensor driver
*
* Copyright (c) 2019 STMicroelectronics
*
* SPDX-License-Identifier: Apache-2.0
*
* Datasheet:
* https://www.st.com/resource/en/datasheet/lsm6dso.pdf
*/
#include <device.h>
#include <drivers/i2c.h>
#include <sys/byteorder.h>
#include <sys/__assert.h>
#include <sys/util.h>
#include <kernel.h>
#include <sensor.h>
#include <logging/log.h>
#include "lsm6dso.h"
#define LOG_LEVEL CONFIG_SENSOR_LOG_LEVEL
LOG_MODULE_DECLARE(LSM6DSO);
#define LSM6DSO_SHUB_DATA_OUT 0x02
#define LSM6DSO_SHUB_SLV0_ADDR 0x15
#define LSM6DSO_SHUB_SLV0_SUBADDR 0x16
#define LSM6DSO_SHUB_SLV0_CONFIG 0x17
#define LSM6DSO_SHUB_SLV1_ADDR 0x18
#define LSM6DSO_SHUB_SLV1_SUBADDR 0x19
#define LSM6DSO_SHUB_SLV1_CONFIG 0x1A
#define LSM6DSO_SHUB_SLV2_ADDR 0x1B
#define LSM6DSO_SHUB_SLV2_SUBADDR 0x1C
#define LSM6DSO_SHUB_SLV2_CONFIG 0x1D
#define LSM6DSO_SHUB_SLV3_ADDR 0x1E
#define LSM6DSO_SHUB_SLV3_SUBADDR 0x1F
#define LSM6DSO_SHUB_SLV3_CONFIG 0x20
#define LSM6DSO_SHUB_SLV0_DATAWRITE 0x21
#define LSM6DSO_SHUB_STATUS_MASTER 0x22
#define LSM6DSO_SHUB_STATUS_SLV0_NACK BIT(3)
#define LSM6DSO_SHUB_STATUS_ENDOP BIT(0)
#define LSM6DSO_SHUB_SLVX_WRITE 0x0
#define LSM6DSO_SHUB_SLVX_READ 0x1
static u8_t num_ext_dev;
static u8_t shub_ext[LSM6DSO_SHUB_MAX_NUM_SLVS];
static int lsm6dso_shub_write_slave_reg(struct lsm6dso_data *data,
u8_t slv_addr, u8_t slv_reg,
u8_t *value, u16_t len);
static int lsm6dso_shub_read_slave_reg(struct lsm6dso_data *data,
u8_t slv_addr, u8_t slv_reg,
u8_t *value, u16_t len);
static void lsm6dso_shub_enable(struct lsm6dso_data *data, u8_t enable);
/*
* LIS2MDL magn device specific part
*/
#ifdef CONFIG_LSM6DSO_EXT_LIS2MDL
#define LIS2MDL_CFG_REG_A 0x60
#define LIS2MDL_CFG_REG_B 0x61
#define LIS2MDL_CFG_REG_C 0x62
#define LIS2MDL_STATUS_REG 0x67
#define LIS2MDL_SW_RESET 0x20
#define LIS2MDL_ODR_10HZ 0x00
#define LIS2MDL_ODR_100HZ 0x0C
#define LIS2MDL_OFF_CANC 0x02
#define LIS2MDL_SENSITIVITY 1500
static int lsm6dso_lis2mdl_init(struct lsm6dso_data *data, u8_t i2c_addr)
{
u8_t mag_cfg[2];
data->magn_gain = LIS2MDL_SENSITIVITY;
/* sw reset device */
mag_cfg[0] = LIS2MDL_SW_RESET;
lsm6dso_shub_write_slave_reg(data, i2c_addr,
LIS2MDL_CFG_REG_A, mag_cfg, 1);
k_sleep(10); /* turn-on time in ms */
/* configure mag */
mag_cfg[0] = LIS2MDL_ODR_10HZ;
mag_cfg[1] = LIS2MDL_OFF_CANC;
lsm6dso_shub_write_slave_reg(data, i2c_addr,
LIS2MDL_CFG_REG_A, mag_cfg, 2);
return 0;
}
static const u16_t lis2mdl_map[] = {10, 20, 50, 100};
static int lsm6dso_lis2mdl_odr_set(struct lsm6dso_data *data,
u8_t i2c_addr, u16_t freq)
{
u8_t odr, cfg;
for (odr = 0; odr < ARRAY_SIZE(lis2mdl_map); odr++) {
if (freq == lis2mdl_map[odr]) {
break;
}
}
if (odr == ARRAY_SIZE(lis2mdl_map)) {
LOG_DBG("shub: LIS2MDL freq val %d not supported.", freq);
return -ENOTSUP;
}
cfg = (odr << 2);
lsm6dso_shub_write_slave_reg(data, i2c_addr,
LIS2MDL_CFG_REG_A, &cfg, 1);
lsm6dso_shub_enable(data, 1);
return 0;
}
static int lsm6dso_lis2mdl_conf(struct lsm6dso_data *data, u8_t i2c_addr,
enum sensor_channel chan,
enum sensor_attribute attr,
const struct sensor_value *val)
{
switch (attr) {
case SENSOR_ATTR_SAMPLING_FREQUENCY:
return lsm6dso_lis2mdl_odr_set(data, i2c_addr, val->val1);
default:
LOG_DBG("shub: LIS2MDL attribute not supported.");
return -ENOTSUP;
}
return 0;
}
#endif /* CONFIG_LSM6DSO_EXT_LIS2MDL */
/*
* HTS221 humidity device specific part
*/
#ifdef CONFIG_LSM6DSO_EXT_HTS221
#define HTS221_AUTOINCREMENT BIT(7)
#define HTS221_REG_CTRL1 0x20
#define HTS221_ODR_1HZ 0x01
#define HTS221_BDU 0x04
#define HTS221_PD 0x80
#define HTS221_REG_CONV_START 0x30
static int lsmdso_hts221_read_conv_data(struct lsm6dso_data *data,
u8_t i2c_addr)
{
u8_t buf[16], i;
struct hts221_data *ht = &data->hts221;
for (i = 0; i < sizeof(buf); i += 7) {
unsigned char len = MIN(7, sizeof(buf) - i);
if (lsm6dso_shub_read_slave_reg(data, i2c_addr,
(HTS221_REG_CONV_START + i) |
HTS221_AUTOINCREMENT,
&buf[i], len) < 0) {
LOG_DBG("shub: failed to read hts221 conv data");
return -EIO;
}
}
ht->y0 = buf[0] / 2;
ht->y1 = buf[1] / 2;
ht->x0 = sys_le16_to_cpu(buf[6] | (buf[7] << 8));
ht->x1 = sys_le16_to_cpu(buf[10] | (buf[11] << 8));
return 0;
}
static int lsm6dso_hts221_init(struct lsm6dso_data *data, u8_t i2c_addr)
{
u8_t hum_cfg;
/* configure ODR and BDU */
hum_cfg = HTS221_ODR_1HZ | HTS221_BDU | HTS221_PD;
lsm6dso_shub_write_slave_reg(data, i2c_addr,
HTS221_REG_CTRL1, &hum_cfg, 1);
return lsmdso_hts221_read_conv_data(data, i2c_addr);
}
static const u16_t hts221_map[] = {0, 1, 7, 12};
static int lsm6dso_hts221_odr_set(struct lsm6dso_data *data,
u8_t i2c_addr, u16_t freq)
{
u8_t odr, cfg;
for (odr = 0; odr < ARRAY_SIZE(hts221_map); odr++) {
if (freq == hts221_map[odr]) {
break;
}
}
if (odr == ARRAY_SIZE(hts221_map)) {
LOG_DBG("shub: HTS221 freq val %d not supported.", freq);
return -ENOTSUP;
}
cfg = odr | HTS221_BDU | HTS221_PD;
lsm6dso_shub_write_slave_reg(data, i2c_addr,
HTS221_REG_CTRL1, &cfg, 1);
lsm6dso_shub_enable(data, 1);
return 0;
}
static int lsm6dso_hts221_conf(struct lsm6dso_data *data, u8_t i2c_addr,
enum sensor_channel chan,
enum sensor_attribute attr,
const struct sensor_value *val)
{
switch (attr) {
case SENSOR_ATTR_SAMPLING_FREQUENCY:
return lsm6dso_hts221_odr_set(data, i2c_addr, val->val1);
default:
LOG_DBG("shub: HTS221 attribute not supported.");
return -ENOTSUP;
}
return 0;
}
#endif /* CONFIG_LSM6DSO_EXT_HTS221 */
/*
* LPS22HB baro/temp device specific part
*/
#ifdef CONFIG_LSM6DSO_EXT_LPS22HB
#define LPS22HB_CTRL_REG1 0x10
#define LPS22HB_CTRL_REG2 0x11
#define LPS22HB_SW_RESET 0x04
#define LPS22HB_ODR_10HZ 0x20
#define LPS22HB_LPF_EN 0x08
#define LPS22HB_BDU_EN 0x02
static int lsm6dso_lps22hb_init(struct lsm6dso_data *data, u8_t i2c_addr)
{
u8_t baro_cfg[2];
/* sw reset device */
baro_cfg[0] = LPS22HB_SW_RESET;
lsm6dso_shub_write_slave_reg(data, i2c_addr,
LPS22HB_CTRL_REG2, baro_cfg, 1);
k_sleep(1); /* turn-on time in ms */
/* configure device */
baro_cfg[0] = LPS22HB_ODR_10HZ | LPS22HB_LPF_EN | LPS22HB_BDU_EN;
lsm6dso_shub_write_slave_reg(data, i2c_addr,
LPS22HB_CTRL_REG1, baro_cfg, 1);
return 0;
}
#endif /* CONFIG_LSM6DSO_EXT_LPS22HB */
/*
* LPS22HH baro/temp device specific part
*/
#ifdef CONFIG_LSM6DSO_EXT_LPS22HH
#define LPS22HH_CTRL_REG1 0x10
#define LPS22HH_CTRL_REG2 0x11
#define LPS22HH_SW_RESET 0x04
#define LPS22HH_IF_ADD_INC 0x10
#define LPS22HH_ODR_10HZ 0x20
#define LPS22HH_LPF_EN 0x08
#define LPS22HH_BDU_EN 0x02
static int lsm6dso_lps22hh_init(struct lsm6dso_data *data, u8_t i2c_addr)
{
u8_t baro_cfg[2];
/* sw reset device */
baro_cfg[0] = LPS22HH_SW_RESET;
lsm6dso_shub_write_slave_reg(data, i2c_addr,
LPS22HH_CTRL_REG2, baro_cfg, 1);
k_sleep(100); /* turn-on time in ms */
/* configure device */
baro_cfg[0] = LPS22HH_IF_ADD_INC;
lsm6dso_shub_write_slave_reg(data, i2c_addr,
LPS22HH_CTRL_REG2, baro_cfg, 1);
baro_cfg[0] = LPS22HH_ODR_10HZ | LPS22HH_LPF_EN | LPS22HH_BDU_EN;
lsm6dso_shub_write_slave_reg(data, i2c_addr,
LPS22HH_CTRL_REG1, baro_cfg, 1);
return 0;
}
static const u16_t lps22hh_map[] = {0, 1, 10, 25, 50, 75, 100, 200};
static int lsm6dso_lps22hh_odr_set(struct lsm6dso_data *data,
u8_t i2c_addr, u16_t freq)
{
u8_t odr, cfg;
for (odr = 0; odr < ARRAY_SIZE(lps22hh_map); odr++) {
if (freq == lps22hh_map[odr]) {
break;
}
}
if (odr == ARRAY_SIZE(lps22hh_map)) {
LOG_DBG("shub: LPS22HH freq val %d not supported.", freq);
return -ENOTSUP;
}
cfg = (odr << 4) | LPS22HH_LPF_EN | LPS22HH_BDU_EN;
lsm6dso_shub_write_slave_reg(data, i2c_addr,
LPS22HH_CTRL_REG1, &cfg, 1);
lsm6dso_shub_enable(data, 1);
return 0;
}
static int lsm6dso_lps22hh_conf(struct lsm6dso_data *data, u8_t i2c_addr,
enum sensor_channel chan,
enum sensor_attribute attr,
const struct sensor_value *val)
{
switch (attr) {
case SENSOR_ATTR_SAMPLING_FREQUENCY:
return lsm6dso_lps22hh_odr_set(data, i2c_addr, val->val1);
default:
LOG_DBG("shub: LPS22HH attribute not supported.");
return -ENOTSUP;
}
return 0;
}
#endif /* CONFIG_LSM6DSO_EXT_LPS22HH */
/* List of supported external sensors */
static struct lsm6dso_shub_slist {
enum sensor_channel type;
u8_t i2c_addr[2];
u8_t ext_i2c_addr;
u8_t wai_addr;
u8_t wai_val;
u8_t out_data_addr;
u8_t out_data_len;
u8_t sh_out_reg;
int (*dev_init)(struct lsm6dso_data *data, u8_t i2c_addr);
int (*dev_conf)(struct lsm6dso_data *data, u8_t i2c_addr,
enum sensor_channel chan, enum sensor_attribute attr,
const struct sensor_value *val);
} lsm6dso_shub_slist[] = {
#ifdef CONFIG_LSM6DSO_EXT_LIS2MDL
{
/* LIS2MDL */
.type = SENSOR_CHAN_MAGN_XYZ,
.i2c_addr = { 0x1E },
.wai_addr = 0x4F,
.wai_val = 0x40,
.out_data_addr = 0x68,
.out_data_len = 0x06,
.dev_init = (lsm6dso_lis2mdl_init),
.dev_conf = (lsm6dso_lis2mdl_conf),
},
#endif /* CONFIG_LSM6DSO_EXT_LIS2MDL */
#ifdef CONFIG_LSM6DSO_EXT_HTS221
{
/* HTS221 */
.type = SENSOR_CHAN_HUMIDITY,
.i2c_addr = { 0x5F },
.wai_addr = 0x0F,
.wai_val = 0xBC,
.out_data_addr = 0x28 | HTS221_AUTOINCREMENT,
.out_data_len = 0x02,
.dev_init = (lsm6dso_hts221_init),
.dev_conf = (lsm6dso_hts221_conf),
},
#endif /* CONFIG_LSM6DSO_EXT_HTS221 */
#ifdef CONFIG_LSM6DSO_EXT_LPS22HB
{
/* LPS22HB */
.type = SENSOR_CHAN_PRESS,
.i2c_addr = { 0x5C, 0x5D },
.wai_addr = 0x0F,
.wai_val = 0xB1,
.out_data_addr = 0x28,
.out_data_len = 0x05,
.dev_init = (lsm6dso_lps22hb_init),
},
#endif /* CONFIG_LSM6DSO_EXT_LPS22HB */
#ifdef CONFIG_LSM6DSO_EXT_LPS22HH
{
/* LPS22HH */
.type = SENSOR_CHAN_PRESS,
.i2c_addr = { 0x5C, 0x5D },
.wai_addr = 0x0F,
.wai_val = 0xB3,
.out_data_addr = 0x28,
.out_data_len = 0x05,
.dev_init = (lsm6dso_lps22hh_init),
.dev_conf = (lsm6dso_lps22hh_conf),
},
#endif /* CONFIG_LSM6DSO_EXT_LPS22HH */
};
static inline void lsm6dso_shub_wait_completed(struct lsm6dso_data *data)
{
u16_t freq;
freq = (data->accel_freq == 0) ? 26 : data->accel_freq;
k_sleep((2000U / freq) + 1);
}
static inline void lsm6dso_shub_embedded_en(struct lsm6dso_data *data, bool on)
{
if (on) {
lsm6dso_mem_bank_set(data->ctx, LSM6DSO_SENSOR_HUB_BANK);
} else {
lsm6dso_mem_bank_set(data->ctx, LSM6DSO_USER_BANK);
}
k_busy_wait(150);
}
static int lsm6dso_shub_read_embedded_regs(struct lsm6dso_data *data,
u8_t reg_addr,
u8_t *value, int len)
{
lsm6dso_shub_embedded_en(data, true);
if (lsm6dso_read_reg(data->ctx, reg_addr, value, len) < 0) {
LOG_DBG("shub: failed to read external reg: %02x", reg_addr);
lsm6dso_shub_embedded_en(data, false);
return -EIO;
}
lsm6dso_shub_embedded_en(data, false);
return 0;
}
static int lsm6dso_shub_write_embedded_regs(struct lsm6dso_data *data,
u8_t reg_addr,
u8_t *value, u8_t len)
{
lsm6dso_shub_embedded_en(data, true);
if (lsm6dso_write_reg(data->ctx, reg_addr, value, len) < 0) {
LOG_DBG("shub: failed to write external reg: %02x", reg_addr);
lsm6dso_shub_embedded_en(data, false);
return -EIO;
}
lsm6dso_shub_embedded_en(data, false);
return 0;
}
static void lsm6dso_shub_enable(struct lsm6dso_data *data, u8_t enable)
{
/* Enable Accel @26hz */
if (!data->accel_freq) {
u8_t odr = (enable) ? 2 : 0;
if (lsm6dso_xl_data_rate_set(data->ctx, odr) < 0) {
LOG_DBG("shub: failed to set XL sampling rate");
return;
}
}
lsm6dso_shub_embedded_en(data, true);
if (lsm6dso_sh_master_set(data->ctx, enable) < 0) {
LOG_DBG("shub: failed to set master on");
lsm6dso_shub_embedded_en(data, false);
return;
}
lsm6dso_shub_embedded_en(data, false);
}
/* must be called with master on */
static int lsm6dso_shub_check_slv0_nack(struct lsm6dso_data *data)
{
u8_t status;
if (lsm6dso_shub_read_embedded_regs(data, LSM6DSO_SHUB_STATUS_MASTER,
&status, 1) < 0) {
LOG_DBG("shub: error reading embedded reg");
return -EIO;
}
if (status & (LSM6DSO_SHUB_STATUS_SLV0_NACK)) {
LOG_DBG("shub: SLV0 nacked");
return -EIO;
}
return 0;
}
/*
* use SLV0 for generic read to slave device
*/
static int lsm6dso_shub_read_slave_reg(struct lsm6dso_data *data,
u8_t slv_addr, u8_t slv_reg,
u8_t *value, u16_t len)
{
u8_t slave[3];
slave[0] = (slv_addr << 1) | LSM6DSO_SHUB_SLVX_READ;
slave[1] = slv_reg;
slave[2] = (len & 0x7);
if (lsm6dso_shub_write_embedded_regs(data, LSM6DSO_SHUB_SLV0_ADDR,
slave, 3) < 0) {
LOG_DBG("shub: error writing embedded reg");
return -EIO;
}
/* turn SH on */
lsm6dso_shub_enable(data, 1);
lsm6dso_shub_wait_completed(data);
/* read data from external slave */
lsm6dso_shub_embedded_en(data, true);
if (lsm6dso_read_reg(data->ctx, LSM6DSO_SHUB_DATA_OUT,
value, len) < 0) {
LOG_DBG("shub: error reading sensor data");
return -EIO;
}
lsm6dso_shub_embedded_en(data, false);
if (lsm6dso_shub_check_slv0_nack(data) < 0) {
lsm6dso_shub_enable(data, 0);
return -EIO;
}
lsm6dso_shub_enable(data, 0);
return 0;
}
/*
* use SLV0 to configure slave device
*/
static int lsm6dso_shub_write_slave_reg(struct lsm6dso_data *data,
u8_t slv_addr, u8_t slv_reg,
u8_t *value, u16_t len)
{
u8_t slv_cfg[3];
u8_t cnt = 0U;
while (cnt < len) {
slv_cfg[0] = (slv_addr << 1) & ~LSM6DSO_SHUB_SLVX_READ;
slv_cfg[1] = slv_reg + cnt;
if (lsm6dso_shub_write_embedded_regs(data,
LSM6DSO_SHUB_SLV0_ADDR,
slv_cfg, 2) < 0) {
LOG_DBG("shub: error writing embedded reg");
return -EIO;
}
slv_cfg[0] = value[cnt];
if (lsm6dso_shub_write_embedded_regs(data,
LSM6DSO_SHUB_SLV0_DATAWRITE,
slv_cfg, 1) < 0) {
LOG_DBG("shub: error writing embedded reg");
return -EIO;
}
/* turn SH on */
lsm6dso_shub_enable(data, 1);
lsm6dso_shub_wait_completed(data);
if (lsm6dso_shub_check_slv0_nack(data) < 0) {
lsm6dso_shub_enable(data, 0);
return -EIO;
}
lsm6dso_shub_enable(data, 0);
cnt++;
}
/* Put SLV0 in IDLE mode */
slv_cfg[0] = 0x7;
slv_cfg[1] = 0x0;
slv_cfg[2] = 0x0;
if (lsm6dso_shub_write_embedded_regs(data, LSM6DSO_SHUB_SLV0_ADDR,
slv_cfg, 3) < 0) {
LOG_DBG("shub: error writing embedded reg");
return -EIO;
}
return 0;
}
/*
* SLAVEs configurations:
*
* - SLAVE 0: used for configuring all slave devices
* - SLAVE 1: used as data read channel for external slave device #1
* - SLAVE 2: used as data read channel for external slave device #2
* - SLAVE 3: used for generic reads while data channel is enabled
*/
static int lsm6dso_shub_set_data_channel(struct lsm6dso_data *data)
{
u8_t n, i, slv_cfg[6];
struct lsm6dso_shub_slist *sp;
/* Set data channel for slave devices */
for (n = 0; n < num_ext_dev; n++) {
sp = &lsm6dso_shub_slist[shub_ext[n]];
i = n * 3;
slv_cfg[i] = (sp->ext_i2c_addr << 1) | LSM6DSO_SHUB_SLVX_READ;
slv_cfg[i + 1] = sp->out_data_addr;
slv_cfg[i + 2] = sp->out_data_len;
}
if (lsm6dso_shub_write_embedded_regs(data,
LSM6DSO_SHUB_SLV1_ADDR,
slv_cfg, n*3) < 0) {
LOG_DBG("shub: error writing embedded reg");
return -EIO;
}
/* Configure the master */
lsm6dso_aux_sens_on_t aux = LSM6DSO_SLV_0_1_2;
if (lsm6dso_sh_slave_connected_set(data->ctx, aux) < 0) {
LOG_DBG("shub: error setting aux sensors");
return -EIO;
}
lsm6dso_write_once_t wo = LSM6DSO_ONLY_FIRST_CYCLE;
if (lsm6dso_sh_write_mode_set(data->ctx, wo) < 0) {
LOG_DBG("shub: error setting write once");
return -EIO;
}
/* turn SH on */
lsm6dso_shub_enable(data, 1);
lsm6dso_shub_wait_completed(data);
return 0;
}
int lsm6dso_shub_get_idx(enum sensor_channel type)
{
u8_t n;
struct lsm6dso_shub_slist *sp;
for (n = 0; n < num_ext_dev; n++) {
sp = &lsm6dso_shub_slist[shub_ext[n]];
if (sp->type == type)
return n;
}
return -ENOTSUP;
}
int lsm6dso_shub_fetch_external_devs(struct device *dev)
{
u8_t n;
struct lsm6dso_data *data = dev->driver_data;
struct lsm6dso_shub_slist *sp;
/* read data from external slave */
lsm6dso_shub_embedded_en(data, true);
for (n = 0; n < num_ext_dev; n++) {
sp = &lsm6dso_shub_slist[shub_ext[n]];
if (lsm6dso_read_reg(data->ctx, sp->sh_out_reg,
data->ext_data[n], sp->out_data_len) < 0) {
LOG_DBG("shub: failed to read sample");
return -EIO;
}
}
lsm6dso_shub_embedded_en(data, false);
return 0;
}
int lsm6dso_shub_config(struct device *dev, enum sensor_channel chan,
enum sensor_attribute attr,
const struct sensor_value *val)
{
struct lsm6dso_data *data = dev->driver_data;
struct lsm6dso_shub_slist *sp = NULL;
u8_t n;
for (n = 0; n < num_ext_dev; n++) {
sp = &lsm6dso_shub_slist[shub_ext[n]];
if (sp->type == chan)
break;
}
if (n == num_ext_dev) {
LOG_DBG("shub: chan not supported");
return -ENOTSUP;
}
if (sp == NULL || sp->dev_conf == NULL) {
LOG_DBG("shub: chan not configurable");
return -ENOTSUP;
}
return sp->dev_conf(data, sp->ext_i2c_addr, chan, attr, val);
}
int lsm6dso_shub_init(struct device *dev)
{
struct lsm6dso_data *data = dev->driver_data;
u8_t i, n = 0, regn;
u8_t chip_id;
struct lsm6dso_shub_slist *sp;
for (n = 0; n < ARRAY_SIZE(lsm6dso_shub_slist); n++) {
if (num_ext_dev >= LSM6DSO_SHUB_MAX_NUM_SLVS)
break;
chip_id = 0;
sp = &lsm6dso_shub_slist[n];
/*
* The external sensor may have different I2C address.
* So, try them one by one until we read the correct
* chip ID.
*/
for (i = 0U; i < ARRAY_SIZE(sp->i2c_addr); i++) {
if (lsm6dso_shub_read_slave_reg(data,
sp->i2c_addr[i],
sp->wai_addr,
&chip_id, 1) < 0) {
LOG_DBG("shub: failed reading chip id");
continue;
}
if (chip_id == sp->wai_val) {
break;
}
}
if (i >= ARRAY_SIZE(sp->i2c_addr)) {
LOG_DBG("shub: invalid chip id 0x%x", chip_id);
continue;
}
LOG_INF("shub: Ext Device Chip Id: %02x", chip_id);
sp->ext_i2c_addr = sp->i2c_addr[i];
shub_ext[num_ext_dev++] = n;
}
if (num_ext_dev == 0) {
LOG_ERR("shub: no slave devices found");
return -EINVAL;
}
/* init external devices */
for (n = 0, regn = 0; n < num_ext_dev; n++) {
sp = &lsm6dso_shub_slist[shub_ext[n]];
sp->sh_out_reg = LSM6DSO_SHUB_DATA_OUT + regn;
regn += sp->out_data_len;
sp->dev_init(data, sp->ext_i2c_addr);
}
lsm6dso_shub_set_data_channel(data);
return 0;
}

127
drivers/sensor/lsm6dso/lsm6dso_spi.c Normal file
View file

@ -0,0 +1,127 @@
/* ST Microelectronics LSM6DSO 6-axis IMU sensor driver
*
* Copyright (c) 2019 STMicroelectronics
*
* SPDX-License-Identifier: Apache-2.0
*
* Datasheet:
* https://www.st.com/resource/en/datasheet/lsm6dso.pdf
*/
#include <string.h>
#include "lsm6dso.h"
#include <logging/log.h>
#ifdef DT_ST_LSM6DSO_BUS_SPI
#define LSM6DSO_SPI_READ (1 << 7)
#define LOG_LEVEL CONFIG_SENSOR_LOG_LEVEL
LOG_MODULE_DECLARE(LSM6DSO);
static int lsm6dso_spi_read(struct device *dev, u8_t reg_addr,
u8_t *value, u8_t len)
{
struct lsm6dso_data *data = dev->driver_data;
const struct lsm6dso_config *cfg = dev->config->config_info;
const struct spi_config *spi_cfg = &cfg->spi_conf;
u8_t buffer_tx[2] = { reg_addr | LSM6DSO_SPI_READ, 0 };
const struct spi_buf tx_buf = {
.buf = buffer_tx,
.len = 2,
};
const struct spi_buf_set tx = {
.buffers = &tx_buf,
.count = 1
};
const struct spi_buf rx_buf[2] = {
{
.buf = NULL,
.len = 1,
},
{
.buf = value,
.len = len,
}
};
const struct spi_buf_set rx = {
.buffers = rx_buf,
.count = 2
};
if (len > 64) {
return -EIO;
}
if (spi_transceive(data->bus, spi_cfg, &tx, &rx)) {
return -EIO;
}
return 0;
}
static int lsm6dso_spi_write(struct device *dev, u8_t reg_addr,
u8_t *value, u8_t len)
{
struct lsm6dso_data *data = dev->driver_data;
const struct lsm6dso_config *cfg = dev->config->config_info;
const struct spi_config *spi_cfg = &cfg->spi_conf;
u8_t buffer_tx[1] = { reg_addr & ~LSM6DSO_SPI_READ };
const struct spi_buf tx_buf[2] = {
{
.buf = buffer_tx,
.len = 1,
},
{
.buf = value,
.len = len,
}
};
const struct spi_buf_set tx = {
.buffers = tx_buf,
.count = 2
};
if (len > 64) {
return -EIO;
}
if (spi_write(data->bus, spi_cfg, &tx)) {
return -EIO;
}
return 0;
}
int lsm6dso_spi_init(struct device *dev)
{
struct lsm6dso_data *data = dev->driver_data;
data->ctx_spi.read_reg = (lsm6dso_read_ptr) lsm6dso_spi_read,
data->ctx_spi.write_reg = (lsm6dso_write_ptr) lsm6dso_spi_write,
data->ctx = &data->ctx_spi;
data->ctx->handle = dev;
#if defined(DT_INST_0_ST_LSM6DSO_CS_GPIO_CONTROLLER)
const struct lsm6dso_config *cfg = dev->config->config_info;
/* handle SPI CS thru GPIO if it is the case */
data->cs_ctrl.gpio_dev = device_get_binding(cfg->gpio_cs_port);
if (!data->cs_ctrl.gpio_dev) {
LOG_ERR("Unable to get GPIO SPI CS device");
return -ENODEV;
}
data->cs_ctrl.gpio_pin = cfg->cs_gpio;
data->cs_ctrl.delay = 0;
LOG_DBG("SPI GPIO CS configured on %s:%u",
cfg->gpio_cs_port, cfg->cs_gpio);
#endif
return 0;
}
#endif /* DT_ST_LSM6DSO_BUS_SPI */

300
drivers/sensor/lsm6dso/lsm6dso_trigger.c Normal file
View file

@ -0,0 +1,300 @@
/* ST Microelectronics LSM6DSO 6-axis IMU sensor driver
*
* Copyright (c) 2019 STMicroelectronics
*
* SPDX-License-Identifier: Apache-2.0
*
* Datasheet:
* https://www.st.com/resource/en/datasheet/lsm6dso.pdf
*/
#include <kernel.h>
#include <sensor.h>
#include <gpio.h>
#include <logging/log.h>
#include "lsm6dso.h"
#define LOG_LEVEL CONFIG_SENSOR_LOG_LEVEL
LOG_MODULE_DECLARE(LSM6DSO);
#if defined(CONFIG_LSM6DSO_ENABLE_TEMP)
/**
* lsm6dso_enable_t_int - TEMP enable selected int pin to generate interrupt
*/
static int lsm6dso_enable_t_int(struct device *dev, int enable)
{
const struct lsm6dso_config *cfg = dev->config->config_info;
struct lsm6dso_data *lsm6dso = dev->driver_data;
lsm6dso_pin_int2_route_t int2_route;
if (enable) {
axis1bit16_t buf;
/* dummy read: re-trigger interrupt */
lsm6dso_temperature_raw_get(lsm6dso->ctx, buf.u8bit);
}
/* set interrupt (TEMP DRDY interrupt is only on INT2) */
if (cfg->int_pin == 1)
return -EIO;
lsm6dso_read_reg(lsm6dso->ctx, LSM6DSO_INT2_CTRL,
(uint8_t *)&int2_route.int2_ctrl, 1);
int2_route.int2_ctrl.int2_drdy_temp = enable;
return lsm6dso_write_reg(lsm6dso->ctx, LSM6DSO_INT2_CTRL,
(uint8_t *)&int2_route.int2_ctrl, 1);
}
#endif
/**
* lsm6dso_enable_xl_int - XL enable selected int pin to generate interrupt
*/
static int lsm6dso_enable_xl_int(struct device *dev, int enable)
{
const struct lsm6dso_config *cfg = dev->config->config_info;
struct lsm6dso_data *lsm6dso = dev->driver_data;
if (enable) {
axis3bit16_t buf;
/* dummy read: re-trigger interrupt */
lsm6dso_acceleration_raw_get(lsm6dso->ctx, buf.u8bit);
}
/* set interrupt */
if (cfg->int_pin == 1) {
lsm6dso_pin_int1_route_t int1_route;
lsm6dso_read_reg(lsm6dso->ctx, LSM6DSO_INT1_CTRL,
(uint8_t *)&int1_route.int1_ctrl, 1);
int1_route.int1_ctrl.int1_drdy_xl = enable;
return lsm6dso_write_reg(lsm6dso->ctx, LSM6DSO_INT1_CTRL,
(uint8_t *)&int1_route.int1_ctrl, 1);
} else {
lsm6dso_pin_int2_route_t int2_route;
lsm6dso_read_reg(lsm6dso->ctx, LSM6DSO_INT2_CTRL,
(uint8_t *)&int2_route.int2_ctrl, 1);
int2_route.int2_ctrl.int2_drdy_xl = enable;
return lsm6dso_write_reg(lsm6dso->ctx, LSM6DSO_INT2_CTRL,
(uint8_t *)&int2_route.int2_ctrl, 1);
}
}
/**
* lsm6dso_enable_g_int - Gyro enable selected int pin to generate interrupt
*/
static int lsm6dso_enable_g_int(struct device *dev, int enable)
{
const struct lsm6dso_config *cfg = dev->config->config_info;
struct lsm6dso_data *lsm6dso = dev->driver_data;
if (enable) {
axis3bit16_t buf;
/* dummy read: re-trigger interrupt */
lsm6dso_angular_rate_raw_get(lsm6dso->ctx, buf.u8bit);
}
/* set interrupt */
if (cfg->int_pin == 1) {
lsm6dso_pin_int1_route_t int1_route;
lsm6dso_read_reg(lsm6dso->ctx, LSM6DSO_INT1_CTRL,
(uint8_t *)&int1_route.int1_ctrl, 1);
int1_route.int1_ctrl.int1_drdy_g = enable;
return lsm6dso_write_reg(lsm6dso->ctx, LSM6DSO_INT1_CTRL,
(uint8_t *)&int1_route.int1_ctrl, 1);
} else {
lsm6dso_pin_int2_route_t int2_route;
lsm6dso_read_reg(lsm6dso->ctx, LSM6DSO_INT2_CTRL,
(uint8_t *)&int2_route.int2_ctrl, 1);
int2_route.int2_ctrl.int2_drdy_g = enable;
return lsm6dso_write_reg(lsm6dso->ctx, LSM6DSO_INT2_CTRL,
(uint8_t *)&int2_route.int2_ctrl, 1);
}
}
/**
* lsm6dso_trigger_set - link external trigger to event data ready
*/
int lsm6dso_trigger_set(struct device *dev,
const struct sensor_trigger *trig,
sensor_trigger_handler_t handler)
{
struct lsm6dso_data *lsm6dso = dev->driver_data;
if (trig->chan == SENSOR_CHAN_ACCEL_XYZ) {
lsm6dso->handler_drdy_acc = handler;
if (handler) {
return lsm6dso_enable_xl_int(dev, LSM6DSO_EN_BIT);
} else {
return lsm6dso_enable_xl_int(dev, LSM6DSO_DIS_BIT);
}
} else if (trig->chan == SENSOR_CHAN_GYRO_XYZ) {
lsm6dso->handler_drdy_gyr = handler;
if (handler) {
return lsm6dso_enable_g_int(dev, LSM6DSO_EN_BIT);
} else {
return lsm6dso_enable_g_int(dev, LSM6DSO_DIS_BIT);
}
}
#if defined(CONFIG_LSM6DSO_ENABLE_TEMP)
else if (trig->chan == SENSOR_CHAN_DIE_TEMP) {
lsm6dso->handler_drdy_temp = handler;
if (handler) {
return lsm6dso_enable_t_int(dev, LSM6DSO_EN_BIT);
} else {
return lsm6dso_enable_t_int(dev, LSM6DSO_DIS_BIT);
}
}
#endif
return -ENOTSUP;
}
/**
* lsm6dso_handle_interrupt - handle the drdy event
* read data and call handler if registered any
*/
static void lsm6dso_handle_interrupt(void *arg)
{
struct device *dev = arg;
struct lsm6dso_data *lsm6dso = dev->driver_data;
struct sensor_trigger drdy_trigger = {
.type = SENSOR_TRIG_DATA_READY,
};
const struct lsm6dso_config *cfg = dev->config->config_info;
lsm6dso_status_reg_t status;
while (1) {
if (lsm6dso_status_reg_get(lsm6dso->ctx, &status) < 0) {
LOG_DBG("failed reading status reg");
return;
}
if ((status.xlda == 0) && (status.gda == 0)
#if defined(CONFIG_LSM6DSO_ENABLE_TEMP)
&& (status.tda == 0)
#endif
) {
break;
}
if ((status.xlda) && (lsm6dso->handler_drdy_acc != NULL)) {
lsm6dso->handler_drdy_acc(dev, &drdy_trigger);
}
if ((status.gda) && (lsm6dso->handler_drdy_gyr != NULL)) {
lsm6dso->handler_drdy_gyr(dev, &drdy_trigger);
}
#if defined(CONFIG_LSM6DSO_ENABLE_TEMP)
if ((status.tda) && (lsm6dso->handler_drdy_temp != NULL)) {
lsm6dso->handler_drdy_temp(dev, &drdy_trigger);
}
#endif
}
gpio_pin_enable_callback(lsm6dso->gpio, cfg->int_gpio_pin);
}
static void lsm6dso_gpio_callback(struct device *dev,
struct gpio_callback *cb, u32_t pins)
{
struct lsm6dso_data *lsm6dso =
CONTAINER_OF(cb, struct lsm6dso_data, gpio_cb);
const struct lsm6dso_config *cfg = dev->config->config_info;
ARG_UNUSED(pins);
gpio_pin_disable_callback(dev, cfg->int_gpio_pin);
#if defined(CONFIG_LSM6DSO_TRIGGER_OWN_THREAD)
k_sem_give(&lsm6dso->gpio_sem);
#elif defined(CONFIG_LSM6DSO_TRIGGER_GLOBAL_THREAD)
k_work_submit(&lsm6dso->work);
#endif /* CONFIG_LSM6DSO_TRIGGER_OWN_THREAD */
}
#ifdef CONFIG_LSM6DSO_TRIGGER_OWN_THREAD
static void lsm6dso_thread(int dev_ptr, int unused)
{
struct device *dev = INT_TO_POINTER(dev_ptr);
struct lsm6dso_data *lsm6dso = dev->driver_data;
ARG_UNUSED(unused);
while (1) {
k_sem_take(&lsm6dso->gpio_sem, K_FOREVER);
lsm6dso_handle_interrupt(dev);
}
}
#endif /* CONFIG_LSM6DSO_TRIGGER_OWN_THREAD */
#ifdef CONFIG_LSM6DSO_TRIGGER_GLOBAL_THREAD
static void lsm6dso_work_cb(struct k_work *work)
{
struct lsm6dso_data *lsm6dso =
CONTAINER_OF(work, struct lsm6dso_data, work);
lsm6dso_handle_interrupt(lsm6dso->dev);
}
#endif /* CONFIG_LSM6DSO_TRIGGER_GLOBAL_THREAD */
int lsm6dso_init_interrupt(struct device *dev)
{
struct lsm6dso_data *lsm6dso = dev->driver_data;
const struct lsm6dso_config *cfg = dev->config->config_info;
int ret;
/* setup data ready gpio interrupt (INT1 or INT2) */
lsm6dso->gpio = device_get_binding(cfg->int_gpio_port);
if (lsm6dso->gpio == NULL) {
LOG_DBG("Cannot get pointer to %s device",
cfg->int_gpio_port);
return -EINVAL;
}
#if defined(CONFIG_LSM6DSO_TRIGGER_OWN_THREAD)
k_sem_init(&lsm6dso->gpio_sem, 0, UINT_MAX);
k_thread_create(&lsm6dso->thread, lsm6dso->thread_stack,
CONFIG_LSM6DSO_THREAD_STACK_SIZE,
(k_thread_entry_t)lsm6dso_thread, dev,
0, NULL, K_PRIO_COOP(CONFIG_LSM6DSO_THREAD_PRIORITY),
0, 0);
#elif defined(CONFIG_LSM6DSO_TRIGGER_GLOBAL_THREAD)
lsm6dso->work.handler = lsm6dso_work_cb;
lsm6dso->dev = dev;
#endif /* CONFIG_LSM6DSO_TRIGGER_OWN_THREAD */
ret = gpio_pin_configure(lsm6dso->gpio, cfg->int_gpio_pin,
GPIO_DIR_IN | GPIO_INT | GPIO_INT_EDGE |
GPIO_INT_ACTIVE_HIGH | GPIO_INT_DEBOUNCE);
if (ret < 0) {
LOG_DBG("Could not configure gpio");
return ret;
}
gpio_init_callback(&lsm6dso->gpio_cb,
lsm6dso_gpio_callback,
BIT(cfg->int_gpio_pin));
if (gpio_add_callback(lsm6dso->gpio, &lsm6dso->gpio_cb) < 0) {
LOG_DBG("Could not set gpio callback");
return -EIO;
}
/* enable interrupt on int1/int2 in pulse mode */
if (lsm6dso_int_notification_set(lsm6dso->ctx,
LSM6DSO_ALL_INT_PULSED) < 0) {
LOG_DBG("Could not set pulse mode");
return -EIO;
}
return gpio_pin_enable_callback(lsm6dso->gpio, cfg->int_gpio_pin);
}

24
dts/bindings/sensor/st,lsm6dso-i2c.yaml Normal file
View file

@ -0,0 +1,24 @@
#
# Copyright (c) 2019 STMicroelectronics
#
# SPDX-License-Identifier: Apache-2.0
#
title: STMicroelectronics MEMS sensors LSM6DSO
version: 0.1
description: >
This binding gives a base representation of LSM6DSO 6-axis IMU
sensor accessed through I2C bus
inherits:
!include i2c-device.yaml
properties:
compatible:
constraint: "st,lsm6dso"
irq-gpios:
type: compound
category: required
generation: define

24
dts/bindings/sensor/st,lsm6dso-spi.yaml Normal file
View file

@ -0,0 +1,24 @@
#
# Copyright (c) 2019 STMicroelectronics
#
# SPDX-License-Identifier: Apache-2.0
#
title: STMicroelectronics MEMS sensors LSM6DSO SPI
version: 0.1
description: >
This binding gives a base representation of LSM6DSO 6-axis IMU
sensor accessed through SPI bus
inherits:
!include spi-device.yaml
properties:
compatible:
constraint: "st,lsm6dso"
irq-gpios:
type: compound
category: required
generation: define

19
tests/drivers/build_all/dts_fixup.h
View file

@ -121,6 +121,15 @@
#define DT_ST_LSM6DSL_BUS_I2C 1 #define DT_ST_LSM6DSL_BUS_I2C 1
#endif #endif
#ifndef DT_INST_0_ST_LSM6DSO_LABEL
#define DT_INST_0_ST_LSM6DSO_LABEL ""
#define DT_INST_0_ST_LSM6DSO_BASE_ADDRESS 0x6a
#define DT_INST_0_ST_LSM6DSO_BUS_NAME ""
#define DT_INST_0_ST_LSM6DSO_IRQ_GPIOS_CONTROLLER ""
#define DT_INST_0_ST_LSM6DSO_IRQ_GPIOS_PIN 0
#define DT_ST_LSM6DSO_BUS_I2C 1
#endif
#ifndef DT_INST_0_ST_LPS22HB_PRESS_LABEL #ifndef DT_INST_0_ST_LPS22HB_PRESS_LABEL
#define DT_INST_0_ST_LPS22HB_PRESS_LABEL "" #define DT_INST_0_ST_LPS22HB_PRESS_LABEL ""
#define DT_INST_0_ST_LPS22HB_PRESS_BASE_ADDRESS 0 #define DT_INST_0_ST_LPS22HB_PRESS_BASE_ADDRESS 0
@ -265,6 +274,16 @@
#define DT_ST_LSM6DSL_BUS_SPI 1 #define DT_ST_LSM6DSL_BUS_SPI 1
#endif #endif
#ifndef DT_INST_0_ST_LSM6DSO_LABEL
#define DT_INST_0_ST_LSM6DSO_LABEL ""
#define DT_INST_0_ST_LSM6DSO_BASE_ADDRESS 0
#define DT_INST_0_ST_LSM6DSO_BUS_NAME ""
#define DT_INST_0_ST_LSM6DSO_IRQ_GPIOS_CONTROLLER ""
#define DT_INST_0_ST_LSM6DSO_IRQ_GPIOS_PIN 0
#define DT_INST_0_ST_LSM6DSO_SPI_MAX_FREQUENCY 6400000
#define DT_ST_LSM6DSO_BUS_SPI 1
#endif
#ifndef DT_INST_0_ST_LIS2DS12_LABEL #ifndef DT_INST_0_ST_LIS2DS12_LABEL
#define DT_INST_0_ST_LIS2DS12_LABEL "" #define DT_INST_0_ST_LIS2DS12_LABEL ""
#define DT_INST_0_ST_LIS2DS12_BUS_NAME "" #define DT_INST_0_ST_LIS2DS12_BUS_NAME ""

1
tests/drivers/build_all/sensors_i_z.conf
View file

@ -16,6 +16,7 @@ CONFIG_LPS25HB=y
CONFIG_LSM303DLHC_MAGN=y CONFIG_LSM303DLHC_MAGN=y
CONFIG_LSM6DS0=y CONFIG_LSM6DS0=y
CONFIG_LSM6DSL=y CONFIG_LSM6DSL=y
CONFIG_LSM6DSO=y
CONFIG_LSM9DS0_GYRO=y CONFIG_LSM9DS0_GYRO=y
CONFIG_LSM9DS0_MFD=y CONFIG_LSM9DS0_MFD=y
CONFIG_MAX30101=y CONFIG_MAX30101=y

2
tests/drivers/build_all/sensors_trigger_i_z.conf
View file

@ -15,6 +15,8 @@ CONFIG_LIS2MDL=y
CONFIG_LIS2MDL_TRIGGER_OWN_THREAD=y CONFIG_LIS2MDL_TRIGGER_OWN_THREAD=y
CONFIG_LSM6DSL=y CONFIG_LSM6DSL=y
CONFIG_LSM6DSL_TRIGGER_OWN_THREAD=y CONFIG_LSM6DSL_TRIGGER_OWN_THREAD=y
CONFIG_LSM6DSO=y
CONFIG_LSM6DSO_TRIGGER_OWN_THREAD=y
CONFIG_LSM9DS0_GYRO=y CONFIG_LSM9DS0_GYRO=y
CONFIG_LSM9DS0_GYRO_TRIGGERS=y CONFIG_LSM9DS0_GYRO_TRIGGERS=y
CONFIG_LSM9DS0_GYRO_TRIGGER_DRDY=y CONFIG_LSM9DS0_GYRO_TRIGGER_DRDY=y