1 /* ST Microelectronics LSM6DSV16X 6-axis IMU sensor driver
2 *
3 * Copyright (c) 2023 Google LLC
4 * Copyright (c) 2024 STMicroelectronics
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #include "lsm6dsv16x.h"
10 #include "lsm6dsv16x_decoder.h"
11 #include <zephyr/dt-bindings/sensor/lsm6dsv16x.h>
12
13 #include <zephyr/logging/log.h>
14 LOG_MODULE_REGISTER(LSM6DSV16X_DECODER, CONFIG_SENSOR_LOG_LEVEL);
15
16 #ifdef CONFIG_LSM6DSV16X_STREAM
17 static const uint32_t accel_period_ns[] = {
18 [LSM6DSV16X_XL_BATCHED_AT_1Hz875] = UINT32_C(1000000000000) / 1875,
19 [LSM6DSV16X_XL_BATCHED_AT_7Hz5] = UINT32_C(1000000000000) / 7500,
20 [LSM6DSV16X_XL_BATCHED_AT_15Hz] = UINT32_C(1000000000) / 15,
21 [LSM6DSV16X_XL_BATCHED_AT_30Hz] = UINT32_C(1000000000) / 30,
22 [LSM6DSV16X_XL_BATCHED_AT_60Hz] = UINT32_C(1000000000) / 60,
23 [LSM6DSV16X_XL_BATCHED_AT_120Hz] = UINT32_C(1000000000) / 120,
24 [LSM6DSV16X_XL_BATCHED_AT_240Hz] = UINT32_C(1000000000) / 240,
25 [LSM6DSV16X_XL_BATCHED_AT_480Hz] = UINT32_C(1000000000) / 480,
26 [LSM6DSV16X_XL_BATCHED_AT_960Hz] = UINT32_C(1000000000) / 960,
27 [LSM6DSV16X_XL_BATCHED_AT_1920Hz] = UINT32_C(1000000000) / 1920,
28 [LSM6DSV16X_XL_BATCHED_AT_3840Hz] = UINT32_C(1000000000) / 3840,
29 [LSM6DSV16X_XL_BATCHED_AT_7680Hz] = UINT32_C(1000000000) / 7680,
30 };
31
32 static const uint32_t gyro_period_ns[] = {
33 [LSM6DSV16X_GY_BATCHED_AT_1Hz875] = UINT32_C(1000000000000) / 1875,
34 [LSM6DSV16X_GY_BATCHED_AT_7Hz5] = UINT32_C(1000000000000) / 7500,
35 [LSM6DSV16X_GY_BATCHED_AT_15Hz] = UINT32_C(1000000000) / 15,
36 [LSM6DSV16X_GY_BATCHED_AT_30Hz] = UINT32_C(1000000000) / 30,
37 [LSM6DSV16X_GY_BATCHED_AT_60Hz] = UINT32_C(1000000000) / 60,
38 [LSM6DSV16X_GY_BATCHED_AT_120Hz] = UINT32_C(1000000000) / 120,
39 [LSM6DSV16X_GY_BATCHED_AT_240Hz] = UINT32_C(1000000000) / 240,
40 [LSM6DSV16X_GY_BATCHED_AT_480Hz] = UINT32_C(1000000000) / 480,
41 [LSM6DSV16X_GY_BATCHED_AT_960Hz] = UINT32_C(1000000000) / 960,
42 [LSM6DSV16X_GY_BATCHED_AT_1920Hz] = UINT32_C(1000000000) / 1920,
43 [LSM6DSV16X_GY_BATCHED_AT_3840Hz] = UINT32_C(1000000000) / 3840,
44 [LSM6DSV16X_GY_BATCHED_AT_7680Hz] = UINT32_C(1000000000) / 7680,
45 };
46
47 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
48 static const uint32_t temp_period_ns[] = {
49 [LSM6DSV16X_TEMP_BATCHED_AT_1Hz875] = UINT32_C(1000000000000) / 1875,
50 [LSM6DSV16X_TEMP_BATCHED_AT_15Hz] = UINT32_C(1000000000) / 15,
51 [LSM6DSV16X_TEMP_BATCHED_AT_60Hz] = UINT32_C(1000000000) / 60,
52 };
53 #endif
54
55 static const uint32_t sflp_period_ns[] = {
56 [LSM6DSV16X_DT_SFLP_ODR_AT_15Hz] = UINT32_C(1000000000) / 15,
57 [LSM6DSV16X_DT_SFLP_ODR_AT_30Hz] = UINT32_C(1000000000) / 30,
58 [LSM6DSV16X_DT_SFLP_ODR_AT_60Hz] = UINT32_C(1000000000) / 60,
59 [LSM6DSV16X_DT_SFLP_ODR_AT_120Hz] = UINT32_C(1000000000) / 120,
60 [LSM6DSV16X_DT_SFLP_ODR_AT_240Hz] = UINT32_C(1000000000) / 240,
61 [LSM6DSV16X_DT_SFLP_ODR_AT_480Hz] = UINT32_C(1000000000) / 480,
62 };
63 #endif /* CONFIG_LSM6DSV16X_STREAM */
64
65 /*
66 * Expand val to q31_t according to its range; this is achieved multiplying by 2^31/2^range.
67 */
68 #define Q31_SHIFT_VAL(val, range) \
69 (q31_t) (round((val) * ((int64_t)1 << (31 - (range)))))
70
71 /*
72 * Expand micro_val (a generic micro unit) to q31_t according to its range; this is achieved
73 * multiplying by 2^31/2^range. Then transform it to val.
74 */
75 #define Q31_SHIFT_MICROVAL(micro_val, range) \
76 (q31_t) ((int64_t)(micro_val) * ((int64_t)1 << (31 - (range))) / 1000000LL)
77
78 /* bit range for Accelerometer for a given fs */
79 static const int8_t accel_range[] = {
80 [LSM6DSV16X_DT_FS_2G] = 5,
81 [LSM6DSV16X_DT_FS_4G] = 6,
82 [LSM6DSV16X_DT_FS_8G] = 7,
83 [LSM6DSV16X_DT_FS_16G] = 8,
84 };
85
86 /* bit range for Gyroscope for a given fs */
87 static const int8_t gyro_range[] = {
88 [LSM6DSV16X_DT_FS_125DPS] = 2,
89 [LSM6DSV16X_DT_FS_250DPS] = 3,
90 [LSM6DSV16X_DT_FS_500DPS] = 4,
91 [LSM6DSV16X_DT_FS_1000DPS] = 5,
92 [LSM6DSV16X_DT_FS_2000DPS] = 6,
93 [LSM6DSV16X_DT_FS_4000DPS] = 7,
94 };
95
96 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
97 /* bit range for Temperature sensor */
98 static const int8_t temp_range = 9;
99
100 /* transform temperature LSB into micro-Celsius */
101 #define SENSOR_TEMP_UCELSIUS(t_lsb) \
102 (int64_t) (25000000LL + (((int64_t)(t_lsb) * 1000000LL) / 256LL))
103
104 #endif
105
106 /* Calculate scaling factor to transform micro-g/LSB unit into micro-ms2/LSB */
107 #define SENSOR_SCALE_UG_TO_UMS2(ug_lsb) \
108 (int32_t)((ug_lsb) * SENSOR_G / 1000000LL)
109
110 /*
111 * Accelerometer scaling factors table (indexed by full scale)
112 * GAIN_UNIT_XL is expressed in ug/LSB.
113 */
114 static const int32_t accel_scaler[] = {
115 [LSM6DSV16X_DT_FS_2G] = SENSOR_SCALE_UG_TO_UMS2(GAIN_UNIT_XL),
116 [LSM6DSV16X_DT_FS_4G] = SENSOR_SCALE_UG_TO_UMS2(2 * GAIN_UNIT_XL),
117 [LSM6DSV16X_DT_FS_8G] = SENSOR_SCALE_UG_TO_UMS2(4 * GAIN_UNIT_XL),
118 [LSM6DSV16X_DT_FS_16G] = SENSOR_SCALE_UG_TO_UMS2(8 * GAIN_UNIT_XL),
119 };
120
121 /* Calculate scaling factor to transform micro-dps/LSB unit into micro-rads/LSB */
122 #define SENSOR_SCALE_UDPS_TO_URADS(udps_lsb) \
123 (int32_t)(((udps_lsb) * SENSOR_PI / 180LL) / 1000000LL)
124
125 /*
126 * Accelerometer scaling factors table (indexed by full scale)
127 * GAIN_UNIT_G is expressed in udps/LSB.
128 */
129 static const int32_t gyro_scaler[] = {
130 [LSM6DSV16X_DT_FS_125DPS] = SENSOR_SCALE_UDPS_TO_URADS(GAIN_UNIT_G),
131 [LSM6DSV16X_DT_FS_250DPS] = SENSOR_SCALE_UDPS_TO_URADS(2 * GAIN_UNIT_G),
132 [LSM6DSV16X_DT_FS_500DPS] = SENSOR_SCALE_UDPS_TO_URADS(4 * GAIN_UNIT_G),
133 [LSM6DSV16X_DT_FS_1000DPS] = SENSOR_SCALE_UDPS_TO_URADS(8 * GAIN_UNIT_G),
134 [LSM6DSV16X_DT_FS_2000DPS] = SENSOR_SCALE_UDPS_TO_URADS(16 * GAIN_UNIT_G),
135 [LSM6DSV16X_DT_FS_4000DPS] = SENSOR_SCALE_UDPS_TO_URADS(32 * GAIN_UNIT_G),
136 };
137
lsm6dsv16x_decoder_get_frame_count(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint16_t * frame_count)138 static int lsm6dsv16x_decoder_get_frame_count(const uint8_t *buffer,
139 struct sensor_chan_spec chan_spec,
140 uint16_t *frame_count)
141 {
142 struct lsm6dsv16x_fifo_data *data = (struct lsm6dsv16x_fifo_data *)buffer;
143 const struct lsm6dsv16x_decoder_header *header = &data->header;
144
145 if (chan_spec.chan_idx != 0) {
146 return -ENOTSUP;
147 }
148
149 if (!header->is_fifo) {
150 switch (chan_spec.chan_type) {
151 case SENSOR_CHAN_ACCEL_X:
152 case SENSOR_CHAN_ACCEL_Y:
153 case SENSOR_CHAN_ACCEL_Z:
154 case SENSOR_CHAN_ACCEL_XYZ:
155 case SENSOR_CHAN_GYRO_X:
156 case SENSOR_CHAN_GYRO_Y:
157 case SENSOR_CHAN_GYRO_Z:
158 case SENSOR_CHAN_GYRO_XYZ:
159 case SENSOR_CHAN_DIE_TEMP:
160 *frame_count = 1;
161 return 0;
162 default:
163 *frame_count = 0;
164 return -ENOTSUP;
165 }
166
167 return 0;
168 }
169
170 #ifdef CONFIG_LSM6DSV16X_STREAM
171 const struct lsm6dsv16x_fifo_data *edata = (const struct lsm6dsv16x_fifo_data *)buffer;
172 const uint8_t *buffer_end;
173 uint8_t fifo_tag;
174 uint8_t tot_accel_fifo_words = 0, tot_gyro_fifo_words = 0;
175 uint8_t tot_sflp_gbias = 0, tot_sflp_gravity = 0, tot_sflp_game_rotation = 0;
176
177 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
178 uint8_t tot_temp_fifo_words = 0;
179 #endif
180
181 buffer += sizeof(struct lsm6dsv16x_fifo_data);
182 buffer_end = buffer + LSM6DSV16X_FIFO_SIZE(edata->fifo_count);
183
184 /* count total FIFO word for each tag */
185 while (buffer < buffer_end) {
186 fifo_tag = (buffer[0] >> 3);
187
188 switch (fifo_tag) {
189 case LSM6DSV16X_XL_NC_TAG:
190 tot_accel_fifo_words++;
191 break;
192 case LSM6DSV16X_GY_NC_TAG:
193 tot_gyro_fifo_words++;
194 break;
195 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
196 case LSM6DSV16X_TEMPERATURE_TAG:
197 tot_temp_fifo_words++;
198 break;
199 #endif
200 case LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG:
201 tot_sflp_gbias++;
202 break;
203 case LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG:
204 tot_sflp_gravity++;
205 break;
206 case LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG:
207 tot_sflp_game_rotation++;
208 break;
209 default:
210 break;
211 }
212
213 buffer += LSM6DSV16X_FIFO_ITEM_LEN;
214 }
215
216 switch (chan_spec.chan_type) {
217 case SENSOR_CHAN_ACCEL_X:
218 case SENSOR_CHAN_ACCEL_Y:
219 case SENSOR_CHAN_ACCEL_Z:
220 case SENSOR_CHAN_ACCEL_XYZ:
221 *frame_count = tot_accel_fifo_words;
222 break;
223
224 case SENSOR_CHAN_GYRO_X:
225 case SENSOR_CHAN_GYRO_Y:
226 case SENSOR_CHAN_GYRO_Z:
227 case SENSOR_CHAN_GYRO_XYZ:
228 *frame_count = tot_gyro_fifo_words;
229 break;
230
231 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
232 case SENSOR_CHAN_DIE_TEMP:
233 *frame_count = tot_temp_fifo_words;
234 break;
235 #endif
236 case SENSOR_CHAN_GAME_ROTATION_VECTOR:
237 *frame_count = tot_sflp_game_rotation;
238 break;
239 case SENSOR_CHAN_GRAVITY_VECTOR:
240 *frame_count = tot_sflp_gravity;
241 break;
242 case SENSOR_CHAN_GBIAS_XYZ:
243 *frame_count = tot_sflp_gbias;
244 break;
245 default:
246 *frame_count = 0;
247 break;
248 }
249 #endif
250
251 return 0;
252 }
253
254 #ifdef CONFIG_LSM6DSV16X_STREAM
lsm6dsv16x_decode_fifo(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)255 static int lsm6dsv16x_decode_fifo(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
256 uint32_t *fit, uint16_t max_count, void *data_out)
257 {
258 const struct lsm6dsv16x_fifo_data *edata = (const struct lsm6dsv16x_fifo_data *)buffer;
259 const uint8_t *buffer_end;
260 const struct lsm6dsv16x_decoder_header *header = &edata->header;
261 int count = 0;
262 uint8_t fifo_tag;
263 uint16_t xl_count = 0, gy_count = 0;
264 uint16_t tot_chan_fifo_words = 0;
265
266 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
267 uint16_t temp_count = 0;
268 #endif
269 uint16_t game_rot_count = 0, gravity_count = 0, gbias_count = 0;
270 int ret;
271
272 /* count total FIFO word for each tag */
273 ret = lsm6dsv16x_decoder_get_frame_count(buffer, chan_spec, &tot_chan_fifo_words);
274 if (ret < 0) {
275 return 0;
276 }
277
278 buffer += sizeof(struct lsm6dsv16x_fifo_data);
279 buffer_end = buffer + LSM6DSV16X_FIFO_SIZE(edata->fifo_count);
280
281 /*
282 * Timestamp in header is set when FIFO threshold is reached, so
283 * set time baseline going back in past according to total number
284 * of FIFO word for each type.
285 */
286 if (SENSOR_CHANNEL_IS_ACCEL(chan_spec.chan_type)) {
287 ((struct sensor_data_header *)data_out)->base_timestamp_ns =
288 edata->header.timestamp -
289 (tot_chan_fifo_words - 1) * accel_period_ns[edata->accel_batch_odr];
290 } else if (SENSOR_CHANNEL_IS_GYRO(chan_spec.chan_type)) {
291 ((struct sensor_data_header *)data_out)->base_timestamp_ns =
292 edata->header.timestamp -
293 (tot_chan_fifo_words - 1) * gyro_period_ns[edata->gyro_batch_odr];
294 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
295 } else if (chan_spec.chan_type == SENSOR_CHAN_DIE_TEMP) {
296 ((struct sensor_data_header *)data_out)->base_timestamp_ns =
297 edata->header.timestamp -
298 (tot_chan_fifo_words - 1) * temp_period_ns[edata->temp_batch_odr];
299 #endif
300 } else if (chan_spec.chan_type == SENSOR_CHAN_GRAVITY_VECTOR ||
301 chan_spec.chan_type == SENSOR_CHAN_GAME_ROTATION_VECTOR ||
302 chan_spec.chan_type == SENSOR_CHAN_GBIAS_XYZ) {
303 ((struct sensor_data_header *)data_out)->base_timestamp_ns =
304 edata->header.timestamp -
305 (tot_chan_fifo_words - 1) * sflp_period_ns[edata->sflp_batch_odr];
306 }
307
308 while (count < max_count && buffer < buffer_end) {
309 const uint8_t *frame_end = buffer;
310 uint8_t skip_frame;
311
312 skip_frame = 0;
313 frame_end += LSM6DSV16X_FIFO_ITEM_LEN;
314
315 fifo_tag = (buffer[0] >> 3);
316
317 switch (fifo_tag) {
318 case LSM6DSV16X_XL_NC_TAG: {
319 struct sensor_three_axis_data *out = data_out;
320 int16_t x, y, z;
321 const int32_t scale = accel_scaler[header->accel_fs];
322
323 xl_count++;
324 if ((uintptr_t)buffer < *fit) {
325 /* This frame was already decoded, move on to the next frame */
326 buffer = frame_end;
327 continue;
328 }
329
330 if (!SENSOR_CHANNEL_IS_ACCEL(chan_spec.chan_type)) {
331 buffer = frame_end;
332 continue;
333 }
334
335 out->readings[count].timestamp_delta =
336 (xl_count - 1) * accel_period_ns[edata->accel_batch_odr];
337
338 x = *(int16_t *)&buffer[1];
339 y = *(int16_t *)&buffer[3];
340 z = *(int16_t *)&buffer[5];
341
342 out->shift = accel_range[header->accel_fs];
343
344 out->readings[count].x = Q31_SHIFT_MICROVAL(scale * x, out->shift);
345 out->readings[count].y = Q31_SHIFT_MICROVAL(scale * y, out->shift);
346 out->readings[count].z = Q31_SHIFT_MICROVAL(scale * z, out->shift);
347 break;
348 }
349 case LSM6DSV16X_GY_NC_TAG: {
350 struct sensor_three_axis_data *out = data_out;
351 int16_t x, y, z;
352 const int32_t scale = gyro_scaler[header->gyro_fs];
353
354 gy_count++;
355 if ((uintptr_t)buffer < *fit) {
356 /* This frame was already decoded, move on to the next frame */
357 buffer = frame_end;
358 continue;
359 }
360
361 if (!SENSOR_CHANNEL_IS_GYRO(chan_spec.chan_type)) {
362 buffer = frame_end;
363 continue;
364 }
365
366 out->readings[count].timestamp_delta =
367 (gy_count - 1) * gyro_period_ns[edata->gyro_batch_odr];
368
369 x = *(int16_t *)&buffer[1];
370 y = *(int16_t *)&buffer[3];
371 z = *(int16_t *)&buffer[5];
372
373 out->shift = gyro_range[header->gyro_fs];
374
375 out->readings[count].x = Q31_SHIFT_MICROVAL(scale * x, out->shift);
376 out->readings[count].y = Q31_SHIFT_MICROVAL(scale * y, out->shift);
377 out->readings[count].z = Q31_SHIFT_MICROVAL(scale * z, out->shift);
378 break;
379 }
380 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
381 case LSM6DSV16X_TEMPERATURE_TAG: {
382 struct sensor_q31_data *out = data_out;
383 int16_t t;
384 int64_t t_uC;
385
386 temp_count++;
387 if ((uintptr_t)buffer < *fit) {
388 /* This frame was already decoded, move on to the next frame */
389 buffer = frame_end;
390 continue;
391 }
392
393 if (chan_spec.chan_type != SENSOR_CHAN_DIE_TEMP) {
394 buffer = frame_end;
395 continue;
396 }
397
398 out->readings[count].timestamp_delta =
399 (temp_count - 1) * temp_period_ns[edata->temp_batch_odr];
400
401 t = *(int16_t *)&buffer[1];
402 t_uC = SENSOR_TEMP_UCELSIUS(t);
403
404 out->shift = temp_range;
405
406 out->readings[count].temperature = Q31_SHIFT_MICROVAL(t_uC, out->shift);
407 break;
408 }
409 #endif
410 case LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: {
411 struct sensor_game_rotation_vector_data *out = data_out;
412 union { float32_t f; uint32_t i; } x, y, z;
413 float32_t w, sumsq;
414
415 game_rot_count++;
416 if ((uintptr_t)buffer < *fit) {
417 /* This frame was already decoded, move on to the next frame */
418 buffer = frame_end;
419 continue;
420 }
421
422 if (chan_spec.chan_type != SENSOR_CHAN_GAME_ROTATION_VECTOR) {
423 buffer = frame_end;
424 continue;
425 }
426
427 out->readings[count].timestamp_delta =
428 (game_rot_count - 1) * sflp_period_ns[edata->sflp_batch_odr];
429
430 x.i = lsm6dsv16x_from_f16_to_f32(buffer[1] | (buffer[2] << 8));
431 y.i = lsm6dsv16x_from_f16_to_f32(buffer[3] | (buffer[4] << 8));
432 z.i = lsm6dsv16x_from_f16_to_f32(buffer[5] | (buffer[6] << 8));
433
434 sumsq = powf(x.f, 2) + powf(y.f, 2) + powf(z.f, 2);
435
436 /*
437 * Theoretically sumsq should never be greater than 1, but due to
438 * lack of precision it might happen. So, add a software correction
439 * which consists in normalizing the (x, y, z) vector.
440 */
441 if (sumsq > 1.0f) {
442 float n = sqrtf(sumsq);
443
444 x.f /= n;
445 y.f /= n;
446 z.f /= n;
447 sumsq = 1.0f;
448 }
449
450 /* unity vector quaternions */
451 w = sqrtf(1.0f - sumsq);
452
453 /*
454 * Quaternions are numbers between -1 and 1. So let's select the signed
455 * Q0.31 format (m = 0, n (fractional bits) == 31)
456 */
457 out->shift = 0;
458
459 out->readings[count].x = Q31_SHIFT_VAL(x.f, out->shift);
460 out->readings[count].y = Q31_SHIFT_VAL(y.f, out->shift);
461 out->readings[count].z = Q31_SHIFT_VAL(z.f, out->shift);
462 out->readings[count].w = Q31_SHIFT_VAL(w, out->shift);
463
464 break;
465 }
466
467 case LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG: {
468 struct sensor_three_axis_data *out = data_out;
469 int16_t x, y, z;
470 const int32_t scale = gyro_scaler[LSM6DSV16X_DT_FS_125DPS];
471
472 gbias_count++;
473 if ((uintptr_t)buffer < *fit) {
474 /* This frame was already decoded, move on to the next frame */
475 buffer = frame_end;
476 continue;
477 }
478
479 if (chan_spec.chan_type != SENSOR_CHAN_GBIAS_XYZ) {
480 buffer = frame_end;
481 continue;
482 }
483
484 out->readings[count].timestamp_delta =
485 (gbias_count - 1) * sflp_period_ns[edata->sflp_batch_odr];
486
487 x = buffer[1] | (buffer[2] << 8);
488 y = buffer[3] | (buffer[4] << 8);
489 z = buffer[5] | (buffer[6] << 8);
490
491 out->shift = gyro_range[LSM6DSV16X_DT_FS_125DPS];
492
493 out->readings[count].x = Q31_SHIFT_MICROVAL(scale * x, out->shift);
494 out->readings[count].y = Q31_SHIFT_MICROVAL(scale * y, out->shift);
495 out->readings[count].z = Q31_SHIFT_MICROVAL(scale * z, out->shift);
496 break;
497 }
498
499 case LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG: {
500 struct sensor_three_axis_data *out = data_out;
501 float32_t x, y, z;
502
503 gravity_count++;
504 if ((uintptr_t)buffer < *fit) {
505 /* This frame was already decoded, move on to the next frame */
506 buffer = frame_end;
507 continue;
508 }
509
510 if (chan_spec.chan_type != SENSOR_CHAN_GRAVITY_VECTOR) {
511 buffer = frame_end;
512 continue;
513 }
514
515 out->readings[count].timestamp_delta =
516 (gravity_count - 1) * sflp_period_ns[edata->sflp_batch_odr];
517
518 x = lsm6dsv16x_from_sflp_to_mg(buffer[1] | (buffer[2] << 8));
519 y = lsm6dsv16x_from_sflp_to_mg(buffer[3] | (buffer[4] << 8));
520 z = lsm6dsv16x_from_sflp_to_mg(buffer[5] | (buffer[6] << 8));
521
522 out->shift = 12;
523
524 out->readings[count].x = Q31_SHIFT_VAL(x, out->shift);
525 out->readings[count].y = Q31_SHIFT_VAL(y, out->shift);
526 out->readings[count].z = Q31_SHIFT_VAL(z, out->shift);
527 break;
528 }
529
530 default:
531 /* skip unhandled FIFO tag */
532 buffer = frame_end;
533 LOG_DBG("unknown FIFO tag %02x", fifo_tag);
534 continue;
535 }
536
537 buffer = frame_end;
538 *fit = (uintptr_t)frame_end;
539 count++;
540 }
541
542 return count;
543 }
544 #endif /* CONFIG_LSM6DSV16X_STREAM */
545
lsm6dsv16x_decode_sample(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)546 static int lsm6dsv16x_decode_sample(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
547 uint32_t *fit, uint16_t max_count, void *data_out)
548 {
549 const struct lsm6dsv16x_rtio_data *edata = (const struct lsm6dsv16x_rtio_data *)buffer;
550 const struct lsm6dsv16x_decoder_header *header = &edata->header;
551
552 if (*fit != 0) {
553 return 0;
554 }
555 if (max_count == 0 || chan_spec.chan_idx != 0) {
556 return -EINVAL;
557 }
558
559 switch (chan_spec.chan_type) {
560 case SENSOR_CHAN_ACCEL_X:
561 case SENSOR_CHAN_ACCEL_Y:
562 case SENSOR_CHAN_ACCEL_Z:
563 case SENSOR_CHAN_ACCEL_XYZ: {
564 const int32_t scale = accel_scaler[header->accel_fs];
565
566 if (edata->has_accel == 0) {
567 return -ENODATA;
568 }
569
570 struct sensor_three_axis_data *out = data_out;
571
572 out->header.base_timestamp_ns = edata->header.timestamp;
573 out->header.reading_count = 1;
574
575 out->shift = accel_range[header->accel_fs];
576
577 out->readings[0].x = Q31_SHIFT_MICROVAL(scale * edata->acc[0], out->shift);
578 out->readings[0].y = Q31_SHIFT_MICROVAL(scale * edata->acc[1], out->shift);
579 out->readings[0].z = Q31_SHIFT_MICROVAL(scale * edata->acc[2], out->shift);
580 *fit = 1;
581 return 1;
582 }
583 case SENSOR_CHAN_GYRO_X:
584 case SENSOR_CHAN_GYRO_Y:
585 case SENSOR_CHAN_GYRO_Z:
586 case SENSOR_CHAN_GYRO_XYZ: {
587 const int32_t scale = gyro_scaler[header->gyro_fs];
588
589 if (edata->has_gyro == 0) {
590 return -ENODATA;
591 }
592
593 struct sensor_three_axis_data *out = data_out;
594
595 out->header.base_timestamp_ns = edata->header.timestamp;
596 out->header.reading_count = 1;
597
598 out->shift = gyro_range[header->gyro_fs];
599
600 out->readings[0].x = Q31_SHIFT_MICROVAL(scale * edata->gyro[0], out->shift);
601 out->readings[0].y = Q31_SHIFT_MICROVAL(scale * edata->gyro[1], out->shift);
602 out->readings[0].z = Q31_SHIFT_MICROVAL(scale * edata->gyro[2], out->shift);
603 *fit = 1;
604 return 1;
605 }
606 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
607 case SENSOR_CHAN_DIE_TEMP: {
608 int64_t t_uC;
609
610 if (edata->has_temp == 0) {
611 return -ENODATA;
612 }
613
614 struct sensor_q31_data *out = data_out;
615
616 out->header.base_timestamp_ns = edata->header.timestamp;
617 out->header.reading_count = 1;
618
619 out->shift = temp_range;
620
621 /* transform temperature LSB into micro-Celsius */
622 t_uC = SENSOR_TEMP_UCELSIUS(edata->temp);
623
624 out->readings[0].temperature = Q31_SHIFT_MICROVAL(t_uC, out->shift);
625 *fit = 1;
626 return 1;
627 }
628 #endif
629 default:
630 return -EINVAL;
631 }
632 }
633
lsm6dsv16x_decoder_decode(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)634 static int lsm6dsv16x_decoder_decode(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
635 uint32_t *fit, uint16_t max_count, void *data_out)
636 {
637 #ifdef CONFIG_LSM6DSV16X_STREAM
638 const struct lsm6dsv16x_decoder_header *header =
639 (const struct lsm6dsv16x_decoder_header *)buffer;
640
641 if (header->is_fifo) {
642 return lsm6dsv16x_decode_fifo(buffer, chan_spec, fit, max_count, data_out);
643 }
644 #endif
645
646 return lsm6dsv16x_decode_sample(buffer, chan_spec, fit, max_count, data_out);
647 }
648
lsm6dsv16x_decoder_get_size_info(struct sensor_chan_spec chan_spec,size_t * base_size,size_t * frame_size)649 static int lsm6dsv16x_decoder_get_size_info(struct sensor_chan_spec chan_spec, size_t *base_size,
650 size_t *frame_size)
651 {
652 switch (chan_spec.chan_type) {
653 case SENSOR_CHAN_ACCEL_X:
654 case SENSOR_CHAN_ACCEL_Y:
655 case SENSOR_CHAN_ACCEL_Z:
656 case SENSOR_CHAN_ACCEL_XYZ:
657 case SENSOR_CHAN_GYRO_X:
658 case SENSOR_CHAN_GYRO_Y:
659 case SENSOR_CHAN_GYRO_Z:
660 case SENSOR_CHAN_GYRO_XYZ:
661 *base_size = sizeof(struct sensor_three_axis_data);
662 *frame_size = sizeof(struct sensor_three_axis_sample_data);
663 return 0;
664 case SENSOR_CHAN_DIE_TEMP:
665 *base_size = sizeof(struct sensor_q31_data);
666 *frame_size = sizeof(struct sensor_q31_sample_data);
667 return 0;
668 default:
669 return -ENOTSUP;
670 }
671 }
672
lsm6dsv16x_decoder_has_trigger(const uint8_t * buffer,enum sensor_trigger_type trigger)673 static bool lsm6dsv16x_decoder_has_trigger(const uint8_t *buffer, enum sensor_trigger_type trigger)
674 {
675 return false;
676 }
677
678 SENSOR_DECODER_API_DT_DEFINE() = {
679 .get_frame_count = lsm6dsv16x_decoder_get_frame_count,
680 .get_size_info = lsm6dsv16x_decoder_get_size_info,
681 .decode = lsm6dsv16x_decoder_decode,
682 .has_trigger = lsm6dsv16x_decoder_has_trigger,
683 };
684
lsm6dsv16x_get_decoder(const struct device * dev,const struct sensor_decoder_api ** decoder)685 int lsm6dsv16x_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder)
686 {
687 ARG_UNUSED(dev);
688 *decoder = &SENSOR_DECODER_NAME();
689
690 return 0;
691 }
692
