1 /*
2 * Copyright (c) 2023 Google LLC
3 * Copyright (c) 2025 Croxel Inc.
4 * Copyright (c) 2025 CogniPilot Foundation
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #include <zephyr/drivers/sensor_clock.h>
10
11 #include "icm45686.h"
12 #include "icm45686_reg.h"
13 #include "icm45686_decoder.h"
14
15 #include <zephyr/logging/log.h>
16 LOG_MODULE_REGISTER(ICM45686_DECODER, CONFIG_SENSOR_LOG_LEVEL);
17
18 #define DT_DRV_COMPAT invensense_icm45686
19
icm45686_get_shift(enum sensor_channel channel,int accel_fs,int gyro_fs,int8_t * shift)20 static int icm45686_get_shift(enum sensor_channel channel,
21 int accel_fs,
22 int gyro_fs,
23 int8_t *shift)
24 {
25 switch (channel) {
26 case SENSOR_CHAN_ACCEL_XYZ:
27 case SENSOR_CHAN_ACCEL_X:
28 case SENSOR_CHAN_ACCEL_Y:
29 case SENSOR_CHAN_ACCEL_Z:
30 switch (accel_fs) {
31 case ICM45686_DT_ACCEL_FS_32:
32 *shift = 9;
33 return 0;
34 case ICM45686_DT_ACCEL_FS_16:
35 *shift = 8;
36 return 0;
37 case ICM45686_DT_ACCEL_FS_8:
38 *shift = 7;
39 return 0;
40 case ICM45686_DT_ACCEL_FS_4:
41 *shift = 6;
42 return 0;
43 case ICM45686_DT_ACCEL_FS_2:
44 *shift = 5;
45 return 0;
46 default:
47 return -EINVAL;
48 }
49 case SENSOR_CHAN_GYRO_XYZ:
50 case SENSOR_CHAN_GYRO_X:
51 case SENSOR_CHAN_GYRO_Y:
52 case SENSOR_CHAN_GYRO_Z:
53 switch (gyro_fs) {
54 case ICM45686_DT_GYRO_FS_4000:
55 *shift = 12;
56 return 0;
57 case ICM45686_DT_GYRO_FS_2000:
58 *shift = 11;
59 return 0;
60 case ICM45686_DT_GYRO_FS_1000:
61 *shift = 10;
62 return 0;
63 case ICM45686_DT_GYRO_FS_500:
64 *shift = 9;
65 return 0;
66 case ICM45686_DT_GYRO_FS_250:
67 *shift = 8;
68 return 0;
69 case ICM45686_DT_GYRO_FS_125:
70 *shift = 7;
71 return 0;
72 case ICM45686_DT_GYRO_FS_62_5:
73 *shift = 6;
74 return 0;
75 case ICM45686_DT_GYRO_FS_31_25:
76 *shift = 5;
77 return 0;
78 default:
79 return -EINVAL;
80 }
81 case SENSOR_CHAN_DIE_TEMP:
82 *shift = 9;
83 return 0;
84 default:
85 return -EINVAL;
86 }
87 }
88
icm45686_convert_raw_to_q31(struct icm45686_encoded_data * edata,enum sensor_channel chan,int32_t reading,q31_t * out)89 int icm45686_convert_raw_to_q31(struct icm45686_encoded_data *edata,
90 enum sensor_channel chan,
91 int32_t reading,
92 q31_t *out)
93 {
94 int32_t whole;
95 int32_t fraction;
96 int64_t intermediate;
97 int8_t shift;
98 int rc;
99
100 rc = icm45686_get_shift(chan, edata->header.accel_fs, edata->header.gyro_fs, &shift);
101 if (rc != 0) {
102 return rc;
103 }
104
105 switch (chan) {
106 case SENSOR_CHAN_ACCEL_XYZ:
107 case SENSOR_CHAN_ACCEL_X:
108 case SENSOR_CHAN_ACCEL_Y:
109 case SENSOR_CHAN_ACCEL_Z:
110 icm45686_accel_ms(edata->header.accel_fs, reading, false, &whole, &fraction);
111 break;
112 case SENSOR_CHAN_GYRO_XYZ:
113 case SENSOR_CHAN_GYRO_X:
114 case SENSOR_CHAN_GYRO_Y:
115 case SENSOR_CHAN_GYRO_Z:
116 icm45686_gyro_rads(edata->header.gyro_fs, reading, false, &whole, &fraction);
117 break;
118 case SENSOR_CHAN_DIE_TEMP:
119 icm45686_temp_c(reading, &whole, &fraction);
120 break;
121 default:
122 return -ENOTSUP;
123 }
124 intermediate = ((int64_t)whole * INT64_C(1000000) + fraction);
125 if (shift < 0) {
126 intermediate =
127 intermediate * ((int64_t)INT32_MAX + 1) * (1 << -shift) / INT64_C(1000000);
128 } else if (shift > 0) {
129 intermediate =
130 intermediate * ((int64_t)INT32_MAX + 1) / ((1 << shift) * INT64_C(1000000));
131 }
132 *out = CLAMP(intermediate, INT32_MIN, INT32_MAX);
133
134 return 0;
135 }
136
icm45686_get_channel_position(enum sensor_channel chan)137 static int icm45686_get_channel_position(enum sensor_channel chan)
138 {
139 switch (chan) {
140 case SENSOR_CHAN_ACCEL_XYZ:
141 case SENSOR_CHAN_ACCEL_X:
142 return offsetof(struct icm45686_encoded_payload, accel.x) / sizeof(int16_t);
143 case SENSOR_CHAN_ACCEL_Y:
144 return offsetof(struct icm45686_encoded_payload, accel.y) / sizeof(int16_t);
145 case SENSOR_CHAN_ACCEL_Z:
146 return offsetof(struct icm45686_encoded_payload, accel.z) / sizeof(int16_t);
147 case SENSOR_CHAN_GYRO_XYZ:
148 case SENSOR_CHAN_GYRO_X:
149 return offsetof(struct icm45686_encoded_payload, gyro.x) / sizeof(int16_t);
150 case SENSOR_CHAN_GYRO_Y:
151 return offsetof(struct icm45686_encoded_payload, gyro.y) / sizeof(int16_t);
152 case SENSOR_CHAN_GYRO_Z:
153 return offsetof(struct icm45686_encoded_payload, gyro.z) / sizeof(int16_t);
154 case SENSOR_CHAN_DIE_TEMP:
155 return offsetof(struct icm45686_encoded_payload, temp) / sizeof(int16_t);
156 default:
157 return 0;
158 }
159 }
160
icm45686_encode_channel(enum sensor_channel chan)161 static uint8_t icm45686_encode_channel(enum sensor_channel chan)
162 {
163 uint8_t encode_bmask = 0;
164
165 switch (chan) {
166 case SENSOR_CHAN_ACCEL_X:
167 case SENSOR_CHAN_ACCEL_Y:
168 case SENSOR_CHAN_ACCEL_Z:
169 case SENSOR_CHAN_GYRO_X:
170 case SENSOR_CHAN_GYRO_Y:
171 case SENSOR_CHAN_GYRO_Z:
172 case SENSOR_CHAN_DIE_TEMP:
173 encode_bmask = BIT(icm45686_get_channel_position(chan));
174 break;
175 case SENSOR_CHAN_ACCEL_XYZ:
176 encode_bmask = BIT(icm45686_get_channel_position(SENSOR_CHAN_ACCEL_X)) |
177 BIT(icm45686_get_channel_position(SENSOR_CHAN_ACCEL_Y)) |
178 BIT(icm45686_get_channel_position(SENSOR_CHAN_ACCEL_Z));
179 break;
180 case SENSOR_CHAN_GYRO_XYZ:
181 encode_bmask = BIT(icm45686_get_channel_position(SENSOR_CHAN_GYRO_X)) |
182 BIT(icm45686_get_channel_position(SENSOR_CHAN_GYRO_Y)) |
183 BIT(icm45686_get_channel_position(SENSOR_CHAN_GYRO_Z));
184 break;
185 default:
186 break;
187 }
188
189 return encode_bmask;
190 }
191
icm45686_encode(const struct device * dev,const struct sensor_chan_spec * const channels,const size_t num_channels,uint8_t * buf)192 int icm45686_encode(const struct device *dev,
193 const struct sensor_chan_spec *const channels,
194 const size_t num_channels,
195 uint8_t *buf)
196 {
197 struct icm45686_encoded_data *edata = (struct icm45686_encoded_data *)buf;
198 const struct icm45686_config *dev_config = dev->config;
199 uint64_t cycles;
200 int err;
201
202 edata->header.channels = 0;
203
204 for (size_t i = 0 ; i < num_channels ; i++) {
205 edata->header.channels |= icm45686_encode_channel(channels[i].chan_type);
206 }
207
208 err = sensor_clock_get_cycles(&cycles);
209 if (err != 0) {
210 return err;
211 }
212
213 edata->header.events = 0;
214 edata->header.accel_fs = dev_config->settings.accel.fs;
215 edata->header.gyro_fs = dev_config->settings.gyro.fs;
216 edata->header.timestamp = sensor_clock_cycles_to_ns(cycles);
217
218 return 0;
219 }
220
icm45686_decoder_get_frame_count(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint16_t * frame_count)221 static int icm45686_decoder_get_frame_count(const uint8_t *buffer,
222 struct sensor_chan_spec chan_spec,
223 uint16_t *frame_count)
224 {
225 struct icm45686_encoded_data *edata = (struct icm45686_encoded_data *)buffer;
226
227 if (chan_spec.chan_idx != 0) {
228 return -ENOTSUP;
229 }
230
231 uint8_t channel_request = icm45686_encode_channel(chan_spec.chan_type);
232
233 if ((edata->header.channels & channel_request) != channel_request) {
234 return -ENODATA;
235 }
236
237 if (!edata->header.events ||
238 (edata->header.events & REG_INT1_STATUS0_DRDY(true))) {
239 switch (chan_spec.chan_type) {
240 case SENSOR_CHAN_ACCEL_X:
241 case SENSOR_CHAN_ACCEL_Y:
242 case SENSOR_CHAN_ACCEL_Z:
243 case SENSOR_CHAN_ACCEL_XYZ:
244 case SENSOR_CHAN_GYRO_X:
245 case SENSOR_CHAN_GYRO_Y:
246 case SENSOR_CHAN_GYRO_Z:
247 case SENSOR_CHAN_GYRO_XYZ:
248 case SENSOR_CHAN_DIE_TEMP:
249 *frame_count = 1;
250 return 0;
251 default:
252 return -ENOTSUP;
253 }
254 }
255
256 if (edata->header.events & REG_INT1_STATUS0_FIFO_THS(true) ||
257 edata->header.events & REG_INT1_STATUS0_FIFO_FULL(true)) {
258 switch (chan_spec.chan_type) {
259 case SENSOR_CHAN_ACCEL_XYZ:
260 case SENSOR_CHAN_GYRO_XYZ:
261 case SENSOR_CHAN_DIE_TEMP:
262 *frame_count = edata->header.fifo_count;
263 return 0;
264 /** We're skipping individual axis for fifo packets */
265 default:
266 return -ENOTSUP;
267 }
268 }
269
270 return -1;
271 }
272
icm45686_decoder_get_size_info(struct sensor_chan_spec chan_spec,size_t * base_size,size_t * frame_size)273 static int icm45686_decoder_get_size_info(struct sensor_chan_spec chan_spec,
274 size_t *base_size,
275 size_t *frame_size)
276 {
277 switch (chan_spec.chan_type) {
278 case SENSOR_CHAN_ACCEL_XYZ:
279 case SENSOR_CHAN_GYRO_XYZ:
280 *base_size = sizeof(struct sensor_three_axis_data);
281 *frame_size = sizeof(struct sensor_three_axis_sample_data);
282 return 0;
283 case SENSOR_CHAN_ACCEL_X:
284 case SENSOR_CHAN_ACCEL_Y:
285 case SENSOR_CHAN_ACCEL_Z:
286 case SENSOR_CHAN_GYRO_X:
287 case SENSOR_CHAN_GYRO_Y:
288 case SENSOR_CHAN_GYRO_Z:
289 case SENSOR_CHAN_DIE_TEMP:
290 *base_size = sizeof(struct sensor_q31_data);
291 *frame_size = sizeof(struct sensor_q31_sample_data);
292 return 0;
293 default:
294 return -ENOTSUP;
295 }
296 }
297
icm45686_one_shot_decode(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)298 static int icm45686_one_shot_decode(const uint8_t *buffer,
299 struct sensor_chan_spec chan_spec,
300 uint32_t *fit,
301 uint16_t max_count,
302 void *data_out)
303 {
304 struct icm45686_encoded_data *edata = (struct icm45686_encoded_data *)buffer;
305 uint8_t channel_request;
306 int err;
307
308 if (*fit != 0) {
309 return 0;
310 }
311
312 if (max_count == 0 || chan_spec.chan_idx != 0) {
313 return -EINVAL;
314 }
315
316 switch (chan_spec.chan_type) {
317 case SENSOR_CHAN_ACCEL_X:
318 case SENSOR_CHAN_ACCEL_Y:
319 case SENSOR_CHAN_ACCEL_Z:
320 case SENSOR_CHAN_GYRO_X:
321 case SENSOR_CHAN_GYRO_Y:
322 case SENSOR_CHAN_GYRO_Z:
323 case SENSOR_CHAN_DIE_TEMP: {
324 channel_request = icm45686_encode_channel(chan_spec.chan_type);
325 if ((channel_request & edata->header.channels) != channel_request) {
326 return -ENODATA;
327 }
328
329 struct sensor_q31_data *out = data_out;
330
331 out->header.base_timestamp_ns = edata->header.timestamp;
332 out->header.reading_count = 1;
333
334 err = icm45686_get_shift(chan_spec.chan_type,
335 edata->header.accel_fs,
336 edata->header.gyro_fs,
337 &out->shift);
338 if (err != 0) {
339 return -EINVAL;
340 }
341
342 icm45686_convert_raw_to_q31(
343 edata,
344 chan_spec.chan_type,
345 edata->payload.readings[icm45686_get_channel_position(chan_spec.chan_type)],
346 &out->readings[0].value);
347 *fit = 1;
348 return 1;
349 }
350 case SENSOR_CHAN_ACCEL_XYZ:
351 case SENSOR_CHAN_GYRO_XYZ: {
352 channel_request = icm45686_encode_channel(chan_spec.chan_type);
353 if ((channel_request & edata->header.channels) != channel_request) {
354 return -ENODATA;
355 }
356
357 struct sensor_three_axis_data *out = data_out;
358 struct icm45686_encoded_payload *payload = &edata->payload;
359
360 out->header.base_timestamp_ns = edata->header.timestamp;
361 out->header.reading_count = 1;
362
363 icm45686_convert_raw_to_q31(
364 edata,
365 chan_spec.chan_type - 3,
366 payload->readings[icm45686_get_channel_position(chan_spec.chan_type - 3)],
367 &out->readings[0].x);
368 icm45686_convert_raw_to_q31(
369 edata,
370 chan_spec.chan_type - 2,
371 payload->readings[icm45686_get_channel_position(chan_spec.chan_type - 2)],
372 &out->readings[0].y);
373 icm45686_convert_raw_to_q31(
374 edata,
375 chan_spec.chan_type - 1,
376 payload->readings[icm45686_get_channel_position(chan_spec.chan_type - 1)],
377 &out->readings[0].z);
378 *fit = 1;
379 return 1;
380 }
381 default:
382 return -EINVAL;
383 }
384 }
385
icm45686_fifo_read_temp_from_packet(const uint8_t * pkt)386 static q31_t icm45686_fifo_read_temp_from_packet(const uint8_t *pkt)
387 {
388 struct icm45686_encoded_fifo_payload *fdata = (struct icm45686_encoded_fifo_payload *)pkt;
389
390 int32_t whole;
391 uint32_t fraction;
392 int64_t intermediate;
393 int8_t shift;
394 int err;
395
396 err = icm45686_get_shift(SENSOR_CHAN_DIE_TEMP, 0, 0, &shift);
397 if (err != 0) {
398 return -1;
399 }
400
401 icm45686_temp_c(fdata->temp, &whole, &fraction);
402
403 intermediate = ((int64_t)whole * INT64_C(1000000) + fraction);
404 if (shift < 0) {
405 intermediate =
406 intermediate * ((int64_t)INT32_MAX + 1) * (1 << -shift) / INT64_C(1000000);
407 } else if (shift > 0) {
408 intermediate =
409 intermediate * ((int64_t)INT32_MAX + 1) / ((1 << shift) * INT64_C(1000000));
410 }
411
412 return CLAMP(intermediate, INT32_MIN, INT32_MAX);
413 }
414
icm45686_fifo_read_imu_from_packet(const uint8_t * pkt,bool is_accel,uint8_t axis_offset,q31_t * out)415 static int icm45686_fifo_read_imu_from_packet(const uint8_t *pkt,
416 bool is_accel,
417 uint8_t axis_offset,
418 q31_t *out)
419 {
420 uint32_t unsigned_value;
421 int32_t signed_value;
422 int offset = 1 + (axis_offset * 2) + (is_accel ? 0 : 6);
423 uint32_t mask = is_accel ? GENMASK(7, 4) : GENMASK(3, 0);
424 uint8_t accel_fs = ICM45686_DT_ACCEL_FS_32;
425 uint8_t gyro_fs = ICM45686_DT_GYRO_FS_4000;
426
427 int32_t whole;
428 int32_t fraction;
429 int64_t intermediate;
430 int8_t shift;
431
432 unsigned_value = (pkt[offset] | (pkt[offset + 1] << 8));
433 if (unsigned_value == FIFO_NO_DATA) {
434 return -ENODATA;
435 }
436
437 unsigned_value = (unsigned_value << 4) |
438 ((pkt[17 + axis_offset] & mask) >> (is_accel ? 4 : 0));
439 signed_value = sign_extend(unsigned_value, 19);
440
441 if (!is_accel) {
442 icm45686_get_shift(SENSOR_CHAN_GYRO_XYZ, accel_fs, gyro_fs, &shift);
443 icm45686_gyro_rads(gyro_fs, signed_value, true, &whole, &fraction);
444 } else {
445 icm45686_get_shift(SENSOR_CHAN_ACCEL_XYZ, accel_fs, gyro_fs, &shift);
446 icm45686_accel_ms(accel_fs, signed_value, true, &whole, &fraction);
447 }
448
449 intermediate = ((int64_t)whole * INT64_C(1000000) + fraction);
450 if (shift < 0) {
451 intermediate =
452 intermediate * ((int64_t)INT32_MAX + 1) * (1 << -shift) / INT64_C(1000000);
453 } else if (shift > 0) {
454 intermediate =
455 intermediate * ((int64_t)INT32_MAX + 1) / ((1 << shift) * INT64_C(1000000));
456 }
457
458 *out = CLAMP(intermediate, INT32_MIN, INT32_MAX);
459
460 return 0;
461 }
462
icm45686_fifo_decode(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)463 static int icm45686_fifo_decode(const uint8_t *buffer,
464 struct sensor_chan_spec chan_spec,
465 uint32_t *fit,
466 uint16_t max_count,
467 void *data_out)
468 {
469 struct icm45686_encoded_data *edata = (struct icm45686_encoded_data *)buffer;
470 struct icm45686_encoded_fifo_payload *frame_begin = &edata->fifo_payload;
471 int count = 0;
472 int err;
473
474 if (*fit >= edata->header.fifo_count || chan_spec.chan_idx != 0) {
475 return 0;
476 }
477
478 while (count < max_count && (*fit < edata->header.fifo_count)) {
479 struct icm45686_encoded_fifo_payload *fdata = &frame_begin[*fit];
480
481 /** This driver assumes 20-byte fifo packets, with both accel and gyro,
482 * and no auxiliary sensors.
483 */
484 __ASSERT(!(fdata->header & FIFO_HEADER_EXT_HEADER_EN(true)) &&
485 (fdata->header & FIFO_HEADER_ACCEL_EN(true)) &&
486 (fdata->header & FIFO_HEADER_GYRO_EN(true)) &&
487 (fdata->header & FIFO_HEADER_HIRES_EN(true)),
488 "Unsupported FIFO packet format");
489
490 switch (chan_spec.chan_type) {
491 case SENSOR_CHAN_ACCEL_XYZ:
492 case SENSOR_CHAN_GYRO_XYZ: {
493 struct sensor_three_axis_data *out = data_out;
494 bool is_accel = chan_spec.chan_type == SENSOR_CHAN_ACCEL_XYZ;
495
496 icm45686_get_shift(chan_spec.chan_type,
497 edata->header.accel_fs,
498 edata->header.gyro_fs,
499 &out->shift);
500
501 out->header.base_timestamp_ns = edata->header.timestamp;
502
503 err = icm45686_fifo_read_imu_from_packet((uint8_t *)fdata,
504 is_accel,
505 0,
506 &out->readings[count].x);
507 err |= icm45686_fifo_read_imu_from_packet((uint8_t *)fdata,
508 is_accel,
509 1,
510 &out->readings[count].y);
511 err |= icm45686_fifo_read_imu_from_packet((uint8_t *)fdata,
512 is_accel,
513 2,
514 &out->readings[count].z);
515 if (err != 0) {
516 count--;
517 }
518 break;
519 }
520 case SENSOR_CHAN_DIE_TEMP: {
521 struct sensor_q31_data *out = data_out;
522
523 icm45686_get_shift(chan_spec.chan_type,
524 edata->header.accel_fs,
525 edata->header.gyro_fs,
526 &out->shift);
527
528 out->header.base_timestamp_ns = edata->header.timestamp;
529 out->readings[count].temperature =
530 icm45686_fifo_read_temp_from_packet((uint8_t *)fdata);
531 break;
532 }
533 default:
534 return 0;
535 }
536 *fit = *fit + 1;
537 count++;
538 }
539
540 return count;
541 }
542
icm45686_decoder_decode(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)543 static int icm45686_decoder_decode(const uint8_t *buffer,
544 struct sensor_chan_spec chan_spec,
545 uint32_t *fit,
546 uint16_t max_count,
547 void *data_out)
548 {
549 struct icm45686_encoded_data *edata = (struct icm45686_encoded_data *)buffer;
550
551 if (edata->header.events & REG_INT1_STATUS0_FIFO_THS(true) ||
552 edata->header.events & REG_INT1_STATUS0_FIFO_FULL(true)) {
553 return icm45686_fifo_decode(buffer, chan_spec, fit, max_count, data_out);
554 } else {
555 return icm45686_one_shot_decode(buffer, chan_spec, fit, max_count, data_out);
556 }
557 }
558
icm45686_decoder_has_trigger(const uint8_t * buffer,enum sensor_trigger_type trigger)559 static bool icm45686_decoder_has_trigger(const uint8_t *buffer, enum sensor_trigger_type trigger)
560 {
561 struct icm45686_encoded_data *edata = (struct icm45686_encoded_data *)buffer;
562
563 switch (trigger) {
564 case SENSOR_TRIG_DATA_READY:
565 return edata->header.events & REG_INT1_STATUS0_DRDY(true);
566 case SENSOR_TRIG_FIFO_WATERMARK:
567 return edata->header.events & REG_INT1_STATUS0_FIFO_THS(true);
568 case SENSOR_TRIG_FIFO_FULL:
569 return edata->header.events & REG_INT1_STATUS0_FIFO_FULL(true);
570 default:
571 break;
572 }
573
574 return false;
575 }
576
577 SENSOR_DECODER_API_DT_DEFINE() = {
578 .get_frame_count = icm45686_decoder_get_frame_count,
579 .get_size_info = icm45686_decoder_get_size_info,
580 .decode = icm45686_decoder_decode,
581 .has_trigger = icm45686_decoder_has_trigger,
582 };
583
icm45686_get_decoder(const struct device * dev,const struct sensor_decoder_api ** decoder)584 int icm45686_get_decoder(const struct device *dev,
585 const struct sensor_decoder_api **decoder)
586 {
587 ARG_UNUSED(dev);
588 *decoder = &SENSOR_DECODER_NAME();
589
590 return 0;
591 }
592
