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