From 6a3e6ac44d159fa96ee07a336ab01115a5590316 Mon Sep 17 00:00:00 2001 From: Armando Visconti Date: Tue, 26 Nov 2024 18:36:47 +0100 Subject: [PATCH] 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 = ; sflp-fifo-enable = ; Signed-off-by: Armando Visconti --- drivers/sensor/st/lsm6dsv16x/lsm6dsv16x.c | 2 + drivers/sensor/st/lsm6dsv16x/lsm6dsv16x.h | 5 +- .../sensor/st/lsm6dsv16x/lsm6dsv16x_decoder.c | 162 ++++++++++++++++++ .../sensor/st/lsm6dsv16x/lsm6dsv16x_decoder.h | 3 +- .../st/lsm6dsv16x/lsm6dsv16x_rtio_stream.c | 22 +++ dts/bindings/sensor/st,lsm6dsv16x-common.yaml | 38 ++++ .../zephyr/dt-bindings/sensor/lsm6dsv16x.h | 19 ++ 7 files changed, 249 insertions(+), 2 deletions(-) diff --git a/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x.c b/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x.c index 2cf2fd3acb7d573..2e156301c5e9a64 100644 --- a/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x.c +++ b/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x.c @@ -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)), \ diff --git a/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x.h b/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x.h index 6bfdd710aeedd48..a6e26b6954e4ded 100644 --- a/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x.h +++ b/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x.h @@ -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 diff --git a/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x_decoder.c b/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x_decoder.c index b77a86cfbeb444e..1ea21ace988b99a 100644 --- a/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x_decoder.c +++ b/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x_decoder.c @@ -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; diff --git a/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x_decoder.h b/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x_decoder.h index 1a02af51e720c17..22553c966517493 100644 --- a/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x_decoder.h +++ b/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x_decoder.h @@ -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 { diff --git a/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x_rtio_stream.c b/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x_rtio_stream.c index 055f56ca1b5365d..27635c3e910d2cb 100644 --- a/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x_rtio_stream.c +++ b/drivers/sensor/st/lsm6dsv16x/lsm6dsv16x_rtio_stream.c @@ -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)); diff --git a/dts/bindings/sensor/st,lsm6dsv16x-common.yaml b/dts/bindings/sensor/st,lsm6dsv16x-common.yaml index 8d8df5037109141..9746012e3107172 100644 --- a/dts/bindings/sensor/st,lsm6dsv16x-common.yaml +++ b/dts/bindings/sensor/st,lsm6dsv16x-common.yaml @@ -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] diff --git a/include/zephyr/dt-bindings/sensor/lsm6dsv16x.h b/include/zephyr/dt-bindings/sensor/lsm6dsv16x.h index 76caabbdb7f9d64..9cb142b2ed68bd5 100644 --- a/include/zephyr/dt-bindings/sensor/lsm6dsv16x.h +++ b/include/zephyr/dt-bindings/sensor/lsm6dsv16x.h @@ -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_ */