Skip to content

Commit cc9b93d

Browse files
aviscontikartben
avisconti
authored and
kartben
committed
sensor: lsm6dsv16x: sensor GBIAS initialization
Use SENSOR_ATTR_OFFSET attribute to initialize the Sensor Fusion GBIAS. This is useful when the device is not sill (e.g. hold in hand) and so the gyroscope calibration does not run. Signed-off-by: Armando Visconti <[email protected]>
1 parent 9a8ecf0 commit cc9b93d

File tree

4 files changed

+107
-2
lines changed

4 files changed

+107
-2
lines changed

‎drivers/sensor/st/lsm6dsv16x/lsm6dsv16x.c

Lines changed: 10 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -165,7 +165,7 @@ static int lsm6dsv16x_accel_set_fs_raw(const struct device *dev, uint8_t fs)
165165
return 0;
166166
}
167167

168-
static int lsm6dsv16x_accel_set_odr_raw(const struct device *dev, uint8_t odr)
168+
int lsm6dsv16x_accel_set_odr_raw(const struct device *dev, uint8_t odr)
169169
{
170170
const struct lsm6dsv16x_config *cfg = dev->config;
171171
stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
@@ -194,7 +194,7 @@ static int lsm6dsv16x_gyro_set_fs_raw(const struct device *dev, uint8_t fs)
194194
return 0;
195195
}
196196

