1 /* lsm6ds0.c - Driver for LSM6DS0 accelerometer, gyroscope and
2 * temperature sensor
3 */
4
5 /*
6 * Copyright (c) 2016 Intel Corporation
7 *
8 * SPDX-License-Identifier: Apache-2.0
9 */
10
11 #define DT_DRV_COMPAT st_lsm6ds0
12
13 #include <zephyr/drivers/sensor.h>
14 #include <zephyr/kernel.h>
15 #include <zephyr/device.h>
16 #include <zephyr/init.h>
17 #include <zephyr/sys/byteorder.h>
18 #include <zephyr/sys/__assert.h>
19 #include <zephyr/logging/log.h>
20
21 #include "lsm6ds0.h"
22
23 LOG_MODULE_REGISTER(LSM6DS0, CONFIG_SENSOR_LOG_LEVEL);
24
lsm6ds0_reboot(const struct device * dev)25 static inline int lsm6ds0_reboot(const struct device *dev)
26 {
27 const struct lsm6ds0_config *config = dev->config;
28
29 if (i2c_reg_update_byte_dt(&config->i2c, LSM6DS0_REG_CTRL_REG8,
30 LSM6DS0_MASK_CTRL_REG8_BOOT,
31 1 << LSM6DS0_SHIFT_CTRL_REG8_BOOT) < 0) {
32 return -EIO;
33 }
34
35 k_busy_wait(USEC_PER_MSEC * 50U);
36
37 return 0;
38 }
39
lsm6ds0_accel_axis_ctrl(const struct device * dev,int x_en,int y_en,int z_en)40 static inline int lsm6ds0_accel_axis_ctrl(const struct device *dev, int x_en,
41 int y_en, int z_en)
42 {
43 const struct lsm6ds0_config *config = dev->config;
44 uint8_t state = (x_en << LSM6DS0_SHIFT_CTRL_REG5_XL_XEN_XL) |
45 (y_en << LSM6DS0_SHIFT_CTRL_REG5_XL_YEN_XL) |
46 (z_en << LSM6DS0_SHIFT_CTRL_REG5_XL_ZEN_XL);
47
48 return i2c_reg_update_byte_dt(&config->i2c, LSM6DS0_REG_CTRL_REG5_XL,
49 LSM6DS0_MASK_CTRL_REG5_XL_XEN_XL |
50 LSM6DS0_MASK_CTRL_REG5_XL_YEN_XL |
51 LSM6DS0_MASK_CTRL_REG5_XL_ZEN_XL,
52 state);
53 }
54
lsm6ds0_accel_set_fs_raw(const struct device * dev,uint8_t fs)55 static int lsm6ds0_accel_set_fs_raw(const struct device *dev, uint8_t fs)
56 {
57 const struct lsm6ds0_config *config = dev->config;
58
59 if (i2c_reg_update_byte_dt(&config->i2c, LSM6DS0_REG_CTRL_REG6_XL,
60 LSM6DS0_MASK_CTRL_REG6_XL_FS_XL,
61 fs << LSM6DS0_SHIFT_CTRL_REG6_XL_FS_XL) < 0) {
62 return -EIO;
63 }
64
65 return 0;
66 }
67
lsm6ds0_accel_set_odr_raw(const struct device * dev,uint8_t odr)68 static int lsm6ds0_accel_set_odr_raw(const struct device *dev, uint8_t odr)
69 {
70 const struct lsm6ds0_config *config = dev->config;
71
72 if (i2c_reg_update_byte_dt(&config->i2c, LSM6DS0_REG_CTRL_REG6_XL,
73 LSM6DS0_MASK_CTRL_REG6_XL_ODR_XL,
74 odr << LSM6DS0_SHIFT_CTRL_REG6_XL_ODR_XL) < 0) {
75 return -EIO;
76 }
77
78 return 0;
79 }
80
lsm6ds0_gyro_axis_ctrl(const struct device * dev,int x_en,int y_en,int z_en)81 static inline int lsm6ds0_gyro_axis_ctrl(const struct device *dev, int x_en,
82 int y_en,
83 int z_en)
84 {
85 const struct lsm6ds0_config *config = dev->config;
86 uint8_t state = (x_en << LSM6DS0_SHIFT_CTRL_REG4_XEN_G) |
87 (y_en << LSM6DS0_SHIFT_CTRL_REG4_YEN_G) |
88 (z_en << LSM6DS0_SHIFT_CTRL_REG4_ZEN_G);
89
90 return i2c_reg_update_byte_dt(&config->i2c, LSM6DS0_REG_CTRL_REG4,
91 LSM6DS0_MASK_CTRL_REG4_XEN_G |
92 LSM6DS0_MASK_CTRL_REG4_YEN_G |
93 LSM6DS0_MASK_CTRL_REG4_ZEN_G,
94 state);
95 }
96
lsm6ds0_gyro_set_fs_raw(const struct device * dev,uint8_t fs)97 static int lsm6ds0_gyro_set_fs_raw(const struct device *dev, uint8_t fs)
98 {
99 const struct lsm6ds0_config *config = dev->config;
100
101 if (i2c_reg_update_byte_dt(&config->i2c, LSM6DS0_REG_CTRL_REG1_G,
102 LSM6DS0_MASK_CTRL_REG1_G_FS_G,
103 fs << LSM6DS0_SHIFT_CTRL_REG1_G_FS_G) < 0) {
104 return -EIO;
105 }
106
107 return 0;
108 }
109
lsm6ds0_gyro_set_odr_raw(const struct device * dev,uint8_t odr)110 static int lsm6ds0_gyro_set_odr_raw(const struct device *dev, uint8_t odr)
111 {
112 const struct lsm6ds0_config *config = dev->config;
113
114 if (i2c_reg_update_byte_dt(&config->i2c, LSM6DS0_REG_CTRL_REG1_G,
115 LSM6DS0_MASK_CTRL_REG1_G_ODR_G,
116 odr << LSM6DS0_SHIFT_CTRL_REG1_G_ODR_G) < 0) {
117 return -EIO;
118 }
119
120 return 0;
121 }
122
lsm6ds0_sample_fetch_accel(const struct device * dev)123 static int lsm6ds0_sample_fetch_accel(const struct device *dev)
124 {
125 struct lsm6ds0_data *data = dev->data;
126 const struct lsm6ds0_config *config = dev->config;
127 uint8_t buf[6];
128
129 if (i2c_burst_read_dt(&config->i2c, LSM6DS0_REG_OUT_X_L_XL, buf, sizeof(buf)) < 0) {
130 LOG_DBG("failed to read sample");
131 return -EIO;
132 }
133
134 #if defined(CONFIG_LSM6DS0_ACCEL_ENABLE_X_AXIS)
135 data->accel_sample_x = (int16_t)((uint16_t)(buf[0]) |
136 ((uint16_t)(buf[1]) << 8));
137 #endif
138 #if defined(CONFIG_LSM6DS0_ACCEL_ENABLE_Y_AXIS)
139 data->accel_sample_y = (int16_t)((uint16_t)(buf[2]) |
140 ((uint16_t)(buf[3]) << 8));
141 #endif
142 #if defined(CONFIG_LSM6DS0_ACCEL_ENABLE_Z_AXIS)
143 data->accel_sample_z = (int16_t)((uint16_t)(buf[4]) |
144 ((uint16_t)(buf[5]) << 8));
145 #endif
146
147 return 0;
148 }
149
lsm6ds0_sample_fetch_gyro(const struct device * dev)150 static int lsm6ds0_sample_fetch_gyro(const struct device *dev)
151 {
152 struct lsm6ds0_data *data = dev->data;
153 const struct lsm6ds0_config *config = dev->config;
154 uint8_t buf[6];
155
156 if (i2c_burst_read_dt(&config->i2c, LSM6DS0_REG_OUT_X_L_G, buf, sizeof(buf)) < 0) {
157 LOG_DBG("failed to read sample");
158 return -EIO;
159 }
160
161 #if defined(CONFIG_LSM6DS0_GYRO_ENABLE_X_AXIS)
162 data->gyro_sample_x = (int16_t)((uint16_t)(buf[0]) |
163 ((uint16_t)(buf[1]) << 8));
164 #endif
165 #if defined(CONFIG_LSM6DS0_GYRO_ENABLE_Y_AXIS)
166 data->gyro_sample_y = (int16_t)((uint16_t)(buf[2]) |
167 ((uint16_t)(buf[3]) << 8));
168 #endif
169 #if defined(CONFIG_LSM6DS0_GYRO_ENABLE_Z_AXIS)
170 data->gyro_sample_z = (int16_t)((uint16_t)(buf[4]) |
171 ((uint16_t)(buf[5]) << 8));
172 #endif
173
174 return 0;
175 }
176
177 #if defined(CONFIG_LSM6DS0_ENABLE_TEMP)
lsm6ds0_sample_fetch_temp(const struct device * dev)178 static int lsm6ds0_sample_fetch_temp(const struct device *dev)
179 {
180 struct lsm6ds0_data *data = dev->data;
181 const struct lsm6ds0_config *config = dev->config;
182 uint8_t buf[2];
183
184 if (i2c_burst_read_dt(&config->i2c, LSM6DS0_REG_OUT_TEMP_L, buf, sizeof(buf)) < 0) {
185 LOG_DBG("failed to read sample");
186 return -EIO;
187 }
188
189 data->temp_sample = (int16_t)((uint16_t)(buf[0]) |
190 ((uint16_t)(buf[1]) << 8));
191
192 return 0;
193 }
194 #endif
195
lsm6ds0_sample_fetch(const struct device * dev,enum sensor_channel chan)196 static int lsm6ds0_sample_fetch(const struct device *dev,
197 enum sensor_channel chan)
198 {
199 __ASSERT_NO_MSG(chan == SENSOR_CHAN_ALL ||
200 chan == SENSOR_CHAN_ACCEL_XYZ ||
201 #if defined(CONFIG_LSM6DS0_ENABLE_TEMP)
202 chan == SENSOR_CHAN_DIE_TEMP ||
203 #endif
204 chan == SENSOR_CHAN_GYRO_XYZ);
205
206 switch (chan) {
207 case SENSOR_CHAN_ACCEL_XYZ:
208 lsm6ds0_sample_fetch_accel(dev);
209 break;
210 case SENSOR_CHAN_GYRO_XYZ:
211 lsm6ds0_sample_fetch_gyro(dev);
212 break;
213 #if defined(CONFIG_LSM6DS0_ENABLE_TEMP)
214 case SENSOR_CHAN_DIE_TEMP:
215 lsm6ds0_sample_fetch_temp(dev);
216 break;
217 #endif
218 case SENSOR_CHAN_ALL:
219 lsm6ds0_sample_fetch_accel(dev);
220 lsm6ds0_sample_fetch_gyro(dev);
221 #if defined(CONFIG_LSM6DS0_ENABLE_TEMP)
222 lsm6ds0_sample_fetch_temp(dev);
223 #endif
224 break;
225 default:
226 return -ENOTSUP;
227 }
228
229 return 0;
230 }
231
lsm6ds0_accel_convert(struct sensor_value * val,int raw_val,float scale)232 static inline void lsm6ds0_accel_convert(struct sensor_value *val, int raw_val,
233 float scale)
234 {
235 double dval;
236
237 dval = (double)(raw_val) * (double)scale / 32767.0;
238 val->val1 = (int32_t)dval;
239 val->val2 = ((int32_t)(dval * 1000000)) % 1000000;
240 }
241
lsm6ds0_accel_get_channel(enum sensor_channel chan,struct sensor_value * val,struct lsm6ds0_data * data,float scale)242 static inline int lsm6ds0_accel_get_channel(enum sensor_channel chan,
243 struct sensor_value *val,
244 struct lsm6ds0_data *data,
245 float scale)
246 {
247 switch (chan) {
248 #if defined(CONFIG_LSM6DS0_ACCEL_ENABLE_X_AXIS)
249 case SENSOR_CHAN_ACCEL_X:
250 lsm6ds0_accel_convert(val, data->accel_sample_x, scale);
251 break;
252 #endif
253 #if defined(CONFIG_LSM6DS0_ACCEL_ENABLE_Y_AXIS)
254 case SENSOR_CHAN_ACCEL_Y:
255 lsm6ds0_accel_convert(val, data->accel_sample_y, scale);
256 break;
257 #endif
258 #if defined(CONFIG_LSM6DS0_ACCEL_ENABLE_Z_AXIS)
259 case SENSOR_CHAN_ACCEL_Z:
260 lsm6ds0_accel_convert(val, data->accel_sample_z, scale);
261 break;
262 #endif
263 case SENSOR_CHAN_ACCEL_XYZ:
264 #if defined(CONFIG_LSM6DS0_ACCEL_ENABLE_X_AXIS)
265 lsm6ds0_accel_convert(val, data->accel_sample_x, scale);
266 #endif
267 #if defined(CONFIG_LSM6DS0_ACCEL_ENABLE_Y_AXIS)
268 lsm6ds0_accel_convert(val + 1, data->accel_sample_y, scale);
269 #endif
270 #if defined(CONFIG_LSM6DS0_ACCEL_ENABLE_Z_AXIS)
271 lsm6ds0_accel_convert(val + 2, data->accel_sample_z, scale);
272 #endif
273 break;
274 default:
275 return -ENOTSUP;
276 }
277
278 return 0;
279 }
280
lsm6ds0_accel_channel_get(enum sensor_channel chan,struct sensor_value * val,struct lsm6ds0_data * data)281 static int lsm6ds0_accel_channel_get(enum sensor_channel chan,
282 struct sensor_value *val,
283 struct lsm6ds0_data *data)
284 {
285 return lsm6ds0_accel_get_channel(chan, val, data,
286 LSM6DS0_DEFAULT_ACCEL_FULLSCALE_FACTOR);
287 }
288
lsm6ds0_gyro_convert(struct sensor_value * val,int raw_val,float numerator)289 static inline void lsm6ds0_gyro_convert(struct sensor_value *val, int raw_val,
290 float numerator)
291 {
292 double dval;
293
294 dval = (double)(raw_val) * (double)numerator / 1000.0 * SENSOR_DEG2RAD_DOUBLE;
295 val->val1 = (int32_t)dval;
296 val->val2 = ((int32_t)(dval * 1000000)) % 1000000;
297 }
298
lsm6ds0_gyro_get_channel(enum sensor_channel chan,struct sensor_value * val,struct lsm6ds0_data * data,float numerator)299 static inline int lsm6ds0_gyro_get_channel(enum sensor_channel chan,
300 struct sensor_value *val,
301 struct lsm6ds0_data *data,
302 float numerator)
303 {
304 switch (chan) {
305 #if defined(CONFIG_LSM6DS0_GYRO_ENABLE_X_AXIS)
306 case SENSOR_CHAN_GYRO_X:
307 lsm6ds0_gyro_convert(val, data->gyro_sample_x, numerator);
308 break;
309 #endif
310 #if defined(CONFIG_LSM6DS0_GYRO_ENABLE_Y_AXIS)
311 case SENSOR_CHAN_GYRO_Y:
312 lsm6ds0_gyro_convert(val, data->gyro_sample_y, numerator);
313 break;
314 #endif
315 #if defined(CONFIG_LSM6DS0_GYRO_ENABLE_Z_AXIS)
316 case SENSOR_CHAN_GYRO_Z:
317 lsm6ds0_gyro_convert(val, data->gyro_sample_z, numerator);
318 break;
319 #endif
320 case SENSOR_CHAN_GYRO_XYZ:
321 #if defined(CONFIG_LSM6DS0_GYRO_ENABLE_X_AXIS)
322 lsm6ds0_gyro_convert(val, data->gyro_sample_x, numerator);
323 #endif
324 #if defined(CONFIG_LSM6DS0_GYRO_ENABLE_Y_AXIS)
325 lsm6ds0_gyro_convert(val + 1, data->gyro_sample_y, numerator);
326 #endif
327 #if defined(CONFIG_LSM6DS0_GYRO_ENABLE_Z_AXIS)
328 lsm6ds0_gyro_convert(val + 2, data->gyro_sample_z, numerator);
329 #endif
330 break;
331 default:
332 return -ENOTSUP;
333 }
334
335 return 0;
336 }
337
lsm6ds0_gyro_channel_get(enum sensor_channel chan,struct sensor_value * val,struct lsm6ds0_data * data)338 static int lsm6ds0_gyro_channel_get(enum sensor_channel chan,
339 struct sensor_value *val,
340 struct lsm6ds0_data *data)
341 {
342 return lsm6ds0_gyro_get_channel(chan, val, data,
343 LSM6DS0_DEFAULT_GYRO_FULLSCALE_FACTOR);
344 }
345
346 #if defined(CONFIG_LSM6DS0_ENABLE_TEMP)
lsm6ds0_gyro_channel_get_temp(struct sensor_value * val,struct lsm6ds0_data * data)347 static void lsm6ds0_gyro_channel_get_temp(struct sensor_value *val,
348 struct lsm6ds0_data *data)
349 {
350 /* val = temp_sample / 16 + 25 */
351 val->val1 = data->temp_sample / 16 + 25;
352 val->val2 = (data->temp_sample % 16) * (1000000 / 16);
353 }
354 #endif
355
lsm6ds0_channel_get(const struct device * dev,enum sensor_channel chan,struct sensor_value * val)356 static int lsm6ds0_channel_get(const struct device *dev,
357 enum sensor_channel chan,
358 struct sensor_value *val)
359 {
360 struct lsm6ds0_data *data = dev->data;
361
362 switch (chan) {
363 case SENSOR_CHAN_ACCEL_X:
364 case SENSOR_CHAN_ACCEL_Y:
365 case SENSOR_CHAN_ACCEL_Z:
366 case SENSOR_CHAN_ACCEL_XYZ:
367 lsm6ds0_accel_channel_get(chan, val, data);
368 break;
369 case SENSOR_CHAN_GYRO_X:
370 case SENSOR_CHAN_GYRO_Y:
371 case SENSOR_CHAN_GYRO_Z:
372 case SENSOR_CHAN_GYRO_XYZ:
373 lsm6ds0_gyro_channel_get(chan, val, data);
374 break;
375 #if defined(CONFIG_LSM6DS0_ENABLE_TEMP)
376 case SENSOR_CHAN_DIE_TEMP:
377 lsm6ds0_gyro_channel_get_temp(val, data);
378 break;
379 #endif
380 default:
381 return -ENOTSUP;
382 }
383
384 return 0;
385 }
386
387 static DEVICE_API(sensor, lsm6ds0_api_funcs) = {
388 .sample_fetch = lsm6ds0_sample_fetch,
389 .channel_get = lsm6ds0_channel_get,
390 };
391
lsm6ds0_init_chip(const struct device * dev)392 static int lsm6ds0_init_chip(const struct device *dev)
393 {
394 const struct lsm6ds0_config *config = dev->config;
395 uint8_t chip_id;
396
397 if (lsm6ds0_reboot(dev) < 0) {
398 LOG_DBG("failed to reboot device");
399 return -EIO;
400 }
401
402 if (i2c_reg_read_byte_dt(&config->i2c, LSM6DS0_REG_WHO_AM_I, &chip_id) < 0) {
403 LOG_DBG("failed reading chip id");
404 return -EIO;
405 }
406 if (chip_id != LSM6DS0_VAL_WHO_AM_I) {
407 LOG_DBG("invalid chip id 0x%x", chip_id);
408 return -EIO;
409 }
410 LOG_DBG("chip id 0x%x", chip_id);
411
412 if (lsm6ds0_accel_axis_ctrl(dev, LSM6DS0_ACCEL_ENABLE_X_AXIS,
413 LSM6DS0_ACCEL_ENABLE_Y_AXIS,
414 LSM6DS0_ACCEL_ENABLE_Z_AXIS) < 0) {
415 LOG_DBG("failed to set accelerometer axis");
416 return -EIO;
417 }
418
419 if (lsm6ds0_accel_set_fs_raw(dev, LSM6DS0_DEFAULT_ACCEL_FULLSCALE)
420 < 0) {
421 LOG_DBG("failed to set accelerometer full-scale");
422 return -EIO;
423 }
424
425 if (lsm6ds0_accel_set_odr_raw(dev, LSM6DS0_DEFAULT_ACCEL_SAMPLING_RATE)
426 < 0) {
427 LOG_DBG("failed to set accelerometer sampling rate");
428 return -EIO;
429 }
430
431 if (lsm6ds0_gyro_axis_ctrl(dev, LSM6DS0_GYRO_ENABLE_X_AXIS,
432 LSM6DS0_GYRO_ENABLE_Y_AXIS,
433 LSM6DS0_GYRO_ENABLE_Z_AXIS) < 0) {
434 LOG_DBG("failed to set gyroscope axis");
435 return -EIO;
436 }
437
438 if (lsm6ds0_gyro_set_fs_raw(dev, LSM6DS0_DEFAULT_GYRO_FULLSCALE)
439 < 0) {
440 LOG_DBG("failed to set gyroscope full-scale");
441 return -EIO;
442 }
443
444 if (lsm6ds0_gyro_set_odr_raw(dev, LSM6DS0_DEFAULT_GYRO_SAMPLING_RATE)
445 < 0) {
446 LOG_DBG("failed to set gyroscope sampling rate");
447 return -EIO;
448 }
449
450 if (i2c_reg_update_byte_dt(&config->i2c, LSM6DS0_REG_CTRL_REG8,
451 LSM6DS0_MASK_CTRL_REG8_BDU |
452 LSM6DS0_MASK_CTRL_REG8_BLE |
453 LSM6DS0_MASK_CTRL_REG8_IF_ADD_INC,
454 (1 << LSM6DS0_SHIFT_CTRL_REG8_BDU) |
455 (0 << LSM6DS0_SHIFT_CTRL_REG8_BLE) |
456 (1 << LSM6DS0_SHIFT_CTRL_REG8_IF_ADD_INC))
457 < 0) {
458 LOG_DBG("failed to set BDU, BLE and burst");
459 return -EIO;
460 }
461
462 return 0;
463 }
464
lsm6ds0_init(const struct device * dev)465 static int lsm6ds0_init(const struct device *dev)
466 {
467 const struct lsm6ds0_config * const config = dev->config;
468
469 if (!device_is_ready(config->i2c.bus)) {
470 LOG_ERR("I2C bus device not ready");
471 return -ENODEV;
472 }
473
474 if (lsm6ds0_init_chip(dev) < 0) {
475 LOG_DBG("failed to initialize chip");
476 return -EIO;
477 }
478
479 return 0;
480 }
481
482 #define LSM6DS0_DEFINE(inst) \
483 static struct lsm6ds0_data lsm6ds0_data_##inst; \
484 \
485 static const struct lsm6ds0_config lsm6ds0_config_##inst = { \
486 .i2c = I2C_DT_SPEC_INST_GET(inst), \
487 }; \
488 \
489 SENSOR_DEVICE_DT_INST_DEFINE(inst, lsm6ds0_init, NULL, \
490 &lsm6ds0_data_##inst, &lsm6ds0_config_##inst, POST_KERNEL,\
491 CONFIG_SENSOR_INIT_PRIORITY, &lsm6ds0_api_funcs); \
492
493 DT_INST_FOREACH_STATUS_OKAY(LSM6DS0_DEFINE)
494
