drivers: sensor: lsm6dsl: add .attr_set callback

In order to be able to change at runtime the Output Data
Rate (ODR) as well as the Full-Scale (FS) for both the
Accelerometr and Gyroscope the driver needs to provide
the .attr_set callback.

Signed-off-by: Armando Visconti <armando.visconti@st.com>
This commit is contained in:
Armando Visconti 2018-04-17 09:32:43 +02:00 committed by Maureen Helm
commit f7f56ccda5
4 changed files with 341 additions and 137 deletions

View file

@ -205,46 +205,74 @@ endif #LSM6DSL_SENSORHUB
menu "Attributes"
depends on LSM6DSL
config LSM6DSL_GYRO_FULLSCALE
config LSM6DSL_GYRO_FS
int
prompt "Gyroscope full-scale range"
depends on LSM6DSL
default 2000
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 125, 245, 500, 1000 and 2000.
second. Valid values are:
0: Full Scale selected at runtime
125: +/- 125dps
245: +/- 245dps
500: +/- 500dps
1000: +/- 1000dps
2000: +/- 2000dps
config LSM6DSL_GYRO_SAMPLING_RATE
int
prompt "Output data rate"
config LSM6DSL_GYRO_ODR
int "Gyroscope Output data rate frequency"
depends on LSM6DSL
default 26
help
Specify the default gyroscope output data rate expressed in samples
per second (Hz).
Data rates supported by the chip are 0, 13, 26, 52, 104, 208, 416,
833, 1660, 3330, 6660.
config LSM6DSL_ACCEL_FULLSCALE
int
prompt "Accelerometer full-scale range"
depends on LSM6DSL
default 2
help
Specify the default accelerometer full-scale range.
An X value for the config represents a range of +/- X G. Valid values
are 2, 4, 8 and 16.
config LSM6DSL_ACCEL_SAMPLING_RATE
int
prompt "Output data rate"
depends on LSM6DSL
default 26
range 0 10
default 0
help
Specify the default accelerometer output data rate expressed in
samples per second (Hz).
Data rates supported by the chip are 0, 2, 13, 26, 52, 104, 208,
416, 833, 1660, 3330, 6660.
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 LSM6DSL_ACCEL_FS
int
prompt "Accelerometer full-scale range"
depends on LSM6DSL
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 LSM6DSL_ACCEL_ODR
int "Accelerometer Output data rate frequency"
depends on LSM6DSL
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

View file