197-
static int lsm6dsv16x_gyro_set_odr_raw(const struct device *dev, uint8_t odr)
197+
int lsm6dsv16x_gyro_set_odr_raw(const struct device *dev, uint8_t odr)
198198
{
199199
const struct lsm6dsv16x_config *cfg = dev->config;
200200
stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
@@ -457,6 +457,10 @@ static int lsm6dsv16x_attr_set(const struct device *dev,
457457

458458
return lsm6dsv16x_shub_config(dev, chan, attr, val);
459459
#endif /* CONFIG_LSM6DSV16X_SENSORHUB */
460+
#ifdef CONFIG_LSM6DSV16X_STREAM
461+
case SENSOR_CHAN_GBIAS_XYZ:
462+
return lsm6dsv16x_gbias_config(dev, chan, attr, val);
463+
#endif /* CONFIG_LSM6DSV16X_STREAM */
460464
default:
461465
LOG_WRN("attr_set() not supported on this channel.");
462466
return -ENOTSUP;
@@ -645,6 +649,10 @@ static int lsm6dsv16x_attr_get(const struct device *dev, enum sensor_channel cha
645649
return lsm6dsv16x_accel_get_config(dev, chan, attr, val);
646650
case SENSOR_CHAN_GYRO_XYZ:
647651
return lsm6dsv16x_gyro_get_config(dev, chan, attr, val);
652+
#ifdef CONFIG_LSM6DSV16X_STREAM
653+
case SENSOR_CHAN_GBIAS_XYZ:
654+
return lsm6dsv16x_gbias_get_config(dev, chan, attr, val);
655+
#endif /* CONFIG_LSM6DSV16X_STREAM */
648656
default:
649657
LOG_WRN("attr_get() not supported on this channel.");
650658
return -ENOTSUP;

‎drivers/sensor/st/lsm6dsv16x/lsm6dsv16x.h

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -169,6 +169,9 @@ struct lsm6dsv16x_data {
169169
uint8_t bus_type : 2; /* I2C is 0, SPI is 1, I3C is 2 */
170170
uint8_t sflp_batch_odr : 3;
171171
uint8_t reserved : 1;
172+
int32_t gbias_x_udps;
173+
int32_t gbias_y_udps;
174+
int32_t gbias_z_udps;
172175
#endif
173176

174177
#ifdef CONFIG_LSM6DSV16X_TRIGGER
@@ -211,6 +214,9 @@ static inline uint8_t lsm6dsv16x_bus_reg(struct lsm6dsv16x_data *data, uint8_t x
211214
#define LSM6DSV16X_FIFO_SIZE(x) (x * LSM6DSV16X_FIFO_ITEM_LEN)
212215
#endif
213216

217+
int lsm6dsv16x_accel_set_odr_raw(const struct device *dev, uint8_t odr);
218+
int lsm6dsv16x_gyro_set_odr_raw(const struct device *dev, uint8_t odr);
219+
214220
#if defined(CONFIG_LSM6DSV16X_SENSORHUB)
215221
int lsm6dsv16x_shub_init(const struct device *dev);
216222
int lsm6dsv16x_shub_fetch_external_devs(const struct device *dev);

‎drivers/sensor/st/lsm6dsv16x/lsm6dsv16x_rtio.h

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,11 @@
1212
#include <zephyr/device.h>
1313
#include <zephyr/rtio/rtio.h>
1414

15+
int lsm6dsv16x_gbias_config(const struct device *dev, enum sensor_channel chan,
16+
enum sensor_attribute attr,
17+
const struct sensor_value *val);
18+
int lsm6dsv16x_gbias_get_config(const struct device *dev, enum sensor_channel chan,
19+
enum sensor_attribute attr, struct sensor_value *val);
1520
void lsm6dsv16x_submit(const struct device *sensor, struct rtio_iodev_sqe *iodev_sqe);
1621

1722
void lsm6dsv16x_submit_stream(const struct device *sensor, struct rtio_iodev_sqe *iodev_sqe);

‎drivers/sensor/st/lsm6dsv16x/lsm6dsv16x_rtio_stream.c

Lines changed: 86 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,45 @@ LOG_MODULE_DECLARE(LSM6DSV16X_RTIO);
1919
#define FIFO_TH 1
2020
#define FIFO_FULL 2
2121

22+
int lsm6dsv16x_gbias_config(const struct device *dev, enum sensor_channel chan,
23+
enum sensor_attribute attr,
24+
const struct sensor_value *val)
25+
{
26+
struct lsm6dsv16x_data *lsm6dsv16x = dev->data;
27+
28+
switch (attr) {
29+
case SENSOR_ATTR_OFFSET:
30+
lsm6dsv16x->gbias_x_udps = 10 * sensor_rad_to_10udegrees(&val[0]);
31+
lsm6dsv16x->gbias_y_udps = 10 * sensor_rad_to_10udegrees(&val[1]);
32+
lsm6dsv16x->gbias_z_udps = 10 * sensor_rad_to_10udegrees(&val[2]);
33+
break;
34+
default:
35+
LOG_DBG("Accel attribute not supported.");
36+
return -ENOTSUP;
37+
}
38+
39+
return 0;
40+
}
41+
42+
int lsm6dsv16x_gbias_get_config(const struct device *dev, enum sensor_channel chan,
43+
enum sensor_attribute attr, struct sensor_value *val)
44+
{
45+
struct lsm6dsv16x_data *lsm6dsv16x = dev->data;
46+
47+
switch (attr) {
48+
case SENSOR_ATTR_OFFSET:
49+
sensor_10udegrees_to_rad(lsm6dsv16x->gbias_x_udps / 10, &val[0]);
50+
sensor_10udegrees_to_rad(lsm6dsv16x->gbias_y_udps / 10, &val[1]);
51+
sensor_10udegrees_to_rad(lsm6dsv16x->gbias_z_udps / 10, &val[2]);
52+
break;
53+
default:
54+
LOG_DBG("Accel attribute not supported.");
55+
return -ENOTSUP;
56+
}
57+
58+
return 0;
59+
}
60+
2261
static void lsm6dsv16x_config_fifo(const struct device *dev, uint8_t fifo_irq)
2362
{
2463
struct lsm6dsv16x_data *lsm6dsv16x = dev->data;
@@ -32,6 +71,7 @@ static void lsm6dsv16x_config_fifo(const struct device *dev, uint8_t fifo_irq)
3271
lsm6dsv16x_fifo_mode_t fifo_mode = LSM6DSV16X_BYPASS_MODE;
3372
lsm6dsv16x_sflp_data_rate_t sflp_odr = LSM6DSV16X_SFLP_120Hz;
3473
lsm6dsv16x_fifo_sflp_raw_t sflp_fifo = { 0 };
74+
lsm6dsv16x_sflp_gbias_t gbias;
3575

3676
/* disable FIFO as first thing */
3777
lsm6dsv16x_fifo_mode_set(ctx, LSM6DSV16X_BYPASS_MODE);
@@ -89,6 +129,52 @@ static void lsm6dsv16x_config_fifo(const struct device *dev, uint8_t fifo_irq)
89129
lsm6dsv16x_fifo_sflp_batch_set(ctx, sflp_fifo);
90130
lsm6dsv16x_sflp_game_rotation_set(ctx, PROPERTY_ENABLE);
91131

132+
/*
133+
* Temporarly set Accel and gyro odr same as sensor fusion LP in order to
134+
* make the SFLP gbias setting effective. Then restore it to saved values.
135+
*/
136+
switch (sflp_odr) {
137+
case LSM6DSV16X_DT_SFLP_ODR_AT_480Hz:
138+
lsm6dsv16x_accel_set_odr_raw(dev, LSM6DSV16X_DT_ODR_AT_480Hz);
139+
lsm6dsv16x_gyro_set_odr_raw(dev, LSM6DSV16X_DT_ODR_AT_480Hz);
140+
break;
141+
142+
case LSM6DSV16X_DT_SFLP_ODR_AT_240Hz:
143+
lsm6dsv16x_accel_set_odr_raw(dev, LSM6DSV16X_DT_ODR_AT_240Hz);
144+
lsm6dsv16x_gyro_set_odr_raw(dev, LSM6DSV16X_DT_ODR_AT_240Hz);
145+
break;
146+
147+
case LSM6DSV16X_DT_SFLP_ODR_AT_120Hz:
148+
lsm6dsv16x_accel_set_odr_raw(dev, LSM6DSV16X_DT_ODR_AT_120Hz);
149+
lsm6dsv16x_gyro_set_odr_raw(dev, LSM6DSV16X_DT_ODR_AT_120Hz);
150+
break;
151+
152+
case LSM6DSV16X_DT_SFLP_ODR_AT_60Hz:
153+
lsm6dsv16x_accel_set_odr_raw(dev, LSM6DSV16X_DT_ODR_AT_60Hz);
154+
lsm6dsv16x_gyro_set_odr_raw(dev, LSM6DSV16X_DT_ODR_AT_60Hz);
155+
break;
156+
157+
case LSM6DSV16X_DT_SFLP_ODR_AT_30Hz:
158+
lsm6dsv16x_accel_set_odr_raw(dev, LSM6DSV16X_DT_ODR_AT_30Hz);
159+
lsm6dsv16x_gyro_set_odr_raw(dev, LSM6DSV16X_DT_ODR_AT_30Hz);
160+
break;
161+
162+
case LSM6DSV16X_DT_SFLP_ODR_AT_15Hz:
163+
lsm6dsv16x_accel_set_odr_raw(dev, LSM6DSV16X_DT_ODR_AT_15Hz);
164+
lsm6dsv16x_gyro_set_odr_raw(dev, LSM6DSV16X_DT_ODR_AT_15Hz);
165+
break;
166+
}
167+
168+
/* set sflp gbias */
169+
gbias.gbias_x = (float)lsm6dsv16x->gbias_x_udps / 1000000;
170+
gbias.gbias_y = (float)lsm6dsv16x->gbias_y_udps / 1000000;
171+
gbias.gbias_z = (float)lsm6dsv16x->gbias_z_udps / 1000000;
172+
lsm6dsv16x_sflp_game_gbias_set(ctx, &gbias);
173+
174+
/* restore accel/gyro odr to saved values */
175+
lsm6dsv16x_accel_set_odr_raw(dev, lsm6dsv16x->accel_freq);
176+
lsm6dsv16x_gyro_set_odr_raw(dev, lsm6dsv16x->gyro_freq);
177+
92178
/* Set pin interrupt (fifo_th could be on or off) */
93179
if ((config->drdy_pin == 1) || (ON_I3C_BUS(config) && (!I3C_INT_PIN(config)))) {
94180
lsm6dsv16x_pin_int1_route_set(ctx, &pin_int);

0 commit comments

Comments
 (0)