michaelh/zephyr
1
0
Fork 0
Code Releases Activity

drivers: sensor: lsm6dsl: Adding sensorhub support

Browse source 
Adding sensorhub support. Only one slave device can be selected
among LIS2MDL (magnetometer) and LPS22HB (pressure and temperature).

Signed-off-by: Armando Visconti <armando.visconti@st.com>
This commit is contained in:
Armando Visconti 2018-03-13 15:16:10 +01:00 • committed by Maureen Helm
commit 180b139786
5 changed files with 607 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

9
drivers/sensor/lsm6dsl/CMakeLists.txt
View file

@ -1,4 +1,5 @@
zephyr_sources_ifdef(CONFIG_LSM6DSL lsm6dsl.c)
zephyr_sources_ifdef(CONFIG_LSM6DSL_SPI lsm6dsl_spi.c)
zephyr_sources_ifdef(CONFIG_LSM6DSL_I2C lsm6dsl_i2c.c)
zephyr_sources_ifdef(CONFIG_LSM6DSL_TRIGGER lsm6dsl_trigger.c)
zephyr_sources_ifdef(CONFIG_LSM6DSL lsm6dsl.c)
zephyr_sources_ifdef(CONFIG_LSM6DSL_SPI lsm6dsl_spi.c)
zephyr_sources_ifdef(CONFIG_LSM6DSL_I2C lsm6dsl_i2c.c)
zephyr_sources_ifdef(CONFIG_LSM6DSL_TRIGGER lsm6dsl_trigger.c)
zephyr_sources_ifdef(CONFIG_LSM6DSL_SENSORHUB lsm6dsl_shub.c)

27
drivers/sensor/lsm6dsl/Kconfig
View file

@ -175,6 +175,33 @@ config LSM6DSL_ENABLE_TEMP
help
Enable/disable temperature
config LSM6DSL_SENSORHUB
bool "Enable I2C sensorhub feature"
depends on LSM6DSL
default n
help
Enable/disable internal sensorhub
if LSM6DSL_SENSORHUB
config LSM6DSL_ENABLE_INTERNAL_PULLUP
bool "Enabled internals pull-up resistors"
default y
choice
prompt "External sensor 0"
default LSM6DSL_EXT0_LIS2MDL
help
Choose the external sensor 0 connected to LSM6DS3.
config LSM6DSL_EXT0_LIS2MDL
bool "LIS2MDL"
config LSM6DSL_EXT0_LPS22HB
bool "LPS22HB"
endchoice
endif #LSM6DSL_SENSORHUB
menu "Attributes"
depends on LSM6DSL

161
drivers/sensor/lsm6dsl/lsm6dsl.c
View file