@ -18,6 +18,69 @@
#include "lsm6dsl.h"
static const u16_t lsm6dsl_odr_map[] = {0, 12, 26, 52, 104, 208, 416, 833,
1660, 3330, 6660};
#if defined(LSM6DSL_ACCEL_ODR_RUNTIME) || defined(LSM6DSL_GYRO_ODR_RUNTIME)
static int lsm6dsl_freq_to_odr_val(u16_t freq)
{
size_t i;
for (i = 0; i < ARRAY_SIZE(lsm6dsl_odr_map); i++) {
if (freq == lsm6dsl_odr_map[i]) {
return i;
}
}
return -EINVAL;
}
#endif
static int lsm6dsl_odr_to_freq_val(u16_t odr)
{
if (odr > ARRAY_SIZE(lsm6dsl_odr_map)) {
odr = ARRAY_SIZE(lsm6dsl_odr_map);
}
return lsm6dsl_odr_map[odr];
}
#ifdef LSM6DSL_ACCEL_FS_RUNTIME
static const u16_t lsm6dsl_accel_fs_map[] = {2, 16, 4, 8};
static const u16_t lsm6dsl_accel_fs_sens[] = {1, 8, 2, 4};
static int lsm6dsl_accel_range_to_fs_val(s32_t range)
{
size_t i;
for (i = 0; i < ARRAY_SIZE(lsm6dsl_accel_fs_map); i++) {
if (range == lsm6dsl_accel_fs_map[i]) {
return i;
}
}
return -EINVAL;
}
#endif
#ifdef LSM6DSL_GYRO_FS_RUNTIME
static const u16_t lsm6dsl_gyro_fs_map[] = {245, 500, 1000, 2000, 125};
static const u16_t lsm6dsl_gyro_fs_sens[] = {2, 4, 8, 16, 1};
static int lsm6dsl_gyro_range_to_fs_val(s32_t range)
{
size_t i;
for (i = 0; i < ARRAY_SIZE(lsm6dsl_gyro_fs_map); i++) {
if (range == lsm6dsl_gyro_fs_map[i]) {
return i;
}
}
return -EINVAL;
}
#endif
static inline int lsm6dsl_reboot(struct device *dev)
{
struct lsm6dsl_data *data = dev->driver_data;
@ -45,6 +108,8 @@ static int lsm6dsl_accel_set_fs_raw(struct device *dev, u8_t fs)
return -EIO;
}
data->accel_fs = fs;
return 0;
}
@ -59,6 +124,8 @@ static int lsm6dsl_accel_set_odr_raw(struct device *dev, u8_t odr)
return -EIO;
}
data->accel_freq = lsm6dsl_odr_to_freq_val(odr);
return 0;
}
@ -99,6 +166,147 @@ static int lsm6dsl_gyro_set_odr_raw(struct device *dev, u8_t odr)
return 0;
}
#ifdef LSM6DSL_ACCEL_ODR_RUNTIME
static int lsm6dsl_accel_odr_set(struct device *dev, u16_t freq)
{
int odr;
odr = lsm6dsl_freq_to_odr_val(freq);
if (odr < 0) {
return odr;
}
if (lsm6dsl_accel_set_odr_raw(dev, odr) < 0) {
SYS_LOG_DBG("failed to set accelerometer sampling rate");
return -EIO;
}
return 0;
}
#endif
#ifdef LSM6DSL_ACCEL_FS_RUNTIME
static int lsm6dsl_accel_range_set(struct device *dev, s32_t range)
{
int fs;
struct lsm6dsl_data *data = dev->driver_data;
fs = lsm6dsl_accel_range_to_fs_val(range);
if (fs < 0) {
return fs;
}
if (lsm6dsl_accel_set_fs_raw(dev, fs) < 0) {
SYS_LOG_DBG("failed to set accelerometer full-scale");
return -EIO;
}
data->accel_sensitivity = (float)(lsm6dsl_accel_fs_sens[fs]
* SENSI_GRAIN_XL);
return 0;
}
#endif
static int lsm6dsl_accel_config(struct device *dev, enum sensor_channel chan,
enum sensor_attribute attr,
const struct sensor_value *val)
{
switch (attr) {
#ifdef LSM6DSL_ACCEL_FS_RUNTIME
case SENSOR_ATTR_FULL_SCALE:
return lsm6dsl_accel_range_set(dev, sensor_ms2_to_g(val));
#endif
#ifdef LSM6DSL_ACCEL_ODR_RUNTIME
case SENSOR_ATTR_SAMPLING_FREQUENCY:
return lsm6dsl_accel_odr_set(dev, val->val1);
#endif
default:
SYS_LOG_DBG("Accel attribute not supported.");
return -ENOTSUP;
}
return 0;
}
#ifdef LSM6DSL_GYRO_ODR_RUNTIME
static int lsm6dsl_gyro_odr_set(struct device *dev, u16_t freq)
{
int odr;
odr = lsm6dsl_freq_to_odr_val(freq);
if (odr < 0) {
return odr;
}
if (lsm6dsl_gyro_set_odr_raw(dev, odr) < 0) {
SYS_LOG_DBG("failed to set gyroscope sampling rate");
return -EIO;
}
return 0;
}
#endif
#ifdef LSM6DSL_GYRO_FS_RUNTIME
static int lsm6dsl_gyro_range_set(struct device *dev, s32_t range)
{
int fs;
struct lsm6dsl_data *data = dev->driver_data;
fs = lsm6dsl_gyro_range_to_fs_val(range);
if (fs < 0) {
return fs;
}
if (lsm6dsl_gyro_set_fs_raw(dev, fs) < 0) {
SYS_LOG_DBG("failed to set gyroscope full-scale");
return -EIO;
}
data->gyro_sensitivity = (float)(lsm6dsl_gyro_fs_sens[fs]
* SENSI_GRAIN_G);
return 0;
}
#endif
static int lsm6dsl_gyro_config(struct device *dev, enum sensor_channel chan,
enum sensor_attribute attr,
const struct sensor_value *val)
{
switch (attr) {
#ifdef LSM6DSL_GYRO_FS_RUNTIME
case SENSOR_ATTR_FULL_SCALE:
return lsm6dsl_gyro_range_set(dev, sensor_ms2_to_g(val));
#endif
#ifdef LSM6DSL_GYRO_ODR_RUNTIME
case SENSOR_ATTR_SAMPLING_FREQUENCY:
return lsm6dsl_gyro_odr_set(dev, val->val1);
#endif
default:
SYS_LOG_DBG("Gyro attribute not supported.");
return -ENOTSUP;
}
return 0;
}
static int lsm6dsl_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 lsm6dsl_accel_config(dev, chan, attr, val);
case SENSOR_CHAN_GYRO_XYZ:
return lsm6dsl_gyro_config(dev, chan, attr, val);
default:
SYS_LOG_WRN("attr_set() not supported on this channel.");
return -ENOTSUP;
}
return 0;
}
static int lsm6dsl_sample_fetch_accel(struct device *dev)
{
struct lsm6dsl_data *data = dev->driver_data;
@ -294,7 +502,7 @@ static int lsm6dsl_accel_channel_get(enum sensor_channel chan,
struct lsm6dsl_data *data)
{
return lsm6dsl_accel_get_channel(chan, val, data,
LSM6DSL_DEFAULT_ACCEL_SENSITIVITY);
data->accel_sensitivity);
}
static inline void lsm6dsl_gyro_convert(struct sensor_value *val, int raw_val,
@ -481,6 +689,7 @@ static int lsm6dsl_channel_get(struct device *dev,
}
static const struct sensor_driver_api lsm6dsl_api_funcs = {
.attr_set = lsm6dsl_attr_set,
#if CONFIG_LSM6DSL_TRIGGER
.trigger_set = lsm6dsl_trigger_set,
#endif
@ -510,13 +719,14 @@ static int lsm6dsl_init_chip(struct device *dev)
SYS_LOG_DBG("chip id 0x%x", chip_id);
if (lsm6dsl_accel_set_fs_raw(dev,
LSM6DSL_DEFAULT_ACCEL_FULLSCALE) < 0) {
LSM6DSL_DEFAULT_ACCEL_FULLSCALE) < 0) {
SYS_LOG_DBG("failed to set accelerometer full-scale");
return -EIO;
}
data->accel_sensitivity = LSM6DSL_DEFAULT_ACCEL_SENSITIVITY;
if (lsm6dsl_accel_set_odr_raw(dev,
LSM6DSL_DEFAULT_ACCEL_SAMPLING_RATE) < 0) {
data->accel_freq = lsm6dsl_odr_to_freq_val(CONFIG_LSM6DSL_ACCEL_ODR);
if (lsm6dsl_accel_set_odr_raw(dev, CONFIG_LSM6DSL_ACCEL_ODR) < 0) {
SYS_LOG_DBG("failed to set accelerometer sampling rate");
return -EIO;
}
@ -525,9 +735,10 @@ static int lsm6dsl_init_chip(struct device *dev)
SYS_LOG_DBG("failed to set gyroscope full-scale");
return -EIO;
}
data->gyro_sensitivity = LSM6DSL_DEFAULT_GYRO_SENSITIVITY;
if (lsm6dsl_gyro_set_odr_raw(dev,
LSM6DSL_DEFAULT_GYRO_SAMPLING_RATE) < 0) {
data->gyro_freq = lsm6dsl_odr_to_freq_val(CONFIG_LSM6DSL_GYRO_ODR);
if (lsm6dsl_gyro_set_odr_raw(dev, CONFIG_LSM6DSL_GYRO_ODR) < 0) {
SYS_LOG_DBG("failed to set gyroscope sampling rate");
return -EIO;
}

View file

@ -554,109 +554,54 @@
#define SENSOR_DEG2RAD_DOUBLE (SENSOR_PI_DOUBLE / 180)
#define SENSOR_G_DOUBLE (SENSOR_G / 1000000.0)
#if CONFIG_LSM6DSL_ACCEL_FULLSCALE == 2
#define LSM6DSL_ACCEL_FULLSCALE_2G
#elif CONFIG_LSM6DSL_ACCEL_FULLSCALE == 4
#define LSM6DSL_ACCEL_FULLSCALE_4G
#elif CONFIG_LSM6DSL_ACCEL_FULLSCALE == 8
#define LSM6DSL_ACCEL_FULLSCALE_8G
#elif CONFIG_LSM6DSL_ACCEL_FULLSCALE == 16
#define LSM6DSL_ACCEL_FULLSCALE_16G
#endif
#if defined(LSM6DSL_ACCEL_FULLSCALE_2G)
#if CONFIG_LSM6DSL_ACCEL_FS == 0
#define LSM6DSL_ACCEL_FS_RUNTIME 1
#define LSM6DSL_DEFAULT_ACCEL_FULLSCALE 0
#define LSM6DSL_DEFAULT_ACCEL_SENSITIVITY SENSI_GRAIN_XL
#elif defined(LSM6DSL_ACCEL_FULLSCALE_4G)
#elif CONFIG_LSM6DSL_ACCEL_FS == 2
#define LSM6DSL_DEFAULT_ACCEL_FULLSCALE 0
#define LSM6DSL_DEFAULT_ACCEL_SENSITIVITY SENSI_GRAIN_XL
#elif CONFIG_LSM6DSL_ACCEL_FS == 4
#define LSM6DSL_DEFAULT_ACCEL_FULLSCALE 2
#define LSM6DSL_DEFAULT_ACCEL_SENSITIVITY (2.0 * SENSI_GRAIN_XL)
#elif defined(LSM6DSL_ACCEL_FULLSCALE_8G)
#elif CONFIG_LSM6DSL_ACCEL_FS == 8
#define LSM6DSL_DEFAULT_ACCEL_FULLSCALE 3
#define LSM6DSL_DEFAULT_ACCEL_SENSITIVITY (4.0 * SENSI_GRAIN_XL)
#elif defined(LSM6DSL_ACCEL_FULLSCALE_16G)
#define LSM6DSL_DEFALUT_ACCEL_FULLSCALE 1
#elif CONFIG_LSM6DSL_ACCEL_FS == 16
#define LSM6DSL_DEFAULT_ACCEL_FULLSCALE 1
#define LSM6DSL_DEFAULT_ACCEL_SENSITIVITY (8.0 * SENSI_GRAIN_XL)
#endif
#if CONFIG_LSM6DSL_ACCEL_SAMPLING_RATE == 0
#define LSM6DSL_DEFAULT_ACCEL_SAMPLING_RATE 0
#elif CONFIG_LSM6DSL_ACCEL_SAMPLING_RATE == 2
#define LSM6DSL_DEFAULT_ACCEL_SAMPLING_RATE 11
#elif CONFIG_LSM6DSL_ACCEL_SAMPLING_RATE == 13
#define LSM6DSL_DEFAULT_ACCEL_SAMPLING_RATE 1
#elif CONFIG_LSM6DSL_ACCEL_SAMPLING_RATE == 26
#define LSM6DSL_DEFAULT_ACCEL_SAMPLING_RATE 2
#elif CONFIG_LSM6DSL_ACCEL_SAMPLING_RATE == 52
#define LSM6DSL_DEFAULT_ACCEL_SAMPLING_RATE 3
#elif CONFIG_LSM6DSL_ACCEL_SAMPLING_RATE == 104
#define LSM6DSL_DEFAULT_ACCEL_SAMPLING_RATE 4
#elif CONFIG_LSM6DSL_ACCEL_SAMPLING_RATE == 208
#define LSM6DSL_DEFAULT_ACCEL_SAMPLING_RATE 5
#elif CONFIG_LSM6DSL_ACCEL_SAMPLING_RATE == 416
#define LSM6DSL_DEFAULT_ACCEL_SAMPLING_RATE 6
#elif CONFIG_LSM6DSL_ACCEL_SAMPLING_RATE == 833
#define LSM6DSL_DEFAULT_ACCEL_SAMPLING_RATE 7
#elif CONFIG_LSM6DSL_ACCEL_SAMPLING_RATE == 1660
#define LSM6DSL_DEFAULT_ACCEL_SAMPLING_RATE 8
#elif CONFIG_LSM6DSL_ACCEL_SAMPLING_RATE == 3330
#define LSM6DSL_DEFAULT_ACCEL_SAMPLING_RATE 9
#elif CONFIG_LSM6DSL_ACCEL_SAMPLING_RATE == 6660
#define LSM6DSL_DEFAULT_ACCEL_SAMPLING_RATE 10
#endif
#if CONFIG_LSM6DSL_GYRO_FULLSCALE == 125
#define LSM6DSL_GYRO_FULLSCALE_125
#elif CONFIG_LSM6DSL_GYRO_FULLSCALE == 245
#define LSM6DSL_GYRO_FULLSCALE_245
#elif CONFIG_LSM6DSL_GYRO_FULLSCALE == 500
#define LSM6DSL_GYRO_FULLSCALE_500
#elif CONFIG_LSM6DSL_GYRO_FULLSCALE == 1000
#define LSM6DSL_GYRO_FULLSCALE_1000
#elif CONFIG_LSM6DSL_GYRO_FULLSCALE == 2000
#define LSM6DSL_GYRO_FULLSCALE_2000
#endif
#if defined(LSM6DSL_GYRO_FULLSCALE_125)
#define LSM6DSL_DEFAULT_GYRO_FULLSCALE 4
#define LSM6DSL_DEFAULT_GYRO_SENSITIVITY SENSI_GRAIN_G
#elif defined(LSM6DSL_GYRO_FULLSCALE_245)
#define LSM6DSL_DEFAULT_GYRO_FULLSCALE 0
#define LSM6DSL_DEFAULT_GYRO_SENSITIVITY (2.0 * SENSI_GRAIN_G)
#elif defined(LSM6DSL_GYRO_FULLSCALE_500)
#define LSM6DSL_DEFAULT_GYRO_FULLSCALE 1
#define LSM6DSL_DEFAULT_GYRO_SENSITIVITY (4.0 * SENSI_GRAIN_G)
#elif defined(LSM6DSL_GYRO_FULLSCALE_1000)
#define LSM6DSL_DEFAULT_GYRO_FULLSCALE 2
#define LSM6DSL_DEFAULT_GYRO_SENSITIVITY (8.0 * SENSI_GRAIN_G)
#elif defined(LSM6DSL_GYRO_FULLSCALE_2000)
#define LSM6DSL_DEFAULT_GYRO_FULLSCALE 3
#define LSM6DSL_DEFAULT_GYRO_SENSITIVITY (16.0 * SENSI_GRAIN_G)
#if (CONFIG_LSM6DSL_ACCEL_ODR == 0)
#define LSM6DSL_ACCEL_ODR_RUNTIME 1
#endif
#define GYRO_FULLSCALE_125 4
#if CONFIG_LSM6DSL_GYRO_SAMPLING_RATE == 0
#define LSM6DSL_DEFAULT_GYRO_SAMPLING_RATE 0
#elif CONFIG_LSM6DSL_GYRO_SAMPLING_RATE == 13
#define LSM6DSL_DEFAULT_GYRO_SAMPLING_RATE 1
#elif CONFIG_LSM6DSL_GYRO_SAMPLING_RATE == 26
#define LSM6DSL_DEFAULT_GYRO_SAMPLING_RATE 2
#elif CONFIG_LSM6DSL_GYRO_SAMPLING_RATE == 52
#define LSM6DSL_DEFAULT_GYRO_SAMPLING_RATE 3
#elif CONFIG_LSM6DSL_GYRO_SAMPLING_RATE == 104
#define LSM6DSL_DEFAULT_GYRO_SAMPLING_RATE 4
#elif CONFIG_LSM6DSL_GYRO_SAMPLING_RATE == 208
#define LSM6DSL_DEFAULT_GYRO_SAMPLING_RATE 5
#elif CONFIG_LSM6DSL_GYRO_SAMPLING_RATE == 416
#define LSM6DSL_DEFAULT_GYRO_SAMPLING_RATE 6
#elif CONFIG_LSM6DSL_GYRO_SAMPLING_RATE == 833
#define LSM6DSL_DEFAULT_GYRO_SAMPLING_RATE 7
#elif CONFIG_LSM6DSL_GYRO_SAMPLING_RATE == 1660
#define LSM6DSL_DEFAULT_GYRO_SAMPLING_RATE 8
#elif CONFIG_LSM6DSL_GYRO_SAMPLING_RATE == 3330
#define LSM6DSL_DEFAULT_GYRO_SAMPLING_RATE 9
#elif CONFIG_LSM6DSL_GYRO_SAMPLING_RATE == 6660
#define LSM6DSL_DEFAULT_GYRO_SAMPLING_RATE 10
#if CONFIG_LSM6DSL_GYRO_FS == 0
#define LSM6DSL_GYRO_FS_RUNTIME 1
#define LSM6DSL_DEFAULT_GYRO_FULLSCALE 4
#define LSM6DSL_DEFAULT_GYRO_SENSITIVITY SENSI_GRAIN_G
#elif CONFIG_LSM6DSL_GYRO_FS == 125
#define LSM6DSL_DEFAULT_GYRO_FULLSCALE 4
#define LSM6DSL_DEFAULT_GYRO_SENSITIVITY SENSI_GRAIN_G
#elif CONFIG_LSM6DSL_GYRO_FS == 245
#define LSM6DSL_DEFAULT_GYRO_FULLSCALE 0
#define LSM6DSL_DEFAULT_GYRO_SENSITIVITY (2.0 * SENSI_GRAIN_G)
#elif CONFIG_LSM6DSL_GYRO_FS == 500
#define LSM6DSL_DEFAULT_GYRO_FULLSCALE 1
#define LSM6DSL_DEFAULT_GYRO_SENSITIVITY (4.0 * SENSI_GRAIN_G)
#elif CONFIG_LSM6DSL_GYRO_FS == 1000
#define LSM6DSL_DEFAULT_GYRO_FULLSCALE 2
#define LSM6DSL_DEFAULT_GYRO_SENSITIVITY (8.0 * SENSI_GRAIN_G)
#elif CONFIG_LSM6DSL_GYRO_FS == 2000
#define LSM6DSL_DEFAULT_GYRO_FULLSCALE 3
#define LSM6DSL_DEFAULT_GYRO_SENSITIVITY (16.0 * SENSI_GRAIN_G)
#endif
#if (CONFIG_LSM6DSL_GYRO_ODR == 0)
#define LSM6DSL_GYRO_ODR_RUNTIME 1
#endif
struct lsm6dsl_config {
@ -681,9 +626,11 @@ struct lsm6dsl_data {
int accel_sample_x;
int accel_sample_y;
int accel_sample_z;
float accel_sensitivity;
int gyro_sample_x;
int gyro_sample_y;
int gyro_sample_z;
float gyro_sensitivity;
#if defined(CONFIG_LSM6DSL_ENABLE_TEMP)
int temp_sample;
#endif
@ -698,6 +645,10 @@ struct lsm6dsl_data {
int sample_temp;
#endif
const struct lsm6dsl_transfer_function *hw_tf;
u16_t accel_freq;
u8_t accel_fs;
u16_t gyro_freq;
u8_t gyro_fs;
#ifdef CONFIG_LSM6DSL_TRIGGER
struct device *gpio;

View file

@ -142,6 +142,14 @@ static struct lsm6dsl_shub_sens_list {
static u8_t ext_i2c_addr;
static inline void lsm6dsl_shub_wait_completed(struct lsm6dsl_data *data)
{
u16_t freq;
freq = (data->accel_freq == 0) ? 26 : data->accel_freq;
k_sleep((2000U / freq) + 1);
}
static inline void lsm6dsl_shub_embedded_en(struct lsm6dsl_data *data, bool on)
{
u8_t func_en = (on) ? 0x1 : 0x0;
@ -195,10 +203,13 @@ static void lsm6dsl_shub_enable(struct lsm6dsl_data *data)
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 Accel @26hz */
if (!data->accel_freq) {
data->hw_tf->update_reg(data,
LSM6DSL_REG_CTRL1_XL,
LSM6DSL_MASK_CTRL1_XL_ODR_XL,
2 << LSM6DSL_SHIFT_CTRL1_XL_ODR_XL);
}
/* Enable Sensor Hub */
data->hw_tf->update_reg(data, LSM6DSL_REG_MASTER_CONFIG,
@ -213,10 +224,13 @@ static void lsm6dsl_shub_disable(struct lsm6dsl_data *data)
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 Accel */
if (!data->accel_freq) {
data->hw_tf->update_reg(data,
LSM6DSL_REG_CTRL1_XL,
LSM6DSL_MASK_CTRL1_XL_ODR_XL,
0 << LSM6DSL_SHIFT_CTRL1_XL_ODR_XL);
}
/* Disable Digital Func */
data->hw_tf->update_reg(data, LSM6DSL_REG_CTRL10_C,
@ -245,7 +259,7 @@ static int lsm6dsl_shub_read_slave_reg(struct lsm6dsl_data *data,
/* turn SH on */
lsm6dsl_shub_enable(data);
k_sleep(100); /* (TBD) based on odr */
lsm6dsl_shub_wait_completed(data);
data->hw_tf->read_data(data, LSM6DSL_REG_SENSORHUB1, value, len);
lsm6dsl_shub_disable(data);
@ -283,7 +297,7 @@ static int lsm6dsl_shub_write_slave_reg(struct lsm6dsl_data *data,
/* turn SH on */
lsm6dsl_shub_enable(data);
k_sleep(100); /* (TBD) based on odr */
lsm6dsl_shub_wait_completed(data);
lsm6dsl_shub_disable(data);
cnt++;
@ -339,7 +353,7 @@ static int lsm6dsl_shub_set_data_channel(struct lsm6dsl_data *data)
/* turn SH on */
lsm6dsl_shub_enable(data);
k_sleep(100); /* (TBD) based on odr */
lsm6dsl_shub_wait_completed(data);
return 0;
}