1 /*
2 * Copyright (C) 2015-2017 Alibaba Group Holding Limited
3 *
4 *
5 */
6
7 #include <stdio.h>
8 #include <stdlib.h>
9 #include <string.h>
10
11 #include "ulog/ulog.h"
12 #include "aos/kernel.h"
13 #include "sensor_drv_api.h"
14 #include "sensor_hal.h"
15
16
17 #define LSM6DSL_I2C_ADDR1 (0x6A)
18 #define LSM6DSL_I2C_ADDR2 (0x6B)
19 #define LSM6DSL_I2C_ADDR_TRANS(n) ((n) << 1)
20 #define LSM6DSL_I2C_ADDR LSM6DSL_I2C_ADDR_TRANS(LSM6DSL_I2C_ADDR2)
21
22 #define LSM6DSL_ACC_GYRO_FUNC_CFG_ACCESS 0x01
23 #define LSM6DSL_ACC_GYRO_SENSOR_SYNC_TIME 0x04
24 #define LSM6DSL_ACC_GYRO_SENSOR_RES_RATIO 0x05
25 #define LSM6DSL_ACC_GYRO_FIFO_CTRL1 0x06
26 #define LSM6DSL_ACC_GYRO_FIFO_CTRL2 0x07
27 #define LSM6DSL_ACC_GYRO_FIFO_CTRL3 0x08
28 #define LSM6DSL_ACC_GYRO_FIFO_CTRL4 0x09
29 #define LSM6DSL_ACC_GYRO_FIFO_CTRL5 0x0A
30 #define LSM6DSL_ACC_GYRO_DRDY_PULSE_CFG_G 0x0B
31 #define LSM6DSL_ACC_GYRO_INT1_CTRL 0x0D
32 #define LSM6DSL_ACC_GYRO_INT2_CTRL 0x0E
33 #define LSM6DSL_ACC_GYRO_WHO_AM_I_REG 0x0F
34 #define LSM6DSL_ACC_GYRO_CTRL1_XL 0x10
35 #define LSM6DSL_ACC_GYRO_CTRL2_G 0x11
36 #define LSM6DSL_ACC_GYRO_CTRL3_C 0x12
37 #define LSM6DSL_ACC_GYRO_CTRL4_C 0x13
38 #define LSM6DSL_ACC_GYRO_CTRL5_C 0x14
39 #define LSM6DSL_ACC_GYRO_CTRL6_C 0x15
40 #define LSM6DSL_ACC_GYRO_CTRL7_G 0x16
41 #define LSM6DSL_ACC_GYRO_CTRL8_XL 0x17
42 #define LSM6DSL_ACC_GYRO_CTRL9_XL 0x18
43 #define LSM6DSL_ACC_GYRO_CTRL10_C 0x19
44
45
46 #define LSM6DSL_ACC_GYRO_MASTER_CONFIG 0x1A
47 #define LSM6DSL_ACC_GYRO_WAKE_UP_SRC 0x1B
48 #define LSM6DSL_ACC_GYRO_TAP_SRC 0x1C
49 #define LSM6DSL_ACC_GYRO_D6D_SRC 0x1D
50 #define LSM6DSL_ACC_GYRO_STATUS_REG 0x1E
51
52 #define LSM6DSL_ACC_GYRO_OUT_TEMP_L 0x20
53 #define LSM6DSL_ACC_GYRO_OUT_TEMP_H 0x21
54 #define LSM6DSL_ACC_GYRO_OUTX_L_G 0x22
55 #define LSM6DSL_ACC_GYRO_OUTX_H_G 0x23
56 #define LSM6DSL_ACC_GYRO_OUTY_L_G 0x24
57 #define LSM6DSL_ACC_GYRO_OUTY_H_G 0x25
58 #define LSM6DSL_ACC_GYRO_OUTZ_L_G 0x26
59 #define LSM6DSL_ACC_GYRO_OUTZ_H_G 0x27
60 #define LSM6DSL_ACC_GYRO_OUTX_L_XL 0x28
61 #define LSM6DSL_ACC_GYRO_OUTX_H_XL 0x29
62 #define LSM6DSL_ACC_GYRO_OUTY_L_XL 0x2A
63 #define LSM6DSL_ACC_GYRO_OUTY_H_XL 0x2B
64 #define LSM6DSL_ACC_GYRO_OUTZ_L_XL 0x2C
65 #define LSM6DSL_ACC_GYRO_OUTZ_H_XL 0x2D
66 #define LSM6DSL_ACC_GYRO_SENSORHUB1_REG 0x2E
67 #define LSM6DSL_ACC_GYRO_SENSORHUB2_REG 0x2F
68 #define LSM6DSL_ACC_GYRO_SENSORHUB3_REG 0x30
69 #define LSM6DSL_ACC_GYRO_SENSORHUB4_REG 0x31
70 #define LSM6DSL_ACC_GYRO_SENSORHUB5_REG 0x32
71 #define LSM6DSL_ACC_GYRO_SENSORHUB6_REG 0x33
72 #define LSM6DSL_ACC_GYRO_SENSORHUB7_REG 0x34
73 #define LSM6DSL_ACC_GYRO_SENSORHUB8_REG 0x35
74 #define LSM6DSL_ACC_GYRO_SENSORHUB9_REG 0x36
75 #define LSM6DSL_ACC_GYRO_SENSORHUB10_REG 0x37
76 #define LSM6DSL_ACC_GYRO_SENSORHUB11_REG 0x38
77 #define LSM6DSL_ACC_GYRO_SENSORHUB12_REG 0x39
78 #define LSM6DSL_ACC_GYRO_FIFO_STATUS1 0x3A
79 #define LSM6DSL_ACC_GYRO_FIFO_STATUS2 0x3B
80 #define LSM6DSL_ACC_GYRO_FIFO_STATUS3 0x3C
81 #define LSM6DSL_ACC_GYRO_FIFO_STATUS4 0x3D
82 #define LSM6DSL_ACC_GYRO_FIFO_DATA_OUT_L 0x3E
83 #define LSM6DSL_ACC_GYRO_FIFO_DATA_OUT_H 0x3F
84 #define LSM6DSL_ACC_GYRO_TIMESTAMP0_REG 0x40
85 #define LSM6DSL_ACC_GYRO_TIMESTAMP1_REG 0x41
86 #define LSM6DSL_ACC_GYRO_TIMESTAMP2_REG 0x42
87
88 #define LSM6DSL_ACC_GYRO_TIMESTAMP_L 0x49
89 #define LSM6DSL_ACC_GYRO_TIMESTAMP_H 0x4A
90
91 #define LSM6DSL_ACC_GYRO_STEP_COUNTER_L 0x4B
92 #define LSM6DSL_ACC_GYRO_STEP_COUNTER_H 0x4C
93
94 #define LSM6DSL_ACC_GYRO_SENSORHUB13_REG 0x4D
95 #define LSM6DSL_ACC_GYRO_SENSORHUB14_REG 0x4E
96 #define LSM6DSL_ACC_GYRO_SENSORHUB15_REG 0x4F
97 #define LSM6DSL_ACC_GYRO_SENSORHUB16_REG 0x50
98 #define LSM6DSL_ACC_GYRO_SENSORHUB17_REG 0x51
99 #define LSM6DSL_ACC_GYRO_SENSORHUB18_REG 0x52
100
101 #define LSM6DSL_ACC_GYRO_FUNC_SRC 0x53
102 #define LSM6DSL_ACC_GYRO_TAP_CFG1 0x58
103 #define LSM6DSL_ACC_GYRO_TAP_THS_6D 0x59
104 #define LSM6DSL_ACC_GYRO_INT_DUR2 0x5A
105 #define LSM6DSL_ACC_GYRO_WAKE_UP_THS 0x5B
106 #define LSM6DSL_ACC_GYRO_WAKE_UP_DUR 0x5C
107 #define LSM6DSL_ACC_GYRO_FREE_FALL 0x5D
108 #define LSM6DSL_ACC_GYRO_MD1_CFG 0x5E
109 #define LSM6DSL_ACC_GYRO_MD2_CFG 0x5F
110
111 #define LSM6DSL_ACC_GYRO_OUT_MAG_RAW_X_L 0x66
112 #define LSM6DSL_ACC_GYRO_OUT_MAG_RAW_X_H 0x67
113 #define LSM6DSL_ACC_GYRO_OUT_MAG_RAW_Y_L 0x68
114 #define LSM6DSL_ACC_GYRO_OUT_MAG_RAW_Y_H 0x69
115 #define LSM6DSL_ACC_GYRO_OUT_MAG_RAW_Z_L 0x6A
116 #define LSM6DSL_ACC_GYRO_OUT_MAG_RAW_Z_H 0x6B
117
118 #define LSM6DSL_ACC_GYRO_X_OFS_USR 0x73
119 #define LSM6DSL_ACC_GYRO_Y_OFS_USR 0x74
120 #define LSM6DSL_ACC_GYRO_Z_OFS_USR 0x75
121
122 #define LSM6DSL_CHIP_ID_VALUE (0x6A)
123
124 #define LSM6DSL_RESET_VALUE (0x1)
125 #define LSM6DSL_RESET_MSK (0X1)
126 #define LSM6DSL_RESET_POS (0)
127
128 #define LSM6DSL_ACC_ODR_POWER_DOWN (0X00)
129 #define LSM6DSL_ACC_ODR_1_6_HZ (0X0B)
130 #define LSM6DSL_ACC_ODR_12_5_HZ (0x01)
131 #define LSM6DSL_ACC_ODR_26_HZ (0x02)
132 #define LSM6DSL_ACC_ODR_52_HZ (0x03)
133 #define LSM6DSL_ACC_ODR_104_HZ (0x04)
134 #define LSM6DSL_ACC_ODR_208_HZ (0x05)
135 #define LSM6DSL_ACC_ODR_416_HZ (0x06)
136 #define LSM6DSL_ACC_ODR_833_HZ (0x07)
137 #define LSM6DSL_ACC_ODR_1_66_KHZ (0x08)
138 #define LSM6DSL_ACC_ODR_3_33_KHZ (0x09)
139 #define LSM6DSL_ACC_ODR_6_66_KHZ (0x0A)
140 #define LSM6DSL_ACC_ODR_MSK (0XF0)
141 #define LSM6DSL_ACC_ODR_POS (4)
142
143 #define LSM6DSL_GYRO_ODR_POWER_DOWN (0X00)
144 #define LSM6DSL_GYRO_ODR_12_5_HZ (0x01)
145 #define LSM6DSL_GYRO_ODR_26_HZ (0x02)
146 #define LSM6DSL_GYRO_ODR_52_HZ (0x03)
147 #define LSM6DSL_GYRO_ODR_104_HZ (0x04)
148 #define LSM6DSL_GYRO_ODR_208_HZ (0x05)
149 #define LSM6DSL_GYRO_ODR_416_HZ (0x06)
150 #define LSM6DSL_GYRO_ODR_833_HZ (0x07)
151 #define LSM6DSL_GYRO_ODR_1_66_KHZ (0x08)
152 #define LSM6DSL_GYRO_ODR_3_33_KHZ (0x09)
153 #define LSM6DSL_GYRO_ODR_6_66_KHZ (0x0A)
154 #define LSM6DSL_GYRO_ODR_MSK (0XF0)
155 #define LSM6DSL_GYRO_ODR_POS (4)
156
157 #define LSM6DSL_ACC_RANGE_2G (0x0)
158 #define LSM6DSL_ACC_RANGE_4G (0x2)
159 #define LSM6DSL_ACC_RANGE_8G (0x3)
160 #define LSM6DSL_ACC_RANGE_16G (0x1)
161 #define LSM6DSL_ACC_RANGE_MSK (0X0C)
162 #define LSM6DSL_ACC_RANGE_POS (2)
163
164 #define LSM6DSL_ACC_SENSITIVITY_2G (61)
165 #define LSM6DSL_ACC_SENSITIVITY_4G (122)
166 #define LSM6DSL_ACC_SENSITIVITY_8G (244)
167 #define LSM6DSL_ACC_SENSITIVITY_16G (488)
168
169 #define LSM6DSL_GYRO_RANGE_245 (0x0)
170 #define LSM6DSL_GYRO_RANGE_500 (0x1)
171 #define LSM6DSL_GYRO_RANGE_1000 (0x2)
172 #define LSM6DSL_GYRO_RANGE_2000 (0x3)
173 #define LSM6DSL_GYRO_RANGE_MSK (0X0C)
174 #define LSM6DSL_GYRO_RANGE_POS (2)
175
176 #define LSM6DSL_GYRO_SENSITIVITY_245DPS (8750)
177 #define LSM6DSL_GYRO_SENSITIVITY_500DPS (17500)
178 #define LSM6DSL_GYRO_SENSITIVITY_1000DPS (35000)
179 #define LSM6DSL_GYRO_SENSITIVITY_2000DPS (70000)
180
181 #define LSM6DSL_SHIFT_EIGHT_BITS (8)
182 #define LSM6DSL_16_BIT_SHIFT (0xFF)
183 #define LSM6DSL_ACC_MUL (1000)
184 #define LSM6DSL_GYRO_MUL (1)
185
186 #define LSM6DSL_ACC_DEFAULT_ODR_100HZ (100)
187 #define LSM6DSL_GYRO_DEFAULT_ODR_100HZ (100)
188
189 #define LSM6DSL_GET_BITSLICE(regvar, bitname) \
190 ((regvar & bitname##_MSK) >> bitname##_POS)
191
192 #define LSM6DSL_SET_BITSLICE(regvar, bitname, val) \
193 ((regvar & ~bitname##_MSK) | ((val << bitname##_POS) & bitname##_MSK))
194
195 static int32_t lsm6dsl_acc_factor[ACC_RANGE_MAX] = {
196 LSM6DSL_ACC_SENSITIVITY_2G, LSM6DSL_ACC_SENSITIVITY_4G,
197 LSM6DSL_ACC_SENSITIVITY_8G, LSM6DSL_ACC_SENSITIVITY_16G
198 };
199 static int32_t lsm6dsl_gyro_factor[GYRO_RANGE_MAX] = {
200 0, LSM6DSL_GYRO_SENSITIVITY_245DPS, LSM6DSL_GYRO_SENSITIVITY_500DPS,
201 LSM6DSL_GYRO_SENSITIVITY_1000DPS, LSM6DSL_GYRO_SENSITIVITY_2000DPS
202 };
203 static int32_t cur_acc_factor = 0;
204 static int32_t cur_gyro_factor = 0;
205 static int32_t g_lsm6dslflag = 0;
206
207 i2c_dev_t lsm6dsl_ctx = {
208 .port = 4,
209 .config.address_width = 8,
210 .config.freq = 400000,
211 .config.dev_addr = LSM6DSL_I2C_ADDR,
212 };
213
drv_acc_gyro_st_lsm6dsl_soft_reset(i2c_dev_t * drv)214 static int drv_acc_gyro_st_lsm6dsl_soft_reset(i2c_dev_t *drv)
215 {
216 int ret = 0;
217 uint8_t value = 0x00;
218
219 ret = sensor_i2c_read(drv, LSM6DSL_ACC_GYRO_CTRL3_C, &value, I2C_DATA_LEN,
220 I2C_OP_RETRIES);
221 if (unlikely(ret)) {
222 return ret;
223 }
224
225 value |= LSM6DSL_RESET_VALUE;
226
227 ret = sensor_i2c_write(drv, LSM6DSL_ACC_GYRO_CTRL3_C, &value, I2C_DATA_LEN,
228 I2C_OP_RETRIES);
229 if (unlikely(ret)) {
230 return -1;
231 }
232 return 0;
233 }
234
drv_acc_gyro_st_lsm6dsl_validate_id(i2c_dev_t * drv,uint8_t id_value)235 static int drv_acc_gyro_st_lsm6dsl_validate_id(i2c_dev_t *drv, uint8_t id_value)
236 {
237 uint8_t value = 0x00;
238 int ret = 0;
239
240 if (drv == NULL) {
241 return -1;
242 }
243
244 ret = sensor_i2c_read(drv, LSM6DSL_ACC_GYRO_WHO_AM_I_REG, &value,
245 I2C_DATA_LEN, I2C_OP_RETRIES);
246 if (unlikely(ret)) {
247 return ret;
248 }
249
250 if (id_value != value) {
251 return -1;
252 }
253
254 return 0;
255 }
256
drv_acc_st_lsm6dsl_set_power_mode(i2c_dev_t * drv,dev_power_mode_e mode)257 static int drv_acc_st_lsm6dsl_set_power_mode(i2c_dev_t * drv,
258 dev_power_mode_e mode)
259 {
260 uint8_t value = 0x00;
261 int ret = 0;
262
263 ret = sensor_i2c_read(drv, LSM6DSL_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN,
264 I2C_OP_RETRIES);
265 if (unlikely(ret)) {
266 return ret;
267 }
268
269 switch (mode) {
270 case DEV_POWER_ON: {
271
272 value = LSM6DSL_SET_BITSLICE(value, LSM6DSL_ACC_ODR,
273 LSM6DSL_ACC_ODR_12_5_HZ);
274 ret = sensor_i2c_write(drv, LSM6DSL_ACC_GYRO_CTRL1_XL, &value,
275 I2C_DATA_LEN, I2C_OP_RETRIES);
276 if (unlikely(ret)) {
277 return ret;
278 }
279 } break;
280
281 case DEV_POWER_OFF: {
282 value = LSM6DSL_SET_BITSLICE(value, LSM6DSL_ACC_ODR,
283 LSM6DSL_ACC_ODR_POWER_DOWN);
284 ret = sensor_i2c_write(drv, LSM6DSL_ACC_GYRO_CTRL1_XL, &value,
285 I2C_DATA_LEN, I2C_OP_RETRIES);
286 if (unlikely(ret)) {
287 return ret;
288 }
289 } break;
290
291 case DEV_SLEEP: {
292 value = LSM6DSL_SET_BITSLICE(value, LSM6DSL_ACC_ODR,
293 LSM6DSL_ACC_ODR_12_5_HZ);
294 ret = sensor_i2c_write(drv, LSM6DSL_ACC_GYRO_CTRL1_XL, &value,
295 I2C_DATA_LEN, I2C_OP_RETRIES);
296 if (unlikely(ret)) {
297 return ret;
298 }
299
300 } break;
301
302 default:
303 break;
304 }
305 return 0;
306 }
307
drv_acc_st_lsm6dsl_hz2odr(uint32_t hz)308 static uint8_t drv_acc_st_lsm6dsl_hz2odr(uint32_t hz)
309 {
310 if (hz > 3330)
311 return LSM6DSL_ACC_ODR_6_66_KHZ;
312 else if (hz > 1660)
313 return LSM6DSL_ACC_ODR_3_33_KHZ;
314 else if (hz > 833)
315 return LSM6DSL_ACC_ODR_1_66_KHZ;
316 else if (hz > 416)
317 return LSM6DSL_ACC_ODR_833_HZ;
318 else if (hz > 208)
319 return LSM6DSL_ACC_ODR_416_HZ;
320 else if (hz > 104)
321 return LSM6DSL_ACC_ODR_208_HZ;
322 else if (hz > 52)
323 return LSM6DSL_ACC_ODR_104_HZ;
324 else if (hz > 26)
325 return LSM6DSL_ACC_ODR_52_HZ;
326 else if (hz > 13)
327 return LSM6DSL_ACC_ODR_26_HZ;
328 else if (hz >= 2)
329 return LSM6DSL_ACC_ODR_12_5_HZ;
330 else
331 return LSM6DSL_ACC_ODR_1_6_HZ;
332 }
333
334
drv_acc_st_lsm6dsl_set_odr(i2c_dev_t * drv,uint32_t hz)335 static int drv_acc_st_lsm6dsl_set_odr(i2c_dev_t *drv, uint32_t hz)
336 {
337 int ret = 0;
338 uint8_t value = 0x00;
339 uint8_t odr = drv_acc_st_lsm6dsl_hz2odr(hz);
340
341 ret = sensor_i2c_read(drv, LSM6DSL_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN,
342 I2C_OP_RETRIES);
343 if (unlikely(ret)) {
344 return ret;
345 }
346
347 value = LSM6DSL_SET_BITSLICE(value, LSM6DSL_ACC_ODR, odr);
348
349 ret = sensor_i2c_write(drv, LSM6DSL_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN,
350 I2C_OP_RETRIES);
351 if (unlikely(ret)) {
352 return ret;
353 }
354 return 0;
355 }
356
drv_acc_st_lsm6dsl_set_range(i2c_dev_t * drv,uint32_t range)357 static int drv_acc_st_lsm6dsl_set_range(i2c_dev_t *drv, uint32_t range)
358 {
359 int ret = 0;
360 uint8_t value = 0x00;
361 uint8_t tmp = 0;
362
363 ret = sensor_i2c_read(drv, LSM6DSL_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN,
364 I2C_OP_RETRIES);
365 if (unlikely(ret)) {
366 return ret;
367 }
368
369 switch (range) {
370 case ACC_RANGE_2G: {
371 tmp = LSM6DSL_ACC_RANGE_2G;
372 } break;
373
374 case ACC_RANGE_4G: {
375 tmp = LSM6DSL_ACC_RANGE_4G;
376 } break;
377
378 case ACC_RANGE_8G: {
379 tmp = LSM6DSL_ACC_RANGE_8G;
380 } break;
381
382 case ACC_RANGE_16G: {
383 tmp = LSM6DSL_ACC_RANGE_16G;
384 } break;
385
386 default:
387 break;
388 }
389
390 value = LSM6DSL_SET_BITSLICE(value, LSM6DSL_ACC_RANGE, tmp);
391 ret = sensor_i2c_write(drv, LSM6DSL_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN,
392 I2C_OP_RETRIES);
393 if (unlikely(ret)) {
394 return ret;
395 }
396
397 if (range <= ACC_RANGE_16G) {
398 cur_acc_factor = lsm6dsl_acc_factor[range];
399 }
400
401 return 0;
402 }
403
404
drv_acc_st_lsm6dsl_irq_handle(void)405 static void drv_acc_st_lsm6dsl_irq_handle(void)
406 {
407 /* no handle so far */
408 }
409
drv_acc_st_lsm6dsl_open(void)410 static int drv_acc_st_lsm6dsl_open(void)
411 {
412 int ret = 0;
413
414 ret = drv_acc_st_lsm6dsl_set_power_mode(&lsm6dsl_ctx, DEV_POWER_ON);
415 if (unlikely(ret)) {
416 return -1;
417 }
418
419 ret = drv_acc_st_lsm6dsl_set_range(&lsm6dsl_ctx, ACC_RANGE_8G);
420 if (unlikely(ret)) {
421 return -1;
422 }
423
424 ret =
425 drv_acc_st_lsm6dsl_set_odr(&lsm6dsl_ctx, LSM6DSL_ACC_DEFAULT_ODR_100HZ);
426 if (unlikely(ret)) {
427 return -1;
428 }
429
430 return 0;
431 }
432
drv_acc_st_lsm6dsl_close(void)433 static int drv_acc_st_lsm6dsl_close(void)
434 {
435 int ret = 0;
436 ret = drv_acc_st_lsm6dsl_set_power_mode(&lsm6dsl_ctx, DEV_POWER_OFF);
437 if (unlikely(ret)) {
438 return -1;
439 }
440 return 0;
441 }
442
drv_acc_st_lsm6dsl_read(void * buf,size_t len)443 static int drv_acc_st_lsm6dsl_read(void *buf, size_t len)
444 {
445 int ret = 0;
446 size_t size;
447 uint8_t reg[6];
448 accel_data_t *accel = (accel_data_t *)buf;
449 if (buf == NULL) {
450 return -1;
451 }
452
453 size = sizeof(accel_data_t);
454 if (len < size) {
455 return -1;
456 }
457
458 ret = sensor_i2c_read(&lsm6dsl_ctx, LSM6DSL_ACC_GYRO_OUTX_L_XL, ®[0],
459 I2C_REG_LEN, I2C_OP_RETRIES);
460 ret |= sensor_i2c_read(&lsm6dsl_ctx, LSM6DSL_ACC_GYRO_OUTX_H_XL, ®[1],
461 I2C_REG_LEN, I2C_OP_RETRIES);
462 ret |= sensor_i2c_read(&lsm6dsl_ctx, LSM6DSL_ACC_GYRO_OUTY_L_XL, ®[2],
463 I2C_REG_LEN, I2C_OP_RETRIES);
464 ret |= sensor_i2c_read(&lsm6dsl_ctx, LSM6DSL_ACC_GYRO_OUTY_H_XL, ®[3],
465 I2C_REG_LEN, I2C_OP_RETRIES);
466 ret |= sensor_i2c_read(&lsm6dsl_ctx, LSM6DSL_ACC_GYRO_OUTZ_L_XL, ®[4],
467 I2C_REG_LEN, I2C_OP_RETRIES);
468 ret |= sensor_i2c_read(&lsm6dsl_ctx, LSM6DSL_ACC_GYRO_OUTZ_H_XL, ®[5],
469 I2C_REG_LEN, I2C_OP_RETRIES);
470 if (unlikely(ret)) {
471 return -1;
472 }
473 accel->data[DATA_AXIS_X] = (int16_t)(
474 (((int16_t)((int8_t)reg[1])) << LSM6DSL_SHIFT_EIGHT_BITS) | (reg[0]));
475 accel->data[DATA_AXIS_Y] = (int16_t)(
476 (((int16_t)((int8_t)reg[3])) << LSM6DSL_SHIFT_EIGHT_BITS) | (reg[2]));
477 accel->data[DATA_AXIS_Z] = (int16_t)(
478 (((int16_t)((int8_t)reg[5])) << LSM6DSL_SHIFT_EIGHT_BITS) | (reg[4]));
479
480 if (cur_acc_factor != 0) {
481 accel->data[DATA_AXIS_X] =
482 (accel->data[DATA_AXIS_X] * cur_acc_factor) / LSM6DSL_ACC_MUL;
483 accel->data[DATA_AXIS_Y] =
484 (accel->data[DATA_AXIS_Y] * cur_acc_factor) / LSM6DSL_ACC_MUL;
485 accel->data[DATA_AXIS_Z] =
486 (accel->data[DATA_AXIS_Z] * cur_acc_factor) / LSM6DSL_ACC_MUL;
487 }
488
489 accel->timestamp = aos_now_ms();
490
491 return (int)size;
492 }
493
drv_acc_st_lsm6dsl_ioctl(int cmd,unsigned long arg)494 static int drv_acc_st_lsm6dsl_ioctl(int cmd, unsigned long arg)
495 {
496 int ret = 0;
497
498 switch (cmd) {
499 case SENSOR_IOCTL_ODR_SET: {
500 ret = drv_acc_st_lsm6dsl_set_odr(&lsm6dsl_ctx, arg);
501 if (unlikely(ret)) {
502 return -1;
503 }
504 } break;
505 case SENSOR_IOCTL_RANGE_SET: {
506 ret = drv_acc_st_lsm6dsl_set_range(&lsm6dsl_ctx, arg);
507 if (unlikely(ret)) {
508 return -1;
509 }
510 } break;
511 case SENSOR_IOCTL_SET_POWER: {
512 ret = drv_acc_st_lsm6dsl_set_power_mode(&lsm6dsl_ctx, arg);
513 if (unlikely(ret)) {
514 return -1;
515 }
516 } break;
517 case SENSOR_IOCTL_GET_INFO: {
518 /* fill the dev info here */
519 dev_sensor_info_t *info = (dev_sensor_info_t *)arg;
520 info->model = "LSM6DSL";
521 info->range_max = 16;
522 info->range_min = 2;
523 info->unit = mg;
524 } break;
525
526 default:
527 break;
528 }
529
530 return 0;
531 }
532
533
drv_acc_st_lsm6dsl_init(void)534 int drv_acc_st_lsm6dsl_init(void)
535 {
536 int ret = 0;
537 sensor_obj_t sensor;
538
539 memset(&sensor, 0, sizeof(sensor));
540
541 /* fill the sensor obj parameters here */
542 sensor.io_port = I2C_PORT;
543 sensor.tag = TAG_DEV_ACC;
544 sensor.path = dev_acc_path;
545 sensor.open = drv_acc_st_lsm6dsl_open;
546 sensor.close = drv_acc_st_lsm6dsl_close;
547 sensor.read = drv_acc_st_lsm6dsl_read;
548 sensor.write = NULL;
549 sensor.ioctl = drv_acc_st_lsm6dsl_ioctl;
550 sensor.irq_handle = drv_acc_st_lsm6dsl_irq_handle;
551
552 ret = sensor_create_obj(&sensor);
553 if (unlikely(ret)) {
554 return -1;
555 }
556 ret =
557 drv_acc_gyro_st_lsm6dsl_validate_id(&lsm6dsl_ctx, LSM6DSL_CHIP_ID_VALUE);
558 if (unlikely(ret)) {
559 return -1;
560 }
561
562 if (0 == g_lsm6dslflag) {
563 ret = drv_acc_gyro_st_lsm6dsl_soft_reset(&lsm6dsl_ctx);
564 if (unlikely(ret)) {
565 return -1;
566 }
567 ret = drv_acc_st_lsm6dsl_set_power_mode(&lsm6dsl_ctx, DEV_POWER_OFF);
568 if (unlikely(ret)) {
569 return -1;
570 }
571 g_lsm6dslflag = 1;
572 } else {
573 LOG("%s %s acc do not need reset\n", SENSOR_STR, __func__);
574 }
575
576 LOG("%s %s successfully \n", SENSOR_STR, __func__);
577 return 0;
578 }
579
580
drv_gyro_st_lsm6dsl_set_power_mode(i2c_dev_t * drv,dev_power_mode_e mode)581 static int drv_gyro_st_lsm6dsl_set_power_mode(i2c_dev_t * drv,
582 dev_power_mode_e mode)
583 {
584 uint8_t value = 0x00;
585 int ret = 0;
586
587 ret = sensor_i2c_read(drv, LSM6DSL_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN,
588 I2C_OP_RETRIES);
589 if (unlikely(ret)) {
590 return ret;
591 }
592
593 switch (mode) {
594 case DEV_POWER_ON: {
595
596 value = LSM6DSL_SET_BITSLICE(value, LSM6DSL_GYRO_ODR,
597 LSM6DSL_GYRO_ODR_12_5_HZ);
598 ret = sensor_i2c_write(drv, LSM6DSL_ACC_GYRO_CTRL2_G, &value,
599 I2C_DATA_LEN, I2C_OP_RETRIES);
600 if (unlikely(ret)) {
601 return ret;
602 }
603 } break;
604
605 case DEV_POWER_OFF: {
606 value = LSM6DSL_SET_BITSLICE(value, LSM6DSL_GYRO_ODR,
607 LSM6DSL_GYRO_ODR_POWER_DOWN);
608 ret = sensor_i2c_write(drv, LSM6DSL_ACC_GYRO_CTRL2_G, &value,
609 I2C_DATA_LEN, I2C_OP_RETRIES);
610 if (unlikely(ret)) {
611 return ret;
612 }
613 } break;
614
615 case DEV_SLEEP: {
616 value = LSM6DSL_SET_BITSLICE(value, LSM6DSL_GYRO_ODR,
617 LSM6DSL_GYRO_ODR_12_5_HZ);
618 ret = sensor_i2c_write(drv, LSM6DSL_ACC_GYRO_CTRL2_G, &value,
619 I2C_DATA_LEN, I2C_OP_RETRIES);
620 if (unlikely(ret)) {
621 return ret;
622 }
623
624 } break;
625
626 default:
627 break;
628 }
629 return 0;
630 }
631
drv_gyro_st_lsm6dsl_hz2odr(uint32_t hz)632 static uint8_t drv_gyro_st_lsm6dsl_hz2odr(uint32_t hz)
633 {
634 if (hz > 3330)
635 return LSM6DSL_GYRO_ODR_6_66_KHZ;
636 else if (hz > 1660)
637 return LSM6DSL_GYRO_ODR_3_33_KHZ;
638 else if (hz > 833)
639 return LSM6DSL_GYRO_ODR_1_66_KHZ;
640 else if (hz > 416)
641 return LSM6DSL_GYRO_ODR_833_HZ;
642 else if (hz > 208)
643 return LSM6DSL_GYRO_ODR_416_HZ;
644 else if (hz > 104)
645 return LSM6DSL_GYRO_ODR_208_HZ;
646 else if (hz > 52)
647 return LSM6DSL_GYRO_ODR_104_HZ;
648 else if (hz > 26)
649 return LSM6DSL_GYRO_ODR_52_HZ;
650 else if (hz > 13)
651 return LSM6DSL_GYRO_ODR_26_HZ;
652 else
653 return LSM6DSL_GYRO_ODR_12_5_HZ;
654 }
655
656
drv_gyro_st_lsm6dsl_set_odr(i2c_dev_t * drv,uint32_t hz)657 static int drv_gyro_st_lsm6dsl_set_odr(i2c_dev_t *drv, uint32_t hz)
658 {
659 int ret = 0;
660 uint8_t value = 0x00;
661 uint8_t odr = drv_gyro_st_lsm6dsl_hz2odr(hz);
662
663 ret = sensor_i2c_read(drv, LSM6DSL_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN,
664 I2C_OP_RETRIES);
665 if (unlikely(ret)) {
666 return ret;
667 }
668
669 value = LSM6DSL_SET_BITSLICE(value, LSM6DSL_GYRO_ODR, odr);
670
671 ret = sensor_i2c_write(drv, LSM6DSL_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN,
672 I2C_OP_RETRIES);
673 if (unlikely(ret)) {
674 return ret;
675 }
676 return 0;
677 }
678
drv_gyro_st_lsm6dsl_set_range(i2c_dev_t * drv,uint32_t range)679 static int drv_gyro_st_lsm6dsl_set_range(i2c_dev_t *drv, uint32_t range)
680 {
681 int ret = 0;
682 uint8_t value = 0x00;
683 uint8_t tmp = 0;
684
685 ret = sensor_i2c_read(drv, LSM6DSL_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN,
686 I2C_OP_RETRIES);
687 if (unlikely(ret)) {
688 return ret;
689 }
690
691 switch (range) {
692 case GYRO_RANGE_250DPS: {
693 tmp = LSM6DSL_GYRO_RANGE_245;
694 } break;
695
696 case GYRO_RANGE_500DPS: {
697 tmp = LSM6DSL_GYRO_RANGE_500;
698 } break;
699
700 case GYRO_RANGE_1000DPS: {
701 tmp = LSM6DSL_GYRO_RANGE_1000;
702 } break;
703
704 case GYRO_RANGE_2000DPS: {
705 tmp = LSM6DSL_GYRO_RANGE_2000;
706 } break;
707
708 default:
709 break;
710 }
711
712 value = LSM6DSL_SET_BITSLICE(value, LSM6DSL_GYRO_RANGE, tmp);
713 ret = sensor_i2c_write(drv, LSM6DSL_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN,
714 I2C_OP_RETRIES);
715 if (unlikely(ret)) {
716 return ret;
717 }
718
719 if ((range >= GYRO_RANGE_250DPS) && (range <= GYRO_RANGE_2000DPS)) {
720 cur_gyro_factor = lsm6dsl_gyro_factor[range];
721 }
722
723 return 0;
724 }
725
726
drv_gyro_st_lsm6dsl_irq_handle(void)727 static void drv_gyro_st_lsm6dsl_irq_handle(void)
728 {
729 /* no handle so far */
730 }
731
drv_gyro_st_lsm6dsl_open(void)732 static int drv_gyro_st_lsm6dsl_open(void)
733 {
734 int ret = 0;
735 ret = drv_gyro_st_lsm6dsl_set_power_mode(&lsm6dsl_ctx, DEV_POWER_ON);
736 if (unlikely(ret)) {
737 return -1;
738 }
739
740 ret = drv_gyro_st_lsm6dsl_set_range(&lsm6dsl_ctx, GYRO_RANGE_1000DPS);
741 if (unlikely(ret)) {
742 return -1;
743 }
744
745 ret =
746 drv_gyro_st_lsm6dsl_set_odr(&lsm6dsl_ctx, LSM6DSL_GYRO_DEFAULT_ODR_100HZ);
747 if (unlikely(ret)) {
748 return -1;
749 }
750
751 return 0;
752 }
753
drv_gyro_st_lsm6dsl_close(void)754 static int drv_gyro_st_lsm6dsl_close(void)
755 {
756 int ret = 0;
757 ret = drv_gyro_st_lsm6dsl_set_power_mode(&lsm6dsl_ctx, DEV_POWER_OFF);
758 if (unlikely(ret)) {
759 return -1;
760 }
761 return 0;
762 }
763
drv_gyro_st_lsm6dsl_read(void * buf,size_t len)764 static int drv_gyro_st_lsm6dsl_read(void *buf, size_t len)
765 {
766 int ret = 0;
767 size_t size;
768 uint8_t reg[6];
769 gyro_data_t *gyro = (gyro_data_t *)buf;
770 if (buf == NULL) {
771 return -1;
772 }
773
774 size = sizeof(gyro_data_t);
775 if (len < size) {
776 return -1;
777 }
778
779 ret = sensor_i2c_read(&lsm6dsl_ctx, LSM6DSL_ACC_GYRO_OUTX_L_G, ®[0],
780 I2C_REG_LEN, I2C_OP_RETRIES);
781 ret |= sensor_i2c_read(&lsm6dsl_ctx, LSM6DSL_ACC_GYRO_OUTX_H_G, ®[1],
782 I2C_REG_LEN, I2C_OP_RETRIES);
783 ret |= sensor_i2c_read(&lsm6dsl_ctx, LSM6DSL_ACC_GYRO_OUTY_L_G, ®[2],
784 I2C_REG_LEN, I2C_OP_RETRIES);
785 ret |= sensor_i2c_read(&lsm6dsl_ctx, LSM6DSL_ACC_GYRO_OUTY_H_G, ®[3],
786 I2C_REG_LEN, I2C_OP_RETRIES);
787 ret |= sensor_i2c_read(&lsm6dsl_ctx, LSM6DSL_ACC_GYRO_OUTZ_L_G, ®[4],
788 I2C_REG_LEN, I2C_OP_RETRIES);
789 ret |= sensor_i2c_read(&lsm6dsl_ctx, LSM6DSL_ACC_GYRO_OUTZ_H_G, ®[5],
790 I2C_REG_LEN, I2C_OP_RETRIES);
791 if (unlikely(ret)) {
792 return -1;
793 }
794 gyro->data[DATA_AXIS_X] = (int16_t)(
795 (((int32_t)((int8_t)reg[1])) << LSM6DSL_SHIFT_EIGHT_BITS) | (reg[0]));
796 gyro->data[DATA_AXIS_Y] = (int16_t)(
797 (((int32_t)((int8_t)reg[3])) << LSM6DSL_SHIFT_EIGHT_BITS) | (reg[2]));
798 gyro->data[DATA_AXIS_Z] = (int16_t)(
799 (((int32_t)((int8_t)reg[5])) << LSM6DSL_SHIFT_EIGHT_BITS) | (reg[4]));
800
801 if (cur_gyro_factor != 0) {
802 gyro->data[DATA_AXIS_X] =
803 (gyro->data[DATA_AXIS_X] * cur_gyro_factor) / LSM6DSL_GYRO_MUL;
804 gyro->data[DATA_AXIS_Y] =
805 (gyro->data[DATA_AXIS_Y] * cur_gyro_factor) / LSM6DSL_GYRO_MUL;
806 gyro->data[DATA_AXIS_Z] =
807 (gyro->data[DATA_AXIS_Z] * cur_gyro_factor) / LSM6DSL_GYRO_MUL;
808 }
809 gyro->timestamp = aos_now_ms();
810
811 return (int)size;
812 }
813
drv_gyro_st_lsm6dsl_ioctl(int cmd,unsigned long arg)814 static int drv_gyro_st_lsm6dsl_ioctl(int cmd, unsigned long arg)
815 {
816 int ret = 0;
817
818 switch (cmd) {
819 case SENSOR_IOCTL_ODR_SET: {
820 ret = drv_gyro_st_lsm6dsl_set_odr(&lsm6dsl_ctx, arg);
821 if (unlikely(ret)) {
822 return -1;
823 }
824 } break;
825 case SENSOR_IOCTL_RANGE_SET: {
826 ret = drv_gyro_st_lsm6dsl_set_range(&lsm6dsl_ctx, arg);
827 if (unlikely(ret)) {
828 return -1;
829 }
830 } break;
831 case SENSOR_IOCTL_SET_POWER: {
832 ret = drv_gyro_st_lsm6dsl_set_power_mode(&lsm6dsl_ctx, arg);
833 if (unlikely(ret)) {
834 return -1;
835 }
836 } break;
837 case SENSOR_IOCTL_GET_INFO: {
838 /* fill the dev info here */
839 dev_sensor_info_t *info = (dev_sensor_info_t *)arg;
840 info->model = "LSM6DSL";
841 info->range_max = 2000;
842 info->range_min = 125;
843 info->unit = udps;
844 } break;
845
846 default:
847 break;
848 }
849
850 return 0;
851 }
852
drv_gyro_st_lsm6dsl_init(void)853 int drv_gyro_st_lsm6dsl_init(void)
854 {
855 int ret = 0;
856 sensor_obj_t sensor;
857
858 memset(&sensor, 0, sizeof(sensor));
859
860 /* fill the sensor obj parameters here */
861 sensor.io_port = I2C_PORT;
862 sensor.tag = TAG_DEV_GYRO;
863 sensor.path = dev_gyro_path;
864 sensor.open = drv_gyro_st_lsm6dsl_open;
865 sensor.close = drv_gyro_st_lsm6dsl_close;
866 sensor.read = drv_gyro_st_lsm6dsl_read;
867 sensor.write = NULL;
868 sensor.ioctl = drv_gyro_st_lsm6dsl_ioctl;
869 sensor.irq_handle = drv_gyro_st_lsm6dsl_irq_handle;
870
871 ret = sensor_create_obj(&sensor);
872 if (unlikely(ret)) {
873 return -1;
874 }
875
876 ret =
877 drv_acc_gyro_st_lsm6dsl_validate_id(&lsm6dsl_ctx, LSM6DSL_CHIP_ID_VALUE);
878 if (unlikely(ret)) {
879 return -1;
880 }
881
882 if (0 == g_lsm6dslflag) {
883 ret = drv_acc_gyro_st_lsm6dsl_soft_reset(&lsm6dsl_ctx);
884 if (unlikely(ret)) {
885 return -1;
886 }
887
888 ret = drv_gyro_st_lsm6dsl_set_power_mode(&lsm6dsl_ctx, DEV_POWER_OFF);
889 if (unlikely(ret)) {
890 return -1;
891 }
892 g_lsm6dslflag = 1;
893 } else {
894 LOG("%s %s gyro do not need reset\n", SENSOR_STR, __func__);
895 }
896
897 /* update the phy sensor info to sensor hal */
898 LOG("%s %s successfully \n", SENSOR_STR, __func__);
899 return 0;
900 }
901 SENSOR_DRV_ADD(drv_acc_st_lsm6dsl_init);
902 SENSOR_DRV_ADD(drv_gyro_st_lsm6dsl_init);
903