1 /*
2 * Copyright (c) 2022 Intel Corporation
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #ifndef ZEPHYR_DRIVERS_SENSOR_ICM42688_H_
8 #define ZEPHYR_DRIVERS_SENSOR_ICM42688_H_
9
10 #include <zephyr/drivers/gpio.h>
11 #include <zephyr/drivers/sensor.h>
12 #include <zephyr/drivers/spi.h>
13 #include <zephyr/sys/byteorder.h>
14 #include <zephyr/dt-bindings/sensor/icm42688.h>
15 #include <stdlib.h>
16
icm42688_accel_fs_to_reg(uint8_t g)17 static inline uint8_t icm42688_accel_fs_to_reg(uint8_t g)
18 {
19 if (g >= 16) {
20 return ICM42688_DT_ACCEL_FS_16;
21 } else if (g >= 8) {
22 return ICM42688_DT_ACCEL_FS_8;
23 } else if (g >= 4) {
24 return ICM42688_DT_ACCEL_FS_4;
25 } else {
26 return ICM42688_DT_ACCEL_FS_2;
27 }
28 }
29
icm42688_accel_reg_to_fs(uint8_t fs,struct sensor_value * out)30 static inline void icm42688_accel_reg_to_fs(uint8_t fs, struct sensor_value *out)
31 {
32 switch (fs) {
33 case ICM42688_DT_ACCEL_FS_16:
34 sensor_g_to_ms2(16, out);
35 return;
36 case ICM42688_DT_ACCEL_FS_8:
37 sensor_g_to_ms2(8, out);
38 return;
39 case ICM42688_DT_ACCEL_FS_4:
40 sensor_g_to_ms2(4, out);
41 return;
42 case ICM42688_DT_ACCEL_FS_2:
43 sensor_g_to_ms2(2, out);
44 return;
45 }
46 }
47
icm42688_gyro_fs_to_reg(uint16_t dps)48 static inline uint8_t icm42688_gyro_fs_to_reg(uint16_t dps)
49 {
50 if (dps >= 2000) {
51 return ICM42688_DT_GYRO_FS_2000;
52 } else if (dps >= 1000) {
53 return ICM42688_DT_GYRO_FS_1000;
54 } else if (dps >= 500) {
55 return ICM42688_DT_GYRO_FS_500;
56 } else if (dps >= 250) {
57 return ICM42688_DT_GYRO_FS_250;
58 } else if (dps >= 125) {
59 return ICM42688_DT_GYRO_FS_125;
60 } else if (dps >= 62) {
61 return ICM42688_DT_GYRO_FS_62_5;
62 } else if (dps >= 31) {
63 return ICM42688_DT_GYRO_FS_31_25;
64 } else {
65 return ICM42688_DT_GYRO_FS_15_625;
66 }
67 }
68
icm42688_gyro_reg_to_fs(uint8_t fs,struct sensor_value * out)69 static inline void icm42688_gyro_reg_to_fs(uint8_t fs, struct sensor_value *out)
70 {
71 switch (fs) {
72 case ICM42688_DT_GYRO_FS_2000:
73 sensor_degrees_to_rad(2000, out);
74 return;
75 case ICM42688_DT_GYRO_FS_1000:
76 sensor_degrees_to_rad(1000, out);
77 return;
78 case ICM42688_DT_GYRO_FS_500:
79 sensor_degrees_to_rad(500, out);
80 return;
81 case ICM42688_DT_GYRO_FS_250:
82 sensor_degrees_to_rad(250, out);
83 return;
84 case ICM42688_DT_GYRO_FS_125:
85 sensor_degrees_to_rad(125, out);
86 return;
87 case ICM42688_DT_GYRO_FS_62_5:
88 sensor_10udegrees_to_rad(6250000, out);
89 return;
90 case ICM42688_DT_GYRO_FS_31_25:
91 sensor_10udegrees_to_rad(3125000, out);
92 return;
93 case ICM42688_DT_GYRO_FS_15_625:
94 sensor_10udegrees_to_rad(1562500, out);
95 return;
96 }
97 }
98
icm42688_accel_hz_to_reg(uint16_t hz)99 static inline uint8_t icm42688_accel_hz_to_reg(uint16_t hz)
100 {
101 if (hz >= 32000) {
102 return ICM42688_DT_ACCEL_ODR_32000;
103 } else if (hz >= 16000) {
104 return ICM42688_DT_ACCEL_ODR_16000;
105 } else if (hz >= 8000) {
106 return ICM42688_DT_ACCEL_ODR_8000;
107 } else if (hz >= 4000) {
108 return ICM42688_DT_ACCEL_ODR_4000;
109 } else if (hz >= 2000) {
110 return ICM42688_DT_ACCEL_ODR_2000;
111 } else if (hz >= 1000) {
112 return ICM42688_DT_ACCEL_ODR_1000;
113 } else if (hz >= 500) {
114 return ICM42688_DT_ACCEL_ODR_500;
115 } else if (hz >= 200) {
116 return ICM42688_DT_ACCEL_ODR_200;
117 } else if (hz >= 100) {
118 return ICM42688_DT_ACCEL_ODR_100;
119 } else if (hz >= 50) {
120 return ICM42688_DT_ACCEL_ODR_50;
121 } else if (hz >= 25) {
122 return ICM42688_DT_ACCEL_ODR_25;
123 } else if (hz >= 12) {
124 return ICM42688_DT_ACCEL_ODR_12_5;
125 } else if (hz >= 6) {
126 return ICM42688_DT_ACCEL_ODR_6_25;
127 } else if (hz >= 3) {
128 return ICM42688_DT_ACCEL_ODR_3_125;
129 } else {
130 return ICM42688_DT_ACCEL_ODR_1_5625;
131 }
132 }
133
icm42688_accel_reg_to_hz(uint8_t odr,struct sensor_value * out)134 static inline void icm42688_accel_reg_to_hz(uint8_t odr, struct sensor_value *out)
135 {
136 switch (odr) {
137 case ICM42688_DT_ACCEL_ODR_32000:
138 out->val1 = 32000;
139 out->val2 = 0;
140 return;
141 case ICM42688_DT_ACCEL_ODR_16000:
142 out->val1 = 1600;
143 out->val2 = 0;
144 return;
145 case ICM42688_DT_ACCEL_ODR_8000:
146 out->val1 = 8000;
147 out->val2 = 0;
148 return;
149 case ICM42688_DT_ACCEL_ODR_4000:
150 out->val1 = 4000;
151 out->val2 = 0;
152 return;
153 case ICM42688_DT_ACCEL_ODR_2000:
154 out->val1 = 2000;
155 out->val2 = 0;
156 return;
157 case ICM42688_DT_ACCEL_ODR_1000:
158 out->val1 = 1000;
159 out->val2 = 0;
160 return;
161 case ICM42688_DT_ACCEL_ODR_500:
162 out->val1 = 500;
163 out->val2 = 0;
164 return;
165 case ICM42688_DT_ACCEL_ODR_200:
166 out->val1 = 200;
167 out->val2 = 0;
168 return;
169 case ICM42688_DT_ACCEL_ODR_100:
170 out->val1 = 100;
171 out->val2 = 0;
172 return;
173 case ICM42688_DT_ACCEL_ODR_50:
174 out->val1 = 50;
175 out->val2 = 0;
176 return;
177 case ICM42688_DT_ACCEL_ODR_25:
178 out->val1 = 25;
179 out->val2 = 0;
180 return;
181 case ICM42688_DT_ACCEL_ODR_12_5:
182 out->val1 = 12;
183 out->val2 = 500000;
184 return;
185 case ICM42688_DT_ACCEL_ODR_6_25:
186 out->val1 = 6;
187 out->val2 = 250000;
188 return;
189 case ICM42688_DT_ACCEL_ODR_3_125:
190 out->val1 = 3;
191 out->val2 = 125000;
192 return;
193 case ICM42688_DT_ACCEL_ODR_1_5625:
194 out->val1 = 1;
195 out->val2 = 562500;
196 return;
197 }
198 }
199
icm42688_gyro_odr_to_reg(uint16_t hz)200 static inline uint8_t icm42688_gyro_odr_to_reg(uint16_t hz)
201 {
202 if (hz >= 32000) {
203 return ICM42688_DT_GYRO_ODR_32000;
204 } else if (hz >= 16000) {
205 return ICM42688_DT_GYRO_ODR_16000;
206 } else if (hz >= 8000) {
207 return ICM42688_DT_GYRO_ODR_8000;
208 } else if (hz >= 4000) {
209 return ICM42688_DT_GYRO_ODR_4000;
210 } else if (hz >= 2000) {
211 return ICM42688_DT_GYRO_ODR_2000;
212 } else if (hz >= 1000) {
213 return ICM42688_DT_GYRO_ODR_1000;
214 } else if (hz >= 500) {
215 return ICM42688_DT_GYRO_ODR_500;
216 } else if (hz >= 200) {
217 return ICM42688_DT_GYRO_ODR_200;
218 } else if (hz >= 100) {
219 return ICM42688_DT_GYRO_ODR_100;
220 } else if (hz >= 50) {
221 return ICM42688_DT_GYRO_ODR_50;
222 } else if (hz >= 25) {
223 return ICM42688_DT_GYRO_ODR_25;
224 } else {
225 return ICM42688_DT_GYRO_ODR_12_5;
226 }
227 }
228
icm42688_gyro_reg_to_odr(uint8_t odr,struct sensor_value * out)229 static inline void icm42688_gyro_reg_to_odr(uint8_t odr, struct sensor_value *out)
230 {
231 switch (odr) {
232 case ICM42688_DT_GYRO_ODR_32000:
233 out->val1 = 32000;
234 out->val2 = 0;
235 return;
236 case ICM42688_DT_GYRO_ODR_16000:
237 out->val1 = 16000;
238 out->val2 = 0;
239 return;
240 case ICM42688_DT_GYRO_ODR_8000:
241 out->val1 = 8000;
242 out->val2 = 0;
243 return;
244 case ICM42688_DT_GYRO_ODR_4000:
245 out->val1 = 4000;
246 out->val2 = 0;
247 return;
248 case ICM42688_DT_GYRO_ODR_2000:
249 out->val1 = 2000;
250 out->val2 = 0;
251 return;
252 case ICM42688_DT_GYRO_ODR_1000:
253 out->val1 = 1000;
254 out->val2 = 0;
255 return;
256 case ICM42688_DT_GYRO_ODR_500:
257 out->val1 = 500;
258 out->val2 = 0;
259 return;
260 case ICM42688_DT_GYRO_ODR_200:
261 out->val1 = 200;
262 out->val2 = 0;
263 return;
264 case ICM42688_DT_GYRO_ODR_100:
265 out->val1 = 100;
266 out->val2 = 0;
267 return;
268 case ICM42688_DT_GYRO_ODR_50:
269 out->val1 = 50;
270 out->val2 = 0;
271 return;
272 case ICM42688_DT_GYRO_ODR_25:
273 out->val1 = 25;
274 out->val2 = 0;
275 return;
276 case ICM42688_DT_GYRO_ODR_12_5:
277 out->val1 = 12;
278 out->val2 = 500000;
279 return;
280 }
281 }
282
283 /**
284 * @brief All sensor configuration options
285 */
286 struct icm42688_cfg {
287 uint8_t accel_pwr_mode;
288 uint8_t accel_fs;
289 uint8_t accel_odr;
290 /* TODO accel signal processing options */
291
292 uint8_t gyro_pwr_mode;
293 uint8_t gyro_fs;
294 uint8_t gyro_odr;
295 /* TODO gyro signal processing options */
296
297 bool temp_dis;
298 /* TODO temp signal processing options */
299
300 /* TODO timestamp options */
301
302 bool fifo_en;
303 int32_t batch_ticks;
304 bool fifo_hires;
305 /* TODO additional FIFO options */
306
307 /* TODO interrupt options */
308 bool interrupt1_drdy;
309 bool interrupt1_fifo_ths;
310 bool interrupt1_fifo_full;
311 };
312
313 struct icm42688_trigger_entry {
314 struct sensor_trigger trigger;
315 sensor_trigger_handler_t handler;
316 };
317
318 /**
319 * @brief Device data (struct device)
320 */
321 struct icm42688_dev_data {
322 struct icm42688_cfg cfg;
323 #ifdef CONFIG_ICM42688_TRIGGER
324 #if defined(CONFIG_ICM42688_TRIGGER_OWN_THREAD)
325 K_KERNEL_STACK_MEMBER(thread_stack, CONFIG_ICM42688_THREAD_STACK_SIZE);
326 struct k_thread thread;
327 struct k_sem gpio_sem;
328 #elif defined(CONFIG_ICM42688_TRIGGER_GLOBAL_THREAD)
329 struct k_work work;
330 #endif
331 #ifdef CONFIG_ICM42688_STREAM
332 struct rtio_iodev_sqe *streaming_sqe;
333 struct rtio *r;
334 struct rtio_iodev *spi_iodev;
335 uint8_t int_status;
336 uint16_t fifo_count;
337 uint64_t timestamp;
338 atomic_t reading_fifo;
339 #endif /* CONFIG_ICM42688_STREAM */
340 const struct device *dev;
341 struct gpio_callback gpio_cb;
342 sensor_trigger_handler_t data_ready_handler;
343 const struct sensor_trigger *data_ready_trigger;
344 struct k_mutex mutex;
345 #endif /* CONFIG_ICM42688_TRIGGER */
346
347 int16_t readings[7];
348 };
349
350 /**
351 * @brief Device config (struct device)
352 */
353 struct icm42688_dev_cfg {
354 struct spi_dt_spec spi;
355 struct gpio_dt_spec gpio_int1;
356 struct gpio_dt_spec gpio_int2;
357 };
358
359 /**
360 * @brief Reset the sensor
361 *
362 * @param dev icm42688 device pointer
363 *
364 * @retval 0 success
365 * @retval -EINVAL Reset status or whoami register returned unexpected value.
366 */
367 int icm42688_reset(const struct device *dev);
368
369 /**
370 * @brief (Re)Configure the sensor with the given configuration
371 *
372 * @param dev icm42688 device pointer
373 * @param cfg icm42688_cfg pointer
374 *
375 * @retval 0 success
376 * @retval -errno Error
377 */
378 int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg);
379
380
381 /**
382 * @brief Safely (re)Configure the sensor with the given configuration
383 *
384 * Will rollback to prior configuration if new configuration is invalid
385 *
386 * @param dev icm42688 device pointer
387 * @param cfg icm42688_cfg pointer
388 *
389 * @retval 0 success
390 * @retval -errno Error
391 */
392 int icm42688_safely_configure(const struct device *dev, struct icm42688_cfg *cfg);
393
394 /**
395 * @brief Reads all channels
396 *
397 * Regardless of what is enabled/disabled this reads all data registers
398 * as the time to read the 14 bytes at 1MHz is going to be 112 us which
399 * is less time than a SPI transaction takes to setup typically.
400 *
401 * @param dev icm42688 device pointer
402 * @param buf 14 byte buffer to store data values (7 channels, 2 bytes each)
403 *
404 * @retval 0 success
405 * @retval -errno Error
406 */
407 int icm42688_read_all(const struct device *dev, uint8_t data[14]);
408
409 /**
410 * @brief Convert icm42688 accelerometer value to useful g values
411 *
412 * @param cfg icm42688_cfg current device configuration
413 * @param in raw data value in int32_t format
414 * @param out_g whole G's output in int32_t
415 * @param out_ug micro (1/1000000) of a G output as uint32_t
416 */
icm42688_accel_g(struct icm42688_cfg * cfg,int32_t in,int32_t * out_g,uint32_t * out_ug)417 static inline void icm42688_accel_g(struct icm42688_cfg *cfg, int32_t in, int32_t *out_g,
418 uint32_t *out_ug)
419 {
420 int32_t sensitivity;
421
422 switch (cfg->accel_fs) {
423 case ICM42688_DT_ACCEL_FS_2:
424 sensitivity = 16384;
425 break;
426 case ICM42688_DT_ACCEL_FS_4:
427 sensitivity = 8192;
428 break;
429 case ICM42688_DT_ACCEL_FS_8:
430 sensitivity = 4096;
431 break;
432 case ICM42688_DT_ACCEL_FS_16:
433 sensitivity = 2048;
434 break;
435 default:
436 CODE_UNREACHABLE;
437 }
438
439 /* Whole g's */
440 *out_g = in / sensitivity;
441
442 /* Micro g's */
443 *out_ug = ((abs(in) - (abs((*out_g)) * sensitivity)) * 1000000) / sensitivity;
444 }
445
446 /**
447 * @brief Convert icm42688 gyroscope value to useful deg/s values
448 *
449 * @param cfg icm42688_cfg current device configuration
450 * @param in raw data value in int32_t format
451 * @param out_dps whole deg/s output in int32_t
452 * @param out_udps micro (1/1000000) deg/s as uint32_t
453 */
icm42688_gyro_dps(const struct icm42688_cfg * cfg,int32_t in,int32_t * out_dps,uint32_t * out_udps)454 static inline void icm42688_gyro_dps(const struct icm42688_cfg *cfg, int32_t in, int32_t *out_dps,
455 uint32_t *out_udps)
456 {
457 int64_t sensitivity;
458
459 switch (cfg->gyro_fs) {
460 case ICM42688_DT_GYRO_FS_2000:
461 sensitivity = 164;
462 break;
463 case ICM42688_DT_GYRO_FS_1000:
464 sensitivity = 328;
465 break;
466 case ICM42688_DT_GYRO_FS_500:
467 sensitivity = 655;
468 break;
469 case ICM42688_DT_GYRO_FS_250:
470 sensitivity = 1310;
471 break;
472 case ICM42688_DT_GYRO_FS_125:
473 sensitivity = 2620;
474 break;
475 case ICM42688_DT_GYRO_FS_62_5:
476 sensitivity = 5243;
477 break;
478 case ICM42688_DT_GYRO_FS_31_25:
479 sensitivity = 10486;
480 break;
481 case ICM42688_DT_GYRO_FS_15_625:
482 sensitivity = 20972;
483 break;
484 default:
485 CODE_UNREACHABLE;
486 }
487
488 int32_t in10 = in * 10;
489
490 /* Whole deg/s */
491 *out_dps = in10 / sensitivity;
492
493 /* Micro deg/s */
494 *out_udps = ((int64_t)(llabs(in10) - (llabs((*out_dps)) * sensitivity)) * 1000000LL) /
495 sensitivity;
496 }
497
498 /**
499 * @brief Convert icm42688 accelerometer value to useful m/s^2 values
500 *
501 * @param cfg icm42688_cfg current device configuration
502 * @param in raw data value in int32_t format
503 * @param out_ms meters/s^2 (whole) output in int32_t
504 * @param out_ums micrometers/s^2 output as uint32_t
505 */
icm42688_accel_ms(const struct icm42688_cfg * cfg,int32_t in,int32_t * out_ms,int32_t * out_ums)506 static inline void icm42688_accel_ms(const struct icm42688_cfg *cfg, int32_t in, int32_t *out_ms,
507 int32_t *out_ums)
508 {
509 int64_t sensitivity = 0; /* value equivalent for 1g */
510
511 switch (cfg->accel_fs) {
512 case ICM42688_DT_ACCEL_FS_2:
513 sensitivity = 16384;
514 break;
515 case ICM42688_DT_ACCEL_FS_4:
516 sensitivity = 8192;
517 break;
518 case ICM42688_DT_ACCEL_FS_8:
519 sensitivity = 4096;
520 break;
521 case ICM42688_DT_ACCEL_FS_16:
522 sensitivity = 2048;
523 break;
524 }
525
526 /* Convert to micrometers/s^2 */
527 int64_t in_ms = in * SENSOR_G;
528
529 /* meters/s^2 whole values */
530 *out_ms = in_ms / (sensitivity * 1000000LL);
531
532 /* micrometers/s^2 */
533 *out_ums = (in_ms - (*out_ms * sensitivity * 1000000LL)) / sensitivity;
534 }
535
536 /**
537 * @brief Convert icm42688 gyroscope value to useful rad/s values
538 *
539 * @param cfg icm42688_cfg current device configuration
540 * @param in raw data value in int32_t format
541 * @param out_rads whole rad/s output in int32_t
542 * @param out_urads microrad/s as uint32_t
543 */
icm42688_gyro_rads(const struct icm42688_cfg * cfg,int32_t in,int32_t * out_rads,int32_t * out_urads)544 static inline void icm42688_gyro_rads(const struct icm42688_cfg *cfg, int32_t in, int32_t *out_rads,
545 int32_t *out_urads)
546 {
547 int64_t sensitivity = 0; /* value equivalent for 10x gyro reading deg/s */
548
549 switch (cfg->gyro_fs) {
550 case ICM42688_DT_GYRO_FS_2000:
551 sensitivity = 164;
552 break;
553 case ICM42688_DT_GYRO_FS_1000:
554 sensitivity = 328;
555 break;
556 case ICM42688_DT_GYRO_FS_500:
557 sensitivity = 655;
558 break;
559 case ICM42688_DT_GYRO_FS_250:
560 sensitivity = 1310;
561 break;
562 case ICM42688_DT_GYRO_FS_125:
563 sensitivity = 2620;
564 break;
565 case ICM42688_DT_GYRO_FS_62_5:
566 sensitivity = 5243;
567 break;
568 case ICM42688_DT_GYRO_FS_31_25:
569 sensitivity = 10486;
570 break;
571 case ICM42688_DT_GYRO_FS_15_625:
572 sensitivity = 20972;
573 break;
574 }
575
576 int64_t in10_rads = (int64_t)in * SENSOR_PI * 10LL;
577
578 /* Whole rad/s */
579 *out_rads = in10_rads / (sensitivity * 180LL * 1000000LL);
580
581 /* microrad/s */
582 *out_urads =
583 (in10_rads - (*out_rads * sensitivity * 180LL * 1000000LL)) / (sensitivity * 180LL);
584 }
585
586 /**
587 * @brief Convert icm42688 temp value to useful celsius values
588 *
589 * @param cfg icm42688_cfg current device configuration
590 * @param in raw data value in int32_t format
591 * @param out_c whole celsius output in int32_t
592 * @param out_uc micro (1/1000000) celsius as uint32_t
593 */
icm42688_temp_c(int32_t in,int32_t * out_c,uint32_t * out_uc)594 static inline void icm42688_temp_c(int32_t in, int32_t *out_c, uint32_t *out_uc)
595 {
596 int64_t sensitivity = 13248; /* value equivalent for x100 1c */
597
598 /* Offset by 25 degrees Celsius */
599 int64_t in100 = (in * 100) + (25 * sensitivity);
600
601 /* Whole celsius */
602 *out_c = in100 / sensitivity;
603
604 /* Micro celsius */
605 *out_uc = ((in100 - (*out_c) * sensitivity) * INT64_C(1000000)) / sensitivity;
606 }
607
608 #endif /* ZEPHYR_DRIVERS_SENSOR_ICM42688_H_ */
609