@ -160,6 +160,48 @@ static int lsm6dsl_sample_fetch_temp(struct device *dev)
}
#endif
#if defined(CONFIG_LSM6DSL_EXT0_LIS2MDL)
static int lsm6dsl_sample_fetch_magn(struct device *dev)
{
struct lsm6dsl_data *data = dev->driver_data;
u8_t buf[6];
if (lsm6dsl_shub_read_external_chip(dev, buf, sizeof(buf)) < 0) {
SYS_LOG_DBG("failed to read ext mag sample");
return -EIO;
}
data->magn_sample_x = (s16_t)((u16_t)(buf[0]) |
((u16_t)(buf[1]) << 8));
data->magn_sample_y = (s16_t)((u16_t)(buf[2]) |
((u16_t)(buf[3]) << 8));
data->magn_sample_z = (s16_t)((u16_t)(buf[4]) |
((u16_t)(buf[5]) << 8));
return 0;
}
#endif
#if defined(CONFIG_LSM6DSL_EXT0_LPS22HB)
static int lsm6dsl_sample_fetch_press(struct device *dev)
{
struct lsm6dsl_data *data = dev->driver_data;
u8_t buf[5];
if (lsm6dsl_shub_read_external_chip(dev, buf, sizeof(buf)) < 0) {
SYS_LOG_DBG("failed to read ext press sample");
return -EIO;
}
data->sample_press = (s32_t)((u32_t)(buf[0]) |
((u32_t)(buf[1]) << 8) |
((u32_t)(buf[2]) << 16));
data->sample_temp = (s16_t)((u16_t)(buf[3]) |
((u16_t)(buf[4]) << 8));
return 0;
}
#endif
static int lsm6dsl_sample_fetch(struct device *dev, enum sensor_channel chan)
{
switch (chan) {
@ -173,12 +215,29 @@ static int lsm6dsl_sample_fetch(struct device *dev, enum sensor_channel chan)
case SENSOR_CHAN_DIE_TEMP:
lsm6dsl_sample_fetch_temp(dev);
break;
#endif
#if defined(CONFIG_LSM6DSL_EXT0_LIS2MDL)
case SENSOR_CHAN_MAGN_XYZ:
lsm6dsl_sample_fetch_magn(dev);
break;
#endif
#if defined(CONFIG_LSM6DSL_EXT0_LPS22HB)
case SENSOR_CHAN_AMBIENT_TEMP:
case SENSOR_CHAN_PRESS:
lsm6dsl_sample_fetch_press(dev);
break;
#endif
case SENSOR_CHAN_ALL:
lsm6dsl_sample_fetch_accel(dev);
lsm6dsl_sample_fetch_gyro(dev);
#if defined(CONFIG_LSM6DSL_ENABLE_TEMP)
lsm6dsl_sample_fetch_temp(dev);
#endif
#if defined(CONFIG_LSM6DSL_EXT0_LIS2MDL)
lsm6dsl_sample_fetch_magn(dev);
#endif
#if defined(CONFIG_LSM6DSL_EXT0_LPS22HB)
lsm6dsl_sample_fetch_press(dev);
#endif
break;
default:
@ -295,6 +354,84 @@ static void lsm6dsl_gyro_channel_get_temp(struct sensor_value *val,
}
#endif
#if defined(CONFIG_LSM6DSL_EXT0_LIS2MDL)
static inline void lsm6dsl_magn_convert(struct sensor_value *val, int raw_val,
float 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 lsm6dsl_magn_get_channel(enum sensor_channel chan,
struct sensor_value *val,
struct lsm6dsl_data *data)
{
switch (chan) {
case SENSOR_CHAN_MAGN_X:
lsm6dsl_magn_convert(val,
data->magn_sample_x,
data->magn_sensitivity);
break;
case SENSOR_CHAN_MAGN_Y:
lsm6dsl_magn_convert(val,
data->magn_sample_y,
data->magn_sensitivity);
break;
case SENSOR_CHAN_MAGN_Z:
lsm6dsl_magn_convert(val,
data->magn_sample_z,
data->magn_sensitivity);
break;
case SENSOR_CHAN_MAGN_XYZ:
lsm6dsl_magn_convert(val,
data->magn_sample_x,
data->magn_sensitivity);
lsm6dsl_magn_convert(val + 1,
data->magn_sample_y,
data->magn_sensitivity);
lsm6dsl_magn_convert(val + 2,
data->magn_sample_z,
data->magn_sensitivity);
break;
default:
return -ENOTSUP;
}
return 0;
}
static int lsm6dsl_magn_channel_get(enum sensor_channel chan,
struct sensor_value *val,
struct lsm6dsl_data *data)
{
return lsm6dsl_magn_get_channel(chan, val, data);
}
#endif
#if defined(CONFIG_LSM6DSL_EXT0_LPS22HB)
static inline void lps22hb_press_convert(struct sensor_value *val,
s32_t raw_val)
{
/* 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 lps22hb_temp_convert(struct sensor_value *val,
s16_t raw_val)
{
/* Temperature sensitivity is 100 LSB/deg C */
val->val1 = raw_val / 100;
val->val2 = (s32_t)raw_val % 100 * (10000);
}
#endif
static int lsm6dsl_channel_get(struct device *dev,
enum sensor_channel chan,
struct sensor_value *val)
@ -318,6 +455,23 @@ static int lsm6dsl_channel_get(struct device *dev,
case SENSOR_CHAN_DIE_TEMP:
lsm6dsl_gyro_channel_get_temp(val, data);
break;
#endif
#if defined(CONFIG_LSM6DSL_EXT0_LIS2MDL)
case SENSOR_CHAN_MAGN_X:
case SENSOR_CHAN_MAGN_Y:
case SENSOR_CHAN_MAGN_Z:
case SENSOR_CHAN_MAGN_XYZ:
lsm6dsl_magn_channel_get(chan, val, data);
break;
#endif
#if defined(CONFIG_LSM6DSL_EXT0_LPS22HB)
case SENSOR_CHAN_PRESS:
lps22hb_press_convert(val, data->sample_press);
break;
case SENSOR_CHAN_AMBIENT_TEMP:
lps22hb_temp_convert(val, data->sample_temp);
break;
#endif
default:
return -ENOTSUP;
@ -439,6 +593,13 @@ static int lsm6dsl_init(struct device *dev)
return -EIO;
}
#ifdef CONFIG_LSM6DSL_SENSORHUB
if (lsm6dsl_shub_init_external_chip(dev) < 0) {
SYS_LOG_DBG("failed to initialize external chip");
return -EIO;
}
#endif
return 0;
}

14
drivers/sensor/lsm6dsl/lsm6dsl.h
View file

@ -716,6 +716,16 @@ struct lsm6dsl_data {
int gyro_sample_z;
#if defined(CONFIG_LSM6DSL_ENABLE_TEMP)
int temp_sample;
#endif
#if defined(CONFIG_LSM6DSL_EXT0_LIS2MDL)
int magn_sample_x;
int magn_sample_y;
int magn_sample_z;
float magn_sensitivity;
#endif
#if defined(CONFIG_LSM6DSL_EXT0_LPS22HB)
int sample_press;
int sample_temp;
#endif
const struct lsm6dsl_transfer_function *hw_tf;
@ -740,6 +750,10 @@ struct lsm6dsl_data {
int lsm6dsl_spi_init(struct device *dev);
int lsm6dsl_i2c_init(struct device *dev);
#if defined(CONFIG_LSM6DSL_SENSORHUB)
int lsm6dsl_shub_init_external_chip(struct device *dev);
int lsm6dsl_shub_read_external_chip(struct device *dev, u8_t *buf, u8_t len);
#endif
#ifdef CONFIG_LSM6DSL_TRIGGER
int lsm6dsl_trigger_set(struct device *dev,

400
drivers/sensor/lsm6dsl/lsm6dsl_shub.c Normal file
View file

@ -0,0 +1,400 @@
/*
* Copyright (c) 2018 STMicroelectronics
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <device.h>
#include <i2c.h>
#include <misc/__assert.h>
#include <misc/util.h>
#include <kernel.h>
#include <sensor.h>
#include "lsm6dsl.h"
#define LSM6DSL_EMBEDDED_SLV0_ADDR 0x02
#define LSM6DSL_EMBEDDED_SLV0_SUBADDR 0x03
#define LSM6DSL_EMBEDDED_SLV0_CONFIG 0x04
#define LSM6DSL_EMBEDDED_SLV1_ADDR 0x05
#define LSM6DSL_EMBEDDED_SLV1_SUBADDR 0x06
#define LSM6DSL_EMBEDDED_SLV1_CONFIG 0x07
#define LSM6DSL_EMBEDDED_SLV2_ADDR 0x08
#define LSM6DSL_EMBEDDED_SLV2_SUBADDR 0x09
#define LSM6DSL_EMBEDDED_SLV2_CONFIG 0x0A
#define LSM6DSL_EMBEDDED_SLV3_ADDR 0x0B
#define LSM6DSL_EMBEDDED_SLV3_SUBADDR 0x0C
#define LSM6DSL_EMBEDDED_SLV3_CONFIG 0x0D
#define LSM6DSL_EMBEDDED_SLV0_DATAWRITE 0x0E
#define LSM6DSL_EMBEDDED_SLVX_READ 0x1
#define LSM6DSL_EMBEDDED_SLVX_THREE_SENS 0x20
#define LSM6DSL_EMBEDDED_SLV0_WRITE_IDLE 0x07
static int lsm6dsl_shub_write_slave_reg(struct lsm6dsl_data *data,
u8_t slv_addr, u8_t slv_reg,
u8_t *value, u16_t len);
/*
* LIS2MDL magn device specific part
*/
#ifdef CONFIG_LSM6DSL_EXT0_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_OFF_CANC 0x02
#define LIS2MDL_SENSITIVITY 1500
static int lsm6dsl_lis2mdl_init(struct lsm6dsl_data *data, u8_t i2c_addr)
{
u8_t mag_cfg[2];
data->magn_sensitivity = LIS2MDL_SENSITIVITY;
/* sw reset device */
mag_cfg[0] = LIS2MDL_SW_RESET;
lsm6dsl_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;
lsm6dsl_shub_write_slave_reg(data, i2c_addr,
LIS2MDL_CFG_REG_A, mag_cfg, 2);
return 0;
}
#endif /* CONFIG_LSM6DSL_EXT0_LIS2MDL */
/*
* LPS22HB baro/temp device specific part
*/
#ifdef CONFIG_LSM6DSL_EXT0_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 lsm6dsl_lps22hb_init(struct lsm6dsl_data *data, u8_t i2c_addr)
{
u8_t baro_cfg[2];
/* sw reset device */
baro_cfg[0] = LPS22HB_SW_RESET;
lsm6dsl_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;
lsm6dsl_shub_write_slave_reg(data, i2c_addr,
LPS22HB_CTRL_REG1, baro_cfg, 1);
return 0;
}
#endif /* CONFIG_LSM6DSL_EXT0_LPS22HB */
/* List of supported external sensors */
static struct lsm6dsl_shub_sens_list {
u8_t i2c_addr[2];
u8_t wai_addr;
u8_t wai_val;
u8_t out_data_addr;
u8_t out_data_len;
int (*dev_init)(struct lsm6dsl_data *data, u8_t i2c_addr);
} lsm6dsl_shub_sens_list[] = {
#ifdef CONFIG_LSM6DSL_EXT0_LIS2MDL
{
/* LIS2MDL */
.i2c_addr = { 0x1E },
.wai_addr = 0x4F,
.wai_val = 0x40,
.out_data_addr = 0x68,
.out_data_len = 0x06,
.dev_init = (lsm6dsl_lis2mdl_init),
},
#endif /* CONFIG_LSM6DSL_EXT0_LIS2MDL */
#ifdef CONFIG_LSM6DSL_EXT0_LPS22HB
{
/* LPS22HB */
.i2c_addr = { 0x5C, 0x5D },
.wai_addr = 0x0F,
.wai_val = 0xB1,
.out_data_addr = 0x28,
.out_data_len = 0x05,
.dev_init = (lsm6dsl_lps22hb_init),
},
#endif /* CONFIG_LSM6DSL_EXT0_LPS22HB */
};
static u8_t ext_i2c_addr;
static inline void lsm6dsl_shub_embedded_en(struct lsm6dsl_data *data, bool on)
{
u8_t func_en = (on) ? 0x1 : 0x0;
data->hw_tf->update_reg(data, LSM6DSL_REG_FUNC_CFG_ACCESS,
LSM6DSL_MASK_FUNC_CFG_EN,
func_en << LSM6DSL_SHIFT_FUNC_CFG_EN);
k_sleep(1);
}
#ifdef LSM6DSL_DEBUG
static int lsm6dsl_read_embedded_reg(struct lsm6dsl_data *data,
u8_t reg_addr, u8_t *value, int len)
{
lsm6dsl_shub_embedded_en(data, true);
if (data->hw_tf->read_data(data, reg_addr, value, len) < 0) {
SYS_LOG_DBG("failed to read external reg: %02x", reg_addr);
lsm6dsl_shub_embedded_en(data, false);
return -EIO;
}
lsm6dsl_shub_embedded_en(data, false);
return 0;
}
#endif
static int lsm6dsl_shub_write_embedded_regs(struct lsm6dsl_data *data,
u8_t reg_addr,
u8_t *value, u8_t len)
{
lsm6dsl_shub_embedded_en(data, true);
if (data->hw_tf->write_data(data, reg_addr, value, len) < 0) {
SYS_LOG_DBG("failed to write external reg: %02x", reg_addr);
lsm6dsl_shub_embedded_en(data, false);
return -EIO;
}
lsm6dsl_shub_embedded_en(data, false);
return 0;
}
static void lsm6dsl_shub_enable(struct lsm6dsl_data *data)
{
/* Enable Digital Func */
data->hw_tf->update_reg(data, LSM6DSL_REG_CTRL10_C,
LSM6DSL_MASK_CTRL10_C_FUNC_EN,
1 << LSM6DSL_SHIFT_CTRL10_C_FUNC_EN);
/* Enable Accel (TBD) */
/* data->hw_tf->update_reg(data, LSM6DSL_REG_CTRL1_XL,
* LSM6DSL_MASK_CTRL10_C_FUNC_EN, 1 << LSM6DSL_SHIFT_CTRL10_C_FUNC_EN);
*/
/* Enable Sensor Hub */
data->hw_tf->update_reg(data, LSM6DSL_REG_MASTER_CONFIG,
LSM6DSL_MASK_MASTER_CONFIG_MASTER_ON,
1 << LSM6DSL_SHIFT_MASTER_CONFIG_MASTER_ON);
}
static void lsm6dsl_shub_disable(struct lsm6dsl_data *data)
{
/* Disable Sensor Hub */
data->hw_tf->update_reg(data, LSM6DSL_REG_MASTER_CONFIG,
LSM6DSL_MASK_MASTER_CONFIG_MASTER_ON,
0 << LSM6DSL_SHIFT_MASTER_CONFIG_MASTER_ON);
/* Disable Accel (TBD) */
/* data->hw_tf->update_reg(data, LSM6DSL_REG_CTRL1_XL,
* LSM6DSL_MASK_CTRL10_C_FUNC_EN, 1 << LSM6DSL_SHIFT_CTRL10_C_FUNC_EN);
*/
/* Disable Digital Func */
data->hw_tf->update_reg(data, LSM6DSL_REG_CTRL10_C,
LSM6DSL_MASK_CTRL10_C_FUNC_EN,
0 << LSM6DSL_SHIFT_CTRL10_C_FUNC_EN);
}
/*
* use SLV0 for generic read to slave device
*/
static int lsm6dsl_shub_read_slave_reg(struct lsm6dsl_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) | LSM6DSL_EMBEDDED_SLVX_READ;
slave[1] = slv_reg;
slave[2] = (len & 0x7);
if (lsm6dsl_shub_write_embedded_regs(data, LSM6DSL_EMBEDDED_SLV0_ADDR,
slave, 3) < 0) {
SYS_LOG_DBG("error writing embedded reg");
return -EIO;
}
/* turn SH on */
lsm6dsl_shub_enable(data);
k_sleep(100); /* (TBD) based on odr */
data->hw_tf->read_data(data, LSM6DSL_REG_SENSORHUB1, value, len);
lsm6dsl_shub_disable(data);
return 0;
}
/*
* use SLV0 to configure slave device
*/
static int lsm6dsl_shub_write_slave_reg(struct lsm6dsl_data *data,
u8_t slv_addr, u8_t slv_reg,
u8_t *value, u16_t len)
{
u8_t slv_cfg[3];
u8_t cnt = 0;
while (cnt < len) {
slv_cfg[0] = (slv_addr << 1) & ~LSM6DSL_EMBEDDED_SLVX_READ;
slv_cfg[1] = slv_reg + cnt;
if (lsm6dsl_shub_write_embedded_regs(data,
LSM6DSL_EMBEDDED_SLV0_ADDR,
slv_cfg, 2) < 0) {
SYS_LOG_DBG("error writing embedded reg");
return -EIO;
}
slv_cfg[0] = value[cnt];
if (lsm6dsl_shub_write_embedded_regs(data,
LSM6DSL_EMBEDDED_SLV0_DATAWRITE,
slv_cfg, 1) < 0) {
SYS_LOG_DBG("error writing embedded reg");
return -EIO;
}
/* turn SH on */
lsm6dsl_shub_enable(data);
k_sleep(100); /* (TBD) based on odr */
lsm6dsl_shub_disable(data);
cnt++;
}
/* Put master in IDLE mode */
slv_cfg[0] = LSM6DSL_EMBEDDED_SLV0_WRITE_IDLE;
slv_cfg[1] = lsm6dsl_shub_sens_list[0].wai_addr;
slv_cfg[2] = LSM6DSL_EMBEDDED_SLVX_THREE_SENS;
if (lsm6dsl_shub_write_embedded_regs(data,
LSM6DSL_EMBEDDED_SLV0_ADDR,
slv_cfg, 3) < 0) {
SYS_LOG_DBG("error writing embedded reg");
return -EIO;
}
return 0;
}
/*
* SLAVEs configurations:
*
* - SLAVE 0: used for configuring the slave device
* - SLAVE 1: used as data read channel to slave device
* - SLAVE 2: used for generic reads while data channel is enabled
*/
static int lsm6dsl_shub_set_data_channel(struct lsm6dsl_data *data)
{
u8_t slv_cfg[3];
u8_t slv_i2c_addr = lsm6dsl_shub_sens_list[0].i2c_addr[ext_i2c_addr];
/* SLV0 is used for generic write */
slv_cfg[0] = LSM6DSL_EMBEDDED_SLV0_WRITE_IDLE;
slv_cfg[1] = lsm6dsl_shub_sens_list[0].wai_addr;
slv_cfg[2] = LSM6DSL_EMBEDDED_SLVX_THREE_SENS;
if (lsm6dsl_shub_write_embedded_regs(data,
LSM6DSL_EMBEDDED_SLV0_ADDR,
slv_cfg, 3) < 0) {
SYS_LOG_DBG("error writing embedded reg");
return -EIO;
}
/* Set data channel for slave device */
slv_cfg[0] = (slv_i2c_addr << 1) | LSM6DSL_EMBEDDED_SLVX_READ;
slv_cfg[1] = lsm6dsl_shub_sens_list[0].out_data_addr;
slv_cfg[2] = lsm6dsl_shub_sens_list[0].out_data_len;
if (lsm6dsl_shub_write_embedded_regs(data,
LSM6DSL_EMBEDDED_SLV1_ADDR,
slv_cfg, 3) < 0) {
SYS_LOG_DBG("error writing embedded reg");
return -EIO;
}
/* turn SH on */
lsm6dsl_shub_enable(data);
k_sleep(100); /* (TBD) based on odr */
return 0;
}
int lsm6dsl_shub_read_external_chip(struct device *dev, u8_t *buf, u8_t len)
{
struct lsm6dsl_data *data = dev->driver_data;
data->hw_tf->read_data(data, LSM6DSL_REG_SENSORHUB1, buf, len);
return 0;
}
int lsm6dsl_shub_init_external_chip(struct device *dev)
{
struct lsm6dsl_data *data = dev->driver_data;
u8_t i;
u8_t chip_id = 0;
u8_t slv_i2c_addr;
u8_t slv_wai_addr = lsm6dsl_shub_sens_list[0].wai_addr;
/*
* The external sensor may have different I2C address.
* So, try them one by one until we read the correct
* chip ID.
*/
for (i = 0; i < ARRAY_SIZE(lsm6dsl_shub_sens_list[0].i2c_addr); i++) {
slv_i2c_addr = lsm6dsl_shub_sens_list[0].i2c_addr[i];
if (slv_i2c_addr == 0) {
continue;
}
if (lsm6dsl_shub_read_slave_reg(data, slv_i2c_addr,
slv_wai_addr,
&chip_id, 1) < 0) {
SYS_LOG_DBG("failed reading external chip id");
return -EIO;
}
if (chip_id == lsm6dsl_shub_sens_list[0].wai_val) {
break;
}
}
if (i >= ARRAY_SIZE(lsm6dsl_shub_sens_list[0].i2c_addr)) {
SYS_LOG_DBG("invalid chip id 0x%x", chip_id);
return -EIO;
}
SYS_LOG_DBG("Ext Device Chip Id: %02x", chip_id);
ext_i2c_addr = i;
/* init external device */
lsm6dsl_shub_sens_list[0].dev_init(data, slv_i2c_addr);
lsm6dsl_shub_set_data_channel(data);
return 0;
}