sensors: drivers: lsm6dsv16x: add SFLP FIFO support
Add the Sensor Fusion Low Power (SFLP) FIFO streaming capability,
using RTIO. The decode function is now aware of parsing following FIFO
tags:
- LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG
- LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG
- LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG
To activate SFLP the user should put in DT the proper configuration.
For example, to activate a 60Hz SFLP FIFO batching rate of game rotation
vector, gravity vector and gbias components, the user should add in DT
the following:
sflp-odr = <LSM6DSV16X_DT_SFLP_ODR_AT_60Hz>;
sflp-fifo-enable = <LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION_GRAVITY_GBIAS>;
Signed-off-by: Armando Visconti <armando.visconti@st.com>
This commit is contained in:
Armando Visconti
Benjamin Cabé
parent
dc03fba586
commit
21584003bf
7 changed files with 249 additions and 2 deletions
|
|
@ -1138,6 +1138,8 @@ static int lsm6dsv16x_init(const struct device *dev)
|
|||
(.fifo_wtm = DT_INST_PROP(inst, fifo_watermark), \
|
||||
.accel_batch = DT_INST_PROP(inst, accel_fifo_batch_rate), \
|
||||
.gyro_batch = DT_INST_PROP(inst, gyro_fifo_batch_rate), \
|
||||
.sflp_odr = DT_INST_PROP(inst, sflp_odr), \
|
||||
.sflp_fifo_en = DT_INST_PROP(inst, sflp_fifo_enable), \
|
||||
.temp_batch = DT_INST_PROP(inst, temp_fifo_batch_rate),)) \
|
||||
IF_ENABLED(UTIL_OR(DT_INST_NODE_HAS_PROP(inst, int1_gpios), \
|
||||
DT_INST_NODE_HAS_PROP(inst, int2_gpios)), \
|
||||
|
|
|
|||
|
|
@ -75,6 +75,8 @@ struct lsm6dsv16x_config {
|
|||
uint8_t accel_batch : 4;
|
||||
uint8_t gyro_batch : 4;
|
||||
uint8_t temp_batch : 2;
|
||||
uint8_t sflp_odr : 3;
|
||||
uint8_t sflp_fifo_en : 3;
|
||||
#endif
|
||||
#ifdef CONFIG_LSM6DSV16X_TRIGGER
|
||||
const struct gpio_dt_spec int1_gpio;
|
||||
|
|
@ -158,7 +160,8 @@ struct lsm6dsv16x_data {
|
|||
uint8_t gyro_batch_odr : 4;
|
||||
uint8_t temp_batch_odr : 2;
|
||||
uint8_t bus_type : 2; /* I2C is 0, SPI is 1, I3C is 2 */
|
||||
uint8_t reserved : 4;
|
||||
uint8_t sflp_batch_odr : 3;
|
||||
uint8_t reserved : 1;
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_LSM6DSV16X_TRIGGER
|
||||
|
|
|
|||
|
|
@ -51,8 +51,23 @@ static const uint32_t temp_period_ns[] = {
|
|||
[LSM6DSV16X_TEMP_BATCHED_AT_60Hz] = UINT32_C(1000000000) / 60,
|
||||
};
|
||||
#endif
|
||||
|
||||
static const uint32_t sflp_period_ns[] = {
|
||||
[LSM6DSV16X_DT_SFLP_ODR_AT_15Hz] = UINT32_C(1000000000) / 15,
|
||||
[LSM6DSV16X_DT_SFLP_ODR_AT_30Hz] = UINT32_C(1000000000) / 30,
|
||||
[LSM6DSV16X_DT_SFLP_ODR_AT_60Hz] = UINT32_C(1000000000) / 60,
|
||||
[LSM6DSV16X_DT_SFLP_ODR_AT_120Hz] = UINT32_C(1000000000) / 120,
|
||||
[LSM6DSV16X_DT_SFLP_ODR_AT_240Hz] = UINT32_C(1000000000) / 240,
|
||||
[LSM6DSV16X_DT_SFLP_ODR_AT_480Hz] = UINT32_C(1000000000) / 480,
|
||||
};
|
||||
#endif /* CONFIG_LSM6DSV16X_STREAM */
|
||||
|
||||
/*
|
||||
* Expand val to q31_t according to its range; this is achieved multiplying by 2^31/2^range.
|
||||
*/
|
||||
#define Q31_SHIFT_VAL(val, range) \
|
||||
(q31_t) (round((val) * ((int64_t)1 << (31 - (range)))))
|
||||
|
||||
/*
|
||||
* Expand micro_val (a generic micro unit) to q31_t according to its range; this is achieved
|
||||
* multiplying by 2^31/2^range. Then transform it to val.
|
||||
|
|
@ -157,6 +172,7 @@ static int lsm6dsv16x_decoder_get_frame_count(const uint8_t *buffer,
|
|||
const uint8_t *buffer_end;
|
||||
uint8_t fifo_tag;
|
||||
uint8_t tot_accel_fifo_words = 0, tot_gyro_fifo_words = 0;
|
||||
uint8_t tot_sflp_gbias = 0, tot_sflp_gravity = 0, tot_sflp_game_rotation = 0;
|
||||
|
||||
#if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
|
||||
uint8_t tot_temp_fifo_words = 0;
|
||||
|
|
@ -181,6 +197,15 @@ static int lsm6dsv16x_decoder_get_frame_count(const uint8_t *buffer,
|
|||
tot_temp_fifo_words++;
|
||||
break;
|
||||
#endif
|
||||
case LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG:
|
||||
tot_sflp_gbias++;
|
||||
break;
|
||||
case LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG:
|
||||
tot_sflp_gravity++;
|
||||
break;
|
||||
case LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG:
|
||||
tot_sflp_game_rotation++;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
|
@ -208,11 +233,21 @@ static int lsm6dsv16x_decoder_get_frame_count(const uint8_t *buffer,
|
|||
*frame_count = tot_temp_fifo_words;
|
||||
break;
|
||||
#endif
|
||||
case SENSOR_CHAN_GAME_ROTATION_VECTOR:
|
||||
*frame_count = tot_sflp_game_rotation;
|
||||
break;
|
||||
case SENSOR_CHAN_GRAVITY_VECTOR:
|
||||
*frame_count = tot_sflp_gravity;
|
||||
break;
|
||||
case SENSOR_CHAN_GBIAS_XYZ:
|
||||
*frame_count = tot_sflp_gbias;
|
||||
break;
|
||||
default:
|
||||
*frame_count = 0;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -231,6 +266,7 @@ static int lsm6dsv16x_decode_fifo(const uint8_t *buffer, struct sensor_chan_spec
|
|||
#if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
|
||||
uint16_t temp_count = 0;
|
||||
#endif
|
||||
uint16_t game_rot_count = 0, gravity_count = 0, gbias_count = 0;
|
||||
int ret;
|
||||
|
||||
/* count total FIFO word for each tag */
|
||||
|
|
@ -261,6 +297,12 @@ static int lsm6dsv16x_decode_fifo(const uint8_t *buffer, struct sensor_chan_spec
|
|||
edata->header.timestamp -
|
||||
(tot_chan_fifo_words - 1) * temp_period_ns[edata->temp_batch_odr];
|
||||
#endif
|
||||
} else if (chan_spec.chan_type == SENSOR_CHAN_GRAVITY_VECTOR ||
|
||||
chan_spec.chan_type == SENSOR_CHAN_GAME_ROTATION_VECTOR ||
|
||||
chan_spec.chan_type == SENSOR_CHAN_GBIAS_XYZ) {
|
||||
((struct sensor_data_header *)data_out)->base_timestamp_ns =
|
||||
edata->header.timestamp -
|
||||
(tot_chan_fifo_words - 1) * sflp_period_ns[edata->sflp_batch_odr];
|
||||
}
|
||||
|
||||
while (count < max_count && buffer < buffer_end) {
|
||||
|
|
@ -365,6 +407,126 @@ static int lsm6dsv16x_decode_fifo(const uint8_t *buffer, struct sensor_chan_spec
|
|||
break;
|
||||
}
|
||||
#endif
|
||||
case LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: {
|
||||
struct sensor_game_rotation_vector_data *out = data_out;
|
||||
union { float32_t f; uint32_t i; } x, y, z;
|
||||
float32_t w, sumsq;
|
||||
|
||||
game_rot_count++;
|
||||
if ((uintptr_t)buffer < *fit) {
|
||||
/* This frame was already decoded, move on to the next frame */
|
||||
buffer = frame_end;
|
||||
continue;
|
||||
}
|
||||
|
||||
if (chan_spec.chan_type != SENSOR_CHAN_GAME_ROTATION_VECTOR) {
|
||||
buffer = frame_end;
|
||||
continue;
|
||||
}
|
||||
|
||||
out->readings[count].timestamp_delta =
|
||||
(game_rot_count - 1) * sflp_period_ns[edata->sflp_batch_odr];
|
||||
|
||||
x.i = lsm6dsv16x_from_f16_to_f32(buffer[1] | (buffer[2] << 8));
|
||||
y.i = lsm6dsv16x_from_f16_to_f32(buffer[3] | (buffer[4] << 8));
|
||||
z.i = lsm6dsv16x_from_f16_to_f32(buffer[5] | (buffer[6] << 8));
|
||||
|
||||
sumsq = powf(x.f, 2) + powf(y.f, 2) + powf(z.f, 2);
|
||||
|
||||
/*
|
||||
* Theoretically sumsq should never be greater than 1, but due to
|
||||
* lack of precision it might happen. So, add a software correction
|
||||
* which consists in normalizing the (x, y, z) vector.
|
||||
*/
|
||||
if (sumsq > 1.0f) {
|
||||
float n = sqrtf(sumsq);
|
||||
|
||||
x.f /= n;
|
||||
y.f /= n;
|
||||
z.f /= n;
|
||||
sumsq = 1.0f;
|
||||
}
|
||||
|
||||
/* unity vector quaternions */
|
||||
w = sqrtf(1.0f - sumsq);
|
||||
|
||||
/*
|
||||
* Quaternions are numbers between -1 and 1. So let's select the signed
|
||||
* Q0.31 format (m = 0, n (fractional bits) == 31)
|
||||
*/
|
||||
out->shift = 0;
|
||||
|
||||
out->readings[count].x = Q31_SHIFT_VAL(x.f, out->shift);
|
||||
out->readings[count].y = Q31_SHIFT_VAL(y.f, out->shift);
|
||||
out->readings[count].z = Q31_SHIFT_VAL(z.f, out->shift);
|
||||
out->readings[count].w = Q31_SHIFT_VAL(w, out->shift);
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG: {
|
||||
struct sensor_three_axis_data *out = data_out;
|
||||
int16_t x, y, z;
|
||||
const int32_t scale = gyro_scaler[LSM6DSV16X_DT_FS_125DPS];
|
||||
|
||||
gbias_count++;
|
||||
if ((uintptr_t)buffer < *fit) {
|
||||
/* This frame was already decoded, move on to the next frame */
|
||||
buffer = frame_end;
|
||||
continue;
|
||||
}
|
||||
|
||||
if (chan_spec.chan_type != SENSOR_CHAN_GBIAS_XYZ) {
|
||||
buffer = frame_end;
|
||||
continue;
|
||||
}
|
||||
|
||||
out->readings[count].timestamp_delta =
|
||||
(gbias_count - 1) * sflp_period_ns[edata->sflp_batch_odr];
|
||||
|
||||
x = buffer[1] | (buffer[2] << 8);
|
||||
y = buffer[3] | (buffer[4] << 8);
|
||||
z = buffer[5] | (buffer[6] << 8);
|
||||
|
||||
out->shift = gyro_range[LSM6DSV16X_DT_FS_125DPS];
|
||||
|
||||
out->readings[count].x = Q31_SHIFT_MICROVAL(scale * x, out->shift);
|
||||
out->readings[count].y = Q31_SHIFT_MICROVAL(scale * y, out->shift);
|
||||
out->readings[count].z = Q31_SHIFT_MICROVAL(scale * z, out->shift);
|
||||
break;
|
||||
}
|
||||
|
||||
case LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG: {
|
||||
struct sensor_three_axis_data *out = data_out;
|
||||
float32_t x, y, z;
|
||||
|
||||
gravity_count++;
|
||||
if ((uintptr_t)buffer < *fit) {
|
||||
/* This frame was already decoded, move on to the next frame */
|
||||
buffer = frame_end;
|
||||
continue;
|
||||
}
|
||||
|
||||
if (chan_spec.chan_type != SENSOR_CHAN_GRAVITY_VECTOR) {
|
||||
buffer = frame_end;
|
||||
continue;
|
||||
}
|
||||
|
||||
out->readings[count].timestamp_delta =
|
||||
(gravity_count - 1) * sflp_period_ns[edata->sflp_batch_odr];
|
||||
|
||||
x = lsm6dsv16x_from_sflp_to_mg(buffer[1] | (buffer[2] << 8));
|
||||
y = lsm6dsv16x_from_sflp_to_mg(buffer[3] | (buffer[4] << 8));
|
||||
z = lsm6dsv16x_from_sflp_to_mg(buffer[5] | (buffer[6] << 8));
|
||||
|
||||
out->shift = 12;
|
||||
|
||||
out->readings[count].x = Q31_SHIFT_VAL(x, out->shift);
|
||||
out->readings[count].y = Q31_SHIFT_VAL(y, out->shift);
|
||||
out->readings[count].z = Q31_SHIFT_VAL(z, out->shift);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
/* skip unhandled FIFO tag */
|
||||
buffer = frame_end;
|
||||
|
|
|
|||
|
|
@ -30,7 +30,8 @@ struct lsm6dsv16x_fifo_data {
|
|||
uint16_t gyro_batch_odr: 4;
|
||||
uint16_t accel_batch_odr: 4;
|
||||
uint16_t temp_batch_odr: 4;
|
||||
uint16_t reserved_2: 4;
|
||||
uint16_t sflp_batch_odr: 3;
|
||||
uint16_t reserved_2: 1;
|
||||
} __attribute__((__packed__));
|
||||
|
||||
struct lsm6dsv16x_rtio_data {
|
||||
|
|
|
|||
|
|
@ -31,6 +31,8 @@ static void lsm6dsv16x_config_fifo(const struct device *dev, uint8_t fifo_irq)
|
|||
lsm6dsv16x_fifo_gy_batch_t gy_batch = LSM6DSV16X_DT_GY_NOT_BATCHED;
|
||||
lsm6dsv16x_fifo_temp_batch_t temp_batch = LSM6DSV16X_DT_TEMP_NOT_BATCHED;
|
||||
lsm6dsv16x_fifo_mode_t fifo_mode = LSM6DSV16X_BYPASS_MODE;
|
||||
lsm6dsv16x_sflp_data_rate_t sflp_odr = LSM6DSV16X_SFLP_120Hz;
|
||||
lsm6dsv16x_fifo_sflp_raw_t sflp_fifo = { 0 };
|
||||
|
||||
/* disable FIFO as first thing */
|
||||
lsm6dsv16x_fifo_mode_set(ctx, LSM6DSV16X_BYPASS_MODE);
|
||||
|
|
@ -48,6 +50,20 @@ static void lsm6dsv16x_config_fifo(const struct device *dev, uint8_t fifo_irq)
|
|||
|
||||
fifo_mode = LSM6DSV16X_STREAM_MODE;
|
||||
fifo_wtm = config->fifo_wtm;
|
||||
|
||||
if (config->sflp_fifo_en & LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION) {
|
||||
sflp_fifo.game_rotation = 1;
|
||||
}
|
||||
|
||||
if (config->sflp_fifo_en & LSM6DSV16X_DT_SFLP_FIFO_GRAVITY) {
|
||||
sflp_fifo.gravity = 1;
|
||||
}
|
||||
|
||||
if (config->sflp_fifo_en & LSM6DSV16X_DT_SFLP_FIFO_GBIAS) {
|
||||
sflp_fifo.gbias = 1;
|
||||
}
|
||||
|
||||
sflp_odr = config->sflp_odr;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -69,6 +85,11 @@ static void lsm6dsv16x_config_fifo(const struct device *dev, uint8_t fifo_irq)
|
|||
lsm6dsv16x->temp_batch_odr = temp_batch;
|
||||
#endif
|
||||
|
||||
lsm6dsv16x_sflp_data_rate_set(ctx, sflp_odr);
|
||||
lsm6dsv16x->sflp_batch_odr = sflp_odr;
|
||||
lsm6dsv16x_fifo_sflp_batch_set(ctx, sflp_fifo);
|
||||
lsm6dsv16x_sflp_game_rotation_set(ctx, PROPERTY_ENABLE);
|
||||
|
||||
/* Set pin interrupt (fifo_th could be on or off) */
|
||||
if ((config->drdy_pin == 1) || (ON_I3C_BUS(config) && (!I3C_INT_PIN(config)))) {
|
||||
lsm6dsv16x_pin_int1_route_set(ctx, &pin_int);
|
||||
|
|
@ -306,6 +327,7 @@ static void lsm6dsv16x_read_fifo_cb(struct rtio *r, const struct rtio_sqe *sqe,
|
|||
#if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
|
||||
.temp_batch_odr = lsm6dsv16x->temp_batch_odr,
|
||||
#endif
|
||||
.sflp_batch_odr = lsm6dsv16x->sflp_batch_odr,
|
||||
};
|
||||
|
||||
memcpy(buf, &hdr, sizeof(hdr));
|
||||
|
|
|
|||
|
|
@ -258,3 +258,41 @@ properties:
|
|||
- 0x3 # LSM6DSV16X_DT_TEMP_BATCHED_AT_60Hz
|
||||
|
||||
enum: [0x00, 0x01, 0x02, 0x03]
|
||||
|
||||
sflp-odr:
|
||||
type: int
|
||||
default: 0x3
|
||||
description: |
|
||||
Specify the Sensor Fusion Low Power output data rate expressed in samples per second (Hz).
|
||||
The values are taken in accordance to lsm6dsv16x_sflp_data_rate_t enumerative in hal/st
|
||||
module.
|
||||
Default is power-up configuration.
|
||||
|
||||
- 0x0 # LSM6DSV16X_DT_SFLP_ODR_AT_15Hz
|
||||
- 0x1 # LSM6DSV16X_DT_SFLP_ODR_AT_30Hz
|
||||
- 0x2 # LSM6DSV16X_DT_SFLP_ODR_AT_60Hz
|
||||
- 0x3 # LSM6DSV16X_DT_SFLP_ODR_AT_120Hz
|
||||
- 0x4 # LSM6DSV16X_DT_SFLP_ODR_AT_240Hz
|
||||
- 0x5 # LSM6DSV16X_DT_SFLP_ODR_AT_480Hz
|
||||
|
||||
enum: [0x00, 0x01, 0x02, 0x03, 0x04, 0x05]
|
||||
|
||||
sflp-fifo-enable:
|
||||
type: int
|
||||
default: 0x0
|
||||
description: |
|
||||
Specify what Sensor Fusion Low Power component has to be batched in FIFO.
|
||||
The values are taken in accordance to lsm6dsv16x_fifo_sflp_raw_t enumerative in hal/st
|
||||
module.
|
||||
Default is power-up configuration.
|
||||
|
||||
- 0x0 # LSM6DSV16X_DT_SFLP_FIFO_OFF
|
||||
- 0x1 # LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION
|
||||
- 0x2 # LSM6DSV16X_DT_SFLP_FIFO_GRAVITY
|
||||
- 0x3 # LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION_GRAVITY
|
||||
- 0x4 # LSM6DSV16X_DT_SFLP_FIFO_GBIAS
|
||||
- 0x5 # LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION_GBIAS
|
||||
- 0x6 # LSM6DSV16X_DT_SFLP_FIFO_GRAVITY_GBIAS
|
||||
- 0x7 # LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION_GRAVITY_GBIAS
|
||||
|
||||
enum: [0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07]
|
||||
|
|
|
|||
|
|
@ -91,4 +91,23 @@
|
|||
#define LSM6DSV16X_DT_TEMP_BATCHED_AT_15Hz 0x2
|
||||
#define LSM6DSV16X_DT_TEMP_BATCHED_AT_60Hz 0x3
|
||||
|
||||
/* Sensor Fusion Low Power Data rates */
|
||||
#define LSM6DSV16X_DT_SFLP_ODR_AT_15Hz 0x0
|
||||
#define LSM6DSV16X_DT_SFLP_ODR_AT_30Hz 0x1
|
||||
#define LSM6DSV16X_DT_SFLP_ODR_AT_60Hz 0x2
|
||||
#define LSM6DSV16X_DT_SFLP_ODR_AT_120Hz 0x3
|
||||
#define LSM6DSV16X_DT_SFLP_ODR_AT_240Hz 0x4
|
||||
#define LSM6DSV16X_DT_SFLP_ODR_AT_480Hz 0x5
|
||||
|
||||
/* Sensor Fusion Low Power FIFO enable defs */
|
||||
#define LSM6DSV16X_DT_SFLP_FIFO_OFF 0x0
|
||||
#define LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION 0x1
|
||||
#define LSM6DSV16X_DT_SFLP_FIFO_GRAVITY 0x2
|
||||
#define LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION_GRAVITY 0x3
|
||||
#define LSM6DSV16X_DT_SFLP_FIFO_GBIAS 0x4
|
||||
#define LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION_GBIAS 0x5
|
||||
#define LSM6DSV16X_DT_SFLP_FIFO_GRAVITY_GBIAS 0x6
|
||||
#define LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION_GRAVITY_GBIAS 0x7
|
||||
|
||||
|
||||
#endif /* ZEPHYR_INCLUDE_DT_BINDINGS_ST_LSM6DSV16X_H_ */
|
||||
|
|
|
|||
Loading…
Reference in a new issue