1 /* ST Microelectronics LSM6DSO16IS 6-axis IMU sensor driver
2 *
3 * Copyright (c) 2023 STMicroelectronics
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 *
7 * Datasheet:
8 * https://www.st.com/resource/en/datasheet/lsm6dso16is.pdf
9 */
10
11 #define DT_DRV_COMPAT st_lsm6dso16is
12
13 #include <zephyr/drivers/sensor.h>
14 #include <zephyr/kernel.h>
15 #include <zephyr/device.h>
16 #include <zephyr/init.h>
17 #include <string.h>
18 #include <zephyr/sys/__assert.h>
19 #include <zephyr/logging/log.h>
20
21 #include "lsm6dso16is.h"
22
23 LOG_MODULE_REGISTER(LSM6DSO16IS, CONFIG_SENSOR_LOG_LEVEL);
24
25 static const uint16_t lsm6dso16is_odr_map[] = {0, 12, 26, 52, 104, 208, 416, 833,
26 1667, 3333, 6667};
27
lsm6dso16is_freq_to_odr_val(uint16_t freq)28 static int lsm6dso16is_freq_to_odr_val(uint16_t freq)
29 {
30 size_t i;
31
32 for (i = 0; i < ARRAY_SIZE(lsm6dso16is_odr_map); i++) {
33 if (freq <= lsm6dso16is_odr_map[i]) {
34 return i;
35 }
36 }
37
38 return -EINVAL;
39 }
40
lsm6dso16is_odr_to_freq_val(uint16_t odr)41 static int lsm6dso16is_odr_to_freq_val(uint16_t odr)
42 {
43 /* for valid index, return value from map */
44 if (odr < ARRAY_SIZE(lsm6dso16is_odr_map)) {
45 return lsm6dso16is_odr_map[odr & 0xF];
46 }
47
48 /* invalid index, return last entry */
49 return lsm6dso16is_odr_map[ARRAY_SIZE(lsm6dso16is_odr_map) - 1];
50 }
51
52 static const uint16_t lsm6dso16is_accel_fs_map[] = {2, 16, 4, 8};
53
lsm6dso16is_accel_range_to_fs_val(int32_t range)54 static int lsm6dso16is_accel_range_to_fs_val(int32_t range)
55 {
56 size_t i;
57
58 for (i = 0; i < ARRAY_SIZE(lsm6dso16is_accel_fs_map); i++) {
59 if (range == lsm6dso16is_accel_fs_map[i]) {
60 return i;
61 }
62 }
63
64 return -EINVAL;
65 }
66
67 static const uint16_t lsm6dso16is_gyro_fs_map[] = {250, 125, 500, 0, 1000, 0, 2000};
68 static const uint16_t lsm6dso16is_gyro_fs_sens[] = {2, 1, 4, 0, 8, 0, 16};
69
lsm6dso16is_gyro_range_to_fs_val(int32_t range)70 static int lsm6dso16is_gyro_range_to_fs_val(int32_t range)
71 {
72 size_t i;
73
74 for (i = 0; i < ARRAY_SIZE(lsm6dso16is_gyro_fs_map); i++) {
75 if (range == lsm6dso16is_gyro_fs_map[i]) {
76 return i;
77 }
78 }
79
80 return -EINVAL;
81 }
82
lsm6dso16is_reboot(const struct device * dev)83 static inline int lsm6dso16is_reboot(const struct device *dev)
84 {
85 const struct lsm6dso16is_config *cfg = dev->config;
86 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
87
88 if (lsm6dso16is_boot_set(ctx, 1) < 0) {
89 return -EIO;
90 }
91
92 /* Wait sensor turn-on time as per datasheet */
93 k_sleep(K_MSEC(35)); /* turn-on time in ms */
94
95 return 0;
96 }
97
lsm6dso16is_accel_set_fs_raw(const struct device * dev,uint8_t fs)98 static int lsm6dso16is_accel_set_fs_raw(const struct device *dev, uint8_t fs)
99 {
100 const struct lsm6dso16is_config *cfg = dev->config;
101 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
102 struct lsm6dso16is_data *data = dev->data;
103
104 if (lsm6dso16is_xl_full_scale_set(ctx, fs) < 0) {
105 return -EIO;
106 }
107
108 data->accel_fs = fs;
109
110 return 0;
111 }
112
lsm6dso16is_accel_set_odr_raw(const struct device * dev,uint8_t odr)113 static int lsm6dso16is_accel_set_odr_raw(const struct device *dev, uint8_t odr)
114 {
115 const struct lsm6dso16is_config *cfg = dev->config;
116 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
117 struct lsm6dso16is_data *data = dev->data;
118
119 if (lsm6dso16is_xl_data_rate_set(ctx, odr) < 0) {
120 return -EIO;
121 }
122
123 data->accel_freq = lsm6dso16is_odr_to_freq_val(odr);
124
125 return 0;
126 }
127
lsm6dso16is_gyro_set_fs_raw(const struct device * dev,uint8_t fs)128 static int lsm6dso16is_gyro_set_fs_raw(const struct device *dev, uint8_t fs)
129 {
130 const struct lsm6dso16is_config *cfg = dev->config;
131 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
132
133 if (lsm6dso16is_gy_full_scale_set(ctx, fs) < 0) {
134 return -EIO;
135 }
136
137 return 0;
138 }
139
lsm6dso16is_gyro_set_odr_raw(const struct device * dev,uint8_t odr)140 static int lsm6dso16is_gyro_set_odr_raw(const struct device *dev, uint8_t odr)
141 {
142 const struct lsm6dso16is_config *cfg = dev->config;
143 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
144
145 if (lsm6dso16is_gy_data_rate_set(ctx, odr) < 0) {
146 return -EIO;
147 }
148
149 return 0;
150 }
151
lsm6dso16is_accel_odr_set(const struct device * dev,uint16_t freq)152 static int lsm6dso16is_accel_odr_set(const struct device *dev, uint16_t freq)
153 {
154 int odr;
155
156 odr = lsm6dso16is_freq_to_odr_val(freq);
157 if (odr < 0) {
158 return odr;
159 }
160
161 if (lsm6dso16is_accel_set_odr_raw(dev, odr) < 0) {
162 LOG_DBG("failed to set accelerometer sampling rate");
163 return -EIO;
164 }
165
166 return 0;
167 }
168
lsm6dso16is_accel_range_set(const struct device * dev,int32_t range)169 static int lsm6dso16is_accel_range_set(const struct device *dev, int32_t range)
170 {
171 int fs;
172 struct lsm6dso16is_data *data = dev->data;
173
174 fs = lsm6dso16is_accel_range_to_fs_val(range);
175 if (fs < 0) {
176 return fs;
177 }
178
179 if (lsm6dso16is_accel_set_fs_raw(dev, fs) < 0) {
180 LOG_DBG("failed to set accelerometer full-scale");
181 return -EIO;
182 }
183
184 data->acc_gain = lsm6dso16is_accel_fs_map[fs] * GAIN_UNIT_XL / 2;
185 return 0;
186 }
187
lsm6dso16is_accel_config(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)188 static int lsm6dso16is_accel_config(const struct device *dev,
189 enum sensor_channel chan,
190 enum sensor_attribute attr,
191 const struct sensor_value *val)
192 {
193 const struct lsm6dso16is_config *cfg = dev->config;
194 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
195 lsm6dso16is_hm_mode_t mode;
196
197 switch (attr) {
198 case SENSOR_ATTR_FULL_SCALE:
199 return lsm6dso16is_accel_range_set(dev, sensor_ms2_to_g(val));
200 case SENSOR_ATTR_SAMPLING_FREQUENCY:
201 return lsm6dso16is_accel_odr_set(dev, val->val1);
202 case SENSOR_ATTR_CONFIGURATION:
203 switch (val->val1) {
204 case 0: /* High Performance */
205 mode = LSM6DSO16IS_HIGH_PERFOMANCE_MODE_ENABLED;
206 break;
207 case 1: /* Low Power */
208 mode = LSM6DSO16IS_HIGH_PERFOMANCE_MODE_DISABLED;
209 break;
210 default:
211 return -EIO;
212 }
213
214 return lsm6dso16is_xl_hm_mode_set(ctx, mode);
215 default:
216 LOG_DBG("Accel attribute not supported.");
217 return -ENOTSUP;
218 }
219
220 return 0;
221 }
222
lsm6dso16is_gyro_odr_set(const struct device * dev,uint16_t freq)223 static int lsm6dso16is_gyro_odr_set(const struct device *dev, uint16_t freq)
224 {
225 int odr;
226
227 odr = lsm6dso16is_freq_to_odr_val(freq);
228 if (odr < 0) {
229 return odr;
230 }
231
232 if (lsm6dso16is_gyro_set_odr_raw(dev, odr) < 0) {
233 LOG_DBG("failed to set gyroscope sampling rate");
234 return -EIO;
235 }
236
237 return 0;
238 }
239
lsm6dso16is_gyro_range_set(const struct device * dev,int32_t range)240 static int lsm6dso16is_gyro_range_set(const struct device *dev, int32_t range)
241 {
242 int fs;
243 struct lsm6dso16is_data *data = dev->data;
244
245 fs = lsm6dso16is_gyro_range_to_fs_val(range);
246 if (fs < 0) {
247 return fs;
248 }
249
250 if (lsm6dso16is_gyro_set_fs_raw(dev, fs) < 0) {
251 LOG_DBG("failed to set gyroscope full-scale");
252 return -EIO;
253 }
254
255 data->gyro_gain = (lsm6dso16is_gyro_fs_sens[fs] * GAIN_UNIT_G);
256 return 0;
257 }
258
lsm6dso16is_gyro_config(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)259 static int lsm6dso16is_gyro_config(const struct device *dev,
260 enum sensor_channel chan,
261 enum sensor_attribute attr,
262 const struct sensor_value *val)
263 {
264 const struct lsm6dso16is_config *cfg = dev->config;
265 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
266 lsm6dso16is_hm_mode_t mode;
267
268 switch (attr) {
269 case SENSOR_ATTR_FULL_SCALE:
270 return lsm6dso16is_gyro_range_set(dev, sensor_rad_to_degrees(val));
271 case SENSOR_ATTR_SAMPLING_FREQUENCY:
272 return lsm6dso16is_gyro_odr_set(dev, val->val1);
273 case SENSOR_ATTR_CONFIGURATION:
274 switch (val->val1) {
275 case 0: /* High Performance */
276 mode = LSM6DSO16IS_HIGH_PERFOMANCE_MODE_ENABLED;
277 break;
278 case 1: /* Low Power */
279 mode = LSM6DSO16IS_HIGH_PERFOMANCE_MODE_DISABLED;
280 break;
281 default:
282 return -EIO;
283 }
284
285 return lsm6dso16is_xl_hm_mode_set(ctx, mode);
286 default:
287 LOG_DBG("Gyro attribute not supported.");
288 return -ENOTSUP;
289 }
290
291 return 0;
292 }
293
lsm6dso16is_attr_set(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)294 static int lsm6dso16is_attr_set(const struct device *dev,
295 enum sensor_channel chan,
296 enum sensor_attribute attr,
297 const struct sensor_value *val)
298 {
299 #if defined(CONFIG_LSM6DSO16IS_SENSORHUB)
300 struct lsm6dso16is_data *data = dev->data;
301 #endif /* CONFIG_LSM6DSO16IS_SENSORHUB */
302
303 switch (chan) {
304 case SENSOR_CHAN_ACCEL_XYZ:
305 return lsm6dso16is_accel_config(dev, chan, attr, val);
306 case SENSOR_CHAN_GYRO_XYZ:
307 return lsm6dso16is_gyro_config(dev, chan, attr, val);
308 #if defined(CONFIG_LSM6DSO16IS_SENSORHUB)
309 case SENSOR_CHAN_MAGN_XYZ:
310 case SENSOR_CHAN_PRESS:
311 case SENSOR_CHAN_HUMIDITY:
312 if (!data->shub_inited) {
313 LOG_ERR("shub not inited.");
314 return -ENOTSUP;
315 }
316
317 return lsm6dso16is_shub_config(dev, chan, attr, val);
318 #endif /* CONFIG_LSM6DSO16IS_SENSORHUB */
319 default:
320 LOG_WRN("attr_set() not supported on this channel.");
321 return -ENOTSUP;
322 }
323
324 return 0;
325 }
326
lsm6dso16is_sample_fetch_accel(const struct device * dev)327 static int lsm6dso16is_sample_fetch_accel(const struct device *dev)
328 {
329 const struct lsm6dso16is_config *cfg = dev->config;
330 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
331 struct lsm6dso16is_data *data = dev->data;
332
333 if (lsm6dso16is_acceleration_raw_get(ctx, data->acc) < 0) {
334 LOG_DBG("Failed to read sample");
335 return -EIO;
336 }
337
338 return 0;
339 }
340
lsm6dso16is_sample_fetch_gyro(const struct device * dev)341 static int lsm6dso16is_sample_fetch_gyro(const struct device *dev)
342 {
343 const struct lsm6dso16is_config *cfg = dev->config;
344 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
345 struct lsm6dso16is_data *data = dev->data;
346
347 if (lsm6dso16is_angular_rate_raw_get(ctx, data->gyro) < 0) {
348 LOG_DBG("Failed to read sample");
349 return -EIO;
350 }
351
352 return 0;
353 }
354
355 #if defined(CONFIG_LSM6DSO16IS_ENABLE_TEMP)
lsm6dso16is_sample_fetch_temp(const struct device * dev)356 static int lsm6dso16is_sample_fetch_temp(const struct device *dev)
357 {
358 const struct lsm6dso16is_config *cfg = dev->config;
359 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
360 struct lsm6dso16is_data *data = dev->data;
361
362 if (lsm6dso16is_temperature_raw_get(ctx, &data->temp_sample) < 0) {
363 LOG_DBG("Failed to read sample");
364 return -EIO;
365 }
366
367 return 0;
368 }
369 #endif
370
371 #if defined(CONFIG_LSM6DSO16IS_SENSORHUB)
lsm6dso16is_sample_fetch_shub(const struct device * dev)372 static int lsm6dso16is_sample_fetch_shub(const struct device *dev)
373 {
374 if (lsm6dso16is_shub_fetch_external_devs(dev) < 0) {
375 LOG_DBG("failed to read ext shub devices");
376 return -EIO;
377 }
378
379 return 0;
380 }
381 #endif /* CONFIG_LSM6DSO16IS_SENSORHUB */
382
lsm6dso16is_sample_fetch(const struct device * dev,enum sensor_channel chan)383 static int lsm6dso16is_sample_fetch(const struct device *dev,
384 enum sensor_channel chan)
385 {
386 #if defined(CONFIG_LSM6DSO16IS_SENSORHUB)
387 struct lsm6dso16is_data *data = dev->data;
388 #endif /* CONFIG_LSM6DSO16IS_SENSORHUB */
389
390 switch (chan) {
391 case SENSOR_CHAN_ACCEL_XYZ:
392 lsm6dso16is_sample_fetch_accel(dev);
393 break;
394 case SENSOR_CHAN_GYRO_XYZ:
395 lsm6dso16is_sample_fetch_gyro(dev);
396 break;
397 #if defined(CONFIG_LSM6DSO16IS_ENABLE_TEMP)
398 case SENSOR_CHAN_DIE_TEMP:
399 lsm6dso16is_sample_fetch_temp(dev);
400 break;
401 #endif
402 case SENSOR_CHAN_ALL:
403 lsm6dso16is_sample_fetch_accel(dev);
404 lsm6dso16is_sample_fetch_gyro(dev);
405 #if defined(CONFIG_LSM6DSO16IS_ENABLE_TEMP)
406 lsm6dso16is_sample_fetch_temp(dev);
407 #endif
408 #if defined(CONFIG_LSM6DSO16IS_SENSORHUB)
409 if (data->shub_inited) {
410 lsm6dso16is_sample_fetch_shub(dev);
411 }
412 #endif
413 break;
414 default:
415 return -ENOTSUP;
416 }
417
418 return 0;
419 }
420
lsm6dso16is_accel_convert(struct sensor_value * val,int raw_val,uint32_t sensitivity)421 static inline void lsm6dso16is_accel_convert(struct sensor_value *val, int raw_val,
422 uint32_t sensitivity)
423 {
424 int64_t dval;
425
426 /* Sensitivity is exposed in ug/LSB */
427 /* Convert to m/s^2 */
428 dval = (int64_t)(raw_val) * sensitivity;
429 sensor_ug_to_ms2(dval, val);
430
431 }
432
lsm6dso16is_accel_get_channel(enum sensor_channel chan,struct sensor_value * val,struct lsm6dso16is_data * data,uint32_t sensitivity)433 static inline int lsm6dso16is_accel_get_channel(enum sensor_channel chan,
434 struct sensor_value *val,
435 struct lsm6dso16is_data *data,
436 uint32_t sensitivity)
437 {
438 uint8_t i;
439
440 switch (chan) {
441 case SENSOR_CHAN_ACCEL_X:
442 lsm6dso16is_accel_convert(val, data->acc[0], sensitivity);
443 break;
444 case SENSOR_CHAN_ACCEL_Y:
445 lsm6dso16is_accel_convert(val, data->acc[1], sensitivity);
446 break;
447 case SENSOR_CHAN_ACCEL_Z:
448 lsm6dso16is_accel_convert(val, data->acc[2], sensitivity);
449 break;
450 case SENSOR_CHAN_ACCEL_XYZ:
451 for (i = 0; i < 3; i++) {
452 lsm6dso16is_accel_convert(val++, data->acc[i], sensitivity);
453 }
454 break;
455 default:
456 return -ENOTSUP;
457 }
458
459 return 0;
460 }
461
lsm6dso16is_accel_channel_get(enum sensor_channel chan,struct sensor_value * val,struct lsm6dso16is_data * data)462 static int lsm6dso16is_accel_channel_get(enum sensor_channel chan,
463 struct sensor_value *val,
464 struct lsm6dso16is_data *data)
465 {
466 return lsm6dso16is_accel_get_channel(chan, val, data, data->acc_gain);
467 }
468
lsm6dso16is_gyro_convert(struct sensor_value * val,int raw_val,uint32_t sensitivity)469 static inline void lsm6dso16is_gyro_convert(struct sensor_value *val, int raw_val,
470 uint32_t sensitivity)
471 {
472 int64_t dval;
473
474 /* Sensitivity is exposed in udps/LSB */
475 /* So, calculate value in 10 udps unit and then to rad/s */
476 dval = (int64_t)(raw_val) * sensitivity / 10;
477 sensor_10udegrees_to_rad(dval, val);
478 }
479
lsm6dso16is_gyro_get_channel(enum sensor_channel chan,struct sensor_value * val,struct lsm6dso16is_data * data,uint32_t sensitivity)480 static inline int lsm6dso16is_gyro_get_channel(enum sensor_channel chan,
481 struct sensor_value *val,
482 struct lsm6dso16is_data *data,
483 uint32_t sensitivity)
484 {
485 uint8_t i;
486
487 switch (chan) {
488 case SENSOR_CHAN_GYRO_X:
489 lsm6dso16is_gyro_convert(val, data->gyro[0], sensitivity);
490 break;
491 case SENSOR_CHAN_GYRO_Y:
492 lsm6dso16is_gyro_convert(val, data->gyro[1], sensitivity);
493 break;
494 case SENSOR_CHAN_GYRO_Z:
495 lsm6dso16is_gyro_convert(val, data->gyro[2], sensitivity);
496 break;
497 case SENSOR_CHAN_GYRO_XYZ:
498 for (i = 0; i < 3; i++) {
499 lsm6dso16is_gyro_convert(val++, data->gyro[i], sensitivity);
500 }
501 break;
502 default:
503 return -ENOTSUP;
504 }
505
506 return 0;
507 }
508
lsm6dso16is_gyro_channel_get(enum sensor_channel chan,struct sensor_value * val,struct lsm6dso16is_data * data)509 static int lsm6dso16is_gyro_channel_get(enum sensor_channel chan,
510 struct sensor_value *val,
511 struct lsm6dso16is_data *data)
512 {
513 return lsm6dso16is_gyro_get_channel(chan, val, data, data->gyro_gain);
514 }
515
516 #if defined(CONFIG_LSM6DSO16IS_ENABLE_TEMP)
lsm6dso16is_gyro_channel_get_temp(struct sensor_value * val,struct lsm6dso16is_data * data)517 static void lsm6dso16is_gyro_channel_get_temp(struct sensor_value *val,
518 struct lsm6dso16is_data *data)
519 {
520 int32_t micro_c;
521
522 /* convert units to micro Celsius. Raw temperature samples are
523 * expressed in 256 LSB/deg_C units. And LSB output is 0 at 25 C.
524 */
525 micro_c = (data->temp_sample * 1000000) / 256;
526
527 val->val1 = micro_c / 1000000 + 25;
528 val->val2 = micro_c % 1000000;
529 }
530 #endif
531
532 #if defined(CONFIG_LSM6DSO16IS_SENSORHUB)
lsm6dso16is_magn_convert(struct sensor_value * val,int raw_val,uint16_t sensitivity)533 static inline void lsm6dso16is_magn_convert(struct sensor_value *val, int raw_val,
534 uint16_t sensitivity)
535 {
536 double dval;
537
538 /* Sensitivity is exposed in ugauss/LSB */
539 dval = (double)(raw_val * sensitivity);
540 val->val1 = (int32_t)dval / 1000000;
541 val->val2 = (int32_t)dval % 1000000;
542 }
543
lsm6dso16is_magn_get_channel(enum sensor_channel chan,struct sensor_value * val,struct lsm6dso16is_data * data)544 static inline int lsm6dso16is_magn_get_channel(enum sensor_channel chan,
545 struct sensor_value *val,
546 struct lsm6dso16is_data *data)
547 {
548 int16_t sample[3];
549 int idx;
550
551 idx = lsm6dso16is_shub_get_idx(data->dev, SENSOR_CHAN_MAGN_XYZ);
552 if (idx < 0) {
553 LOG_DBG("external magn not supported");
554 return -ENOTSUP;
555 }
556
557
558 sample[0] = (int16_t)(data->ext_data[idx][0] |
559 (data->ext_data[idx][1] << 8));
560 sample[1] = (int16_t)(data->ext_data[idx][2] |
561 (data->ext_data[idx][3] << 8));
562 sample[2] = (int16_t)(data->ext_data[idx][4] |
563 (data->ext_data[idx][5] << 8));
564
565 switch (chan) {
566 case SENSOR_CHAN_MAGN_X:
567 lsm6dso16is_magn_convert(val, sample[0], data->magn_gain);
568 break;
569 case SENSOR_CHAN_MAGN_Y:
570 lsm6dso16is_magn_convert(val, sample[1], data->magn_gain);
571 break;
572 case SENSOR_CHAN_MAGN_Z:
573 lsm6dso16is_magn_convert(val, sample[2], data->magn_gain);
574 break;
575 case SENSOR_CHAN_MAGN_XYZ:
576 lsm6dso16is_magn_convert(val, sample[0], data->magn_gain);
577 lsm6dso16is_magn_convert(val + 1, sample[1], data->magn_gain);
578 lsm6dso16is_magn_convert(val + 2, sample[2], data->magn_gain);
579 break;
580 default:
581 return -ENOTSUP;
582 }
583
584 return 0;
585 }
586
lsm6dso16is_hum_convert(struct sensor_value * val,struct lsm6dso16is_data * data)587 static inline void lsm6dso16is_hum_convert(struct sensor_value *val,
588 struct lsm6dso16is_data *data)
589 {
590 float rh;
591 int16_t raw_val;
592 struct hts221_data *ht = &data->hts221;
593 int idx;
594
595 idx = lsm6dso16is_shub_get_idx(data->dev, SENSOR_CHAN_HUMIDITY);
596 if (idx < 0) {
597 LOG_DBG("external press/temp not supported");
598 return;
599 }
600
601 raw_val = (int16_t)(data->ext_data[idx][0] |
602 (data->ext_data[idx][1] << 8));
603
604 /* find relative humidty by linear interpolation */
605 rh = (ht->y1 - ht->y0) * raw_val + ht->x1 * ht->y0 - ht->x0 * ht->y1;
606 rh /= (ht->x1 - ht->x0);
607
608 /* convert humidity to integer and fractional part */
609 val->val1 = rh;
610 val->val2 = rh * 1000000;
611 }
612
lsm6dso16is_press_convert(struct sensor_value * val,struct lsm6dso16is_data * data)613 static inline void lsm6dso16is_press_convert(struct sensor_value *val,
614 struct lsm6dso16is_data *data)
615 {
616 int32_t raw_val;
617 int idx;
618
619 idx = lsm6dso16is_shub_get_idx(data->dev, SENSOR_CHAN_PRESS);
620 if (idx < 0) {
621 LOG_DBG("external press/temp not supported");
622 return;
623 }
624
625 raw_val = (int32_t)(data->ext_data[idx][0] |
626 (data->ext_data[idx][1] << 8) |
627 (data->ext_data[idx][2] << 16));
628
629 /* Pressure sensitivity is 4096 LSB/hPa */
630 /* Convert raw_val to val in kPa */
631 val->val1 = (raw_val >> 12) / 10;
632 val->val2 = (raw_val >> 12) % 10 * 100000 +
633 (((int32_t)((raw_val) & 0x0FFF) * 100000L) >> 12);
634 }
635
lsm6dso16is_temp_convert(struct sensor_value * val,struct lsm6dso16is_data * data)636 static inline void lsm6dso16is_temp_convert(struct sensor_value *val,
637 struct lsm6dso16is_data *data)
638 {
639 int16_t raw_val;
640 int idx;
641
642 idx = lsm6dso16is_shub_get_idx(data->dev, SENSOR_CHAN_PRESS);
643 if (idx < 0) {
644 LOG_DBG("external press/temp not supported");
645 return;
646 }
647
648 raw_val = (int16_t)(data->ext_data[idx][3] |
649 (data->ext_data[idx][4] << 8));
650
651 /* Temperature sensitivity is 100 LSB/deg C */
652 val->val1 = raw_val / 100;
653 val->val2 = (int32_t)raw_val % 100 * (10000);
654 }
655 #endif
656
lsm6dso16is_channel_get(const struct device * dev,enum sensor_channel chan,struct sensor_value * val)657 static int lsm6dso16is_channel_get(const struct device *dev,
658 enum sensor_channel chan,
659 struct sensor_value *val)
660 {
661 struct lsm6dso16is_data *data = dev->data;
662
663 switch (chan) {
664 case SENSOR_CHAN_ACCEL_X:
665 case SENSOR_CHAN_ACCEL_Y:
666 case SENSOR_CHAN_ACCEL_Z:
667 case SENSOR_CHAN_ACCEL_XYZ:
668 lsm6dso16is_accel_channel_get(chan, val, data);
669 break;
670 case SENSOR_CHAN_GYRO_X:
671 case SENSOR_CHAN_GYRO_Y:
672 case SENSOR_CHAN_GYRO_Z:
673 case SENSOR_CHAN_GYRO_XYZ:
674 lsm6dso16is_gyro_channel_get(chan, val, data);
675 break;
676 #if defined(CONFIG_LSM6DSO16IS_ENABLE_TEMP)
677 case SENSOR_CHAN_DIE_TEMP:
678 lsm6dso16is_gyro_channel_get_temp(val, data);
679 break;
680 #endif
681 #if defined(CONFIG_LSM6DSO16IS_SENSORHUB)
682 case SENSOR_CHAN_MAGN_X:
683 case SENSOR_CHAN_MAGN_Y:
684 case SENSOR_CHAN_MAGN_Z:
685 case SENSOR_CHAN_MAGN_XYZ:
686 if (!data->shub_inited) {
687 LOG_ERR("attr_set() shub not inited.");
688 return -ENOTSUP;
689 }
690
691 lsm6dso16is_magn_get_channel(chan, val, data);
692 break;
693
694 case SENSOR_CHAN_HUMIDITY:
695 if (!data->shub_inited) {
696 LOG_ERR("attr_set() shub not inited.");
697 return -ENOTSUP;
698 }
699
700 lsm6dso16is_hum_convert(val, data);
701 break;
702
703 case SENSOR_CHAN_PRESS:
704 if (!data->shub_inited) {
705 LOG_ERR("attr_set() shub not inited.");
706 return -ENOTSUP;
707 }
708
709 lsm6dso16is_press_convert(val, data);
710 break;
711
712 case SENSOR_CHAN_AMBIENT_TEMP:
713 if (!data->shub_inited) {
714 LOG_ERR("attr_set() shub not inited.");
715 return -ENOTSUP;
716 }
717
718 lsm6dso16is_temp_convert(val, data);
719 break;
720 #endif
721 default:
722 return -ENOTSUP;
723 }
724
725 return 0;
726 }
727
728 static DEVICE_API(sensor, lsm6dso16is_driver_api) = {
729 .attr_set = lsm6dso16is_attr_set,
730 #if CONFIG_LSM6DSO16IS_TRIGGER
731 .trigger_set = lsm6dso16is_trigger_set,
732 #endif
733 .sample_fetch = lsm6dso16is_sample_fetch,
734 .channel_get = lsm6dso16is_channel_get,
735 };
736
lsm6dso16is_init_chip(const struct device * dev)737 static int lsm6dso16is_init_chip(const struct device *dev)
738 {
739 const struct lsm6dso16is_config *cfg = dev->config;
740 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
741 struct lsm6dso16is_data *lsm6dso16is = dev->data;
742 uint8_t chip_id;
743 uint8_t odr, fs;
744
745 /* All registers except 0x01 are different between banks, including the WHO_AM_I
746 * register and the register used for a SW reset. If the lsm6dso16is wasn't on the user
747 * bank when it reset, then both the chip id check and the sw reset will fail unless we
748 * set the bank now.
749 */
750 if (lsm6dso16is_mem_bank_set(ctx, LSM6DSO16IS_MAIN_MEM_BANK) < 0) {
751 LOG_DBG("Failed to set user bank");
752 return -EIO;
753 }
754
755 if (lsm6dso16is_device_id_get(ctx, &chip_id) < 0) {
756 LOG_DBG("Failed reading chip id");
757 return -EIO;
758 }
759
760 LOG_INF("chip id 0x%x", chip_id);
761
762 if (chip_id != LSM6DSO16IS_ID) {
763 LOG_DBG("Invalid chip id 0x%x", chip_id);
764 return -EIO;
765 }
766
767 /* reset device */
768 if (lsm6dso16is_software_reset(ctx) < 0) {
769 return -EIO;
770 }
771
772 fs = cfg->accel_range;
773 LOG_DBG("accel range is %d", fs);
774 if (lsm6dso16is_accel_set_fs_raw(dev, fs) < 0) {
775 LOG_ERR("failed to set accelerometer range %d", fs);
776 return -EIO;
777 }
778 lsm6dso16is->acc_gain = lsm6dso16is_accel_fs_map[fs] * GAIN_UNIT_XL / 2;
779
780 odr = cfg->accel_odr;
781 LOG_DBG("accel odr is %d", odr);
782 if (lsm6dso16is_accel_set_odr_raw(dev, odr) < 0) {
783 LOG_ERR("failed to set accelerometer odr %d", odr);
784 return -EIO;
785 }
786
787 fs = cfg->gyro_range;
788 LOG_DBG("gyro range is %d", fs);
789 if (lsm6dso16is_gyro_set_fs_raw(dev, fs) < 0) {
790 LOG_ERR("failed to set gyroscope range %d", fs);
791 return -EIO;
792 }
793 lsm6dso16is->gyro_gain = (lsm6dso16is_gyro_fs_sens[fs] * GAIN_UNIT_G);
794
795 odr = cfg->gyro_odr;
796 LOG_DBG("gyro odr is %d", odr);
797 lsm6dso16is->gyro_freq = lsm6dso16is_odr_to_freq_val(odr);
798 if (lsm6dso16is_gyro_set_odr_raw(dev, odr) < 0) {
799 LOG_ERR("failed to set gyroscope odr %d", odr);
800 return -EIO;
801 }
802
803 if (lsm6dso16is_block_data_update_set(ctx, 1) < 0) {
804 LOG_DBG("failed to set BDU mode");
805 return -EIO;
806 }
807
808 return 0;
809 }
810
lsm6dso16is_init(const struct device * dev)811 static int lsm6dso16is_init(const struct device *dev)
812 {
813 #ifdef CONFIG_LSM6DSO16IS_TRIGGER
814 const struct lsm6dso16is_config *cfg = dev->config;
815 #endif
816 struct lsm6dso16is_data *data = dev->data;
817
818 LOG_INF("Initialize device %s", dev->name);
819 data->dev = dev;
820
821 if (lsm6dso16is_init_chip(dev) < 0) {
822 LOG_DBG("failed to initialize chip");
823 return -EIO;
824 }
825
826 #ifdef CONFIG_LSM6DSO16IS_TRIGGER
827 if (cfg->trig_enabled) {
828 if (lsm6dso16is_init_interrupt(dev) < 0) {
829 LOG_ERR("Failed to initialize interrupt.");
830 return -EIO;
831 }
832 }
833 #endif
834
835 #ifdef CONFIG_LSM6DSO16IS_SENSORHUB
836 data->shub_inited = true;
837 if (lsm6dso16is_shub_init(dev) < 0) {
838 LOG_INF("shub: no external chips found");
839 data->shub_inited = false;
840 }
841 #endif
842
843 return 0;
844 }
845
846 #if DT_NUM_INST_STATUS_OKAY(DT_DRV_COMPAT) == 0
847 #warning "LSM6DSO16IS driver enabled without any devices"
848 #endif
849
850 /*
851 * Device creation macro, shared by LSM6DSO16IS_DEFINE_SPI() and
852 * LSM6DSO16IS_DEFINE_I2C().
853 */
854
855 #define LSM6DSO16IS_DEVICE_INIT(inst) \
856 SENSOR_DEVICE_DT_INST_DEFINE(inst, \
857 lsm6dso16is_init, \
858 NULL, \
859 &lsm6dso16is_data_##inst, \
860 &lsm6dso16is_config_##inst, \
861 POST_KERNEL, \
862 CONFIG_SENSOR_INIT_PRIORITY, \
863 &lsm6dso16is_driver_api);
864
865 /*
866 * Instantiation macros used when a device is on a SPI bus.
867 */
868
869 #ifdef CONFIG_LSM6DSO16IS_TRIGGER
870 #define LSM6DSO16IS_CFG_IRQ(inst) \
871 .trig_enabled = true, \
872 .gpio_drdy = GPIO_DT_SPEC_INST_GET(inst, irq_gpios), \
873 .drdy_pin = DT_INST_PROP(inst, drdy_pin)
874 #else
875 #define LSM6DSO16IS_CFG_IRQ(inst)
876 #endif /* CONFIG_LSM6DSO16IS_TRIGGER */
877
878 #define LSM6DSO16IS_SPI_OP (SPI_WORD_SET(8) | \
879 SPI_OP_MODE_MASTER | \
880 SPI_MODE_CPOL | \
881 SPI_MODE_CPHA) \
882
883 #define LSM6DSO16IS_CONFIG_COMMON(inst) \
884 .accel_odr = DT_INST_PROP(inst, accel_odr), \
885 .accel_range = DT_INST_PROP(inst, accel_range), \
886 .gyro_odr = DT_INST_PROP(inst, gyro_odr), \
887 .gyro_range = DT_INST_PROP(inst, gyro_range), \
888 .drdy_pulsed = DT_INST_PROP(inst, drdy_pulsed), \
889 COND_CODE_1(DT_INST_NODE_HAS_PROP(inst, irq_gpios), \
890 (LSM6DSO16IS_CFG_IRQ(inst)), ())
891
892
893 #define LSM6DSO16IS_CONFIG_SPI(inst) \
894 { \
895 STMEMSC_CTX_SPI(&lsm6dso16is_config_##inst.stmemsc_cfg), \
896 .stmemsc_cfg = { \
897 .spi = SPI_DT_SPEC_INST_GET(inst, \
898 LSM6DSO16IS_SPI_OP, \
899 0), \
900 }, \
901 LSM6DSO16IS_CONFIG_COMMON(inst) \
902 }
903
904 /*
905 * Instantiation macros used when a device is on an I2C bus.
906 */
907
908 #define LSM6DSO16IS_CONFIG_I2C(inst) \
909 { \
910 STMEMSC_CTX_I2C(&lsm6dso16is_config_##inst.stmemsc_cfg), \
911 .stmemsc_cfg = { \
912 .i2c = I2C_DT_SPEC_INST_GET(inst), \
913 }, \
914 LSM6DSO16IS_CONFIG_COMMON(inst) \
915 }
916
917 /*
918 * Main instantiation macro. Use of COND_CODE_1() selects the right
919 * bus-specific macro at preprocessor time.
920 */
921
922 #define LSM6DSO16IS_DEFINE(inst) \
923 static struct lsm6dso16is_data lsm6dso16is_data_##inst; \
924 static const struct lsm6dso16is_config lsm6dso16is_config_##inst = \
925 COND_CODE_1(DT_INST_ON_BUS(inst, spi), \
926 (LSM6DSO16IS_CONFIG_SPI(inst)), \
927 (LSM6DSO16IS_CONFIG_I2C(inst))); \
928 LSM6DSO16IS_DEVICE_INIT(inst)
929
930 DT_INST_FOREACH_STATUS_OKAY(LSM6DSO16IS_DEFINE)
931
