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 #include "aos/kernel.h"
11 #include "sensor_drv_api.h"
12 #include "sensor_hal.h"
13
14
15 #define LSM6DSR_I2C_ADDR1 (0x6A)
16 #define LSM6DSR_I2C_ADDR2 (0x6B)
17 #define LSM6DSR_I2C_ADDR_TRANS(n) ((n)<<1)
18 #define LSM6DSR_I2C_ADDR LSM6DSR_I2C_ADDR_TRANS(LSM6DSR_I2C_ADDR2)
19
20 #define LSM6DSR_ACC_GYRO_FUNC_CFG_ACCESS 0x01
21 #define LSM6DSR_ACC_GYRO_PIN_CTRL 0x02
22 #define LSM6DSR_ACC_GYRO_FIFO_CTRL1 0x07
23 #define LSM6DSR_ACC_GYRO_FIFO_CTRL2 0x08
24 #define LSM6DSR_ACC_GYRO_FIFO_CTRL3 0x09
25 #define LSM6DSR_ACC_GYRO_FIFO_CTRL4 0x0A
26 #define LSM6DSR_ACC_GYRO_COUNTER_BDR_REG1 0x0B
27 #define LSM6DSR_ACC_GYRO_COUNTER_BDR_REG2 0x0C
28 #define LSM6DSR_ACC_GYRO_INT1_CTRL 0x0D
29 #define LSM6DSR_ACC_GYRO_INT2_CTRL 0x0E
30 #define LSM6DSR_ACC_GYRO_WHO_AM_I_REG 0x0F
31 #define LSM6DSR_ACC_GYRO_CTRL1_XL 0x10
32 #define LSM6DSR_ACC_GYRO_CTRL2_G 0x11
33 #define LSM6DSR_ACC_GYRO_CTRL3_C 0x12
34 #define LSM6DSR_ACC_GYRO_CTRL4_C 0x13
35 #define LSM6DSR_ACC_GYRO_CTRL5_C 0x14
36 #define LSM6DSR_ACC_GYRO_CTRL6_C 0x15
37 #define LSM6DSR_ACC_GYRO_CTRL7_G 0x16
38 #define LSM6DSR_ACC_GYRO_CTRL8_XL 0x17
39 #define LSM6DSR_ACC_GYRO_CTRL9_XL 0x18
40 #define LSM6DSR_ACC_GYRO_CTRL10_C 0x19
41
42 #define LSM6DSR_ACC_GYRO_ALL_INT_SRC 0x1A
43 #define LSM6DSR_ACC_GYRO_WAKE_UP_SRC 0x1B
44 #define LSM6DSR_ACC_GYRO_TAP_SRC 0x1C
45 #define LSM6DSR_ACC_GYRO_D6D_SRC 0x1D
46 #define LSM6DSR_ACC_GYRO_STATUS_REG 0x1E
47
48 #define LSM6DSR_ACC_GYRO_OUT_TEMP_L 0x20
49 #define LSM6DSR_ACC_GYRO_OUT_TEMP_H 0x21
50 #define LSM6DSR_ACC_GYRO_OUTX_L_G 0x22
51 #define LSM6DSR_ACC_GYRO_OUTX_H_G 0x23
52 #define LSM6DSR_ACC_GYRO_OUTY_L_G 0x24
53 #define LSM6DSR_ACC_GYRO_OUTY_H_G 0x25
54 #define LSM6DSR_ACC_GYRO_OUTZ_L_G 0x26
55 #define LSM6DSR_ACC_GYRO_OUTZ_H_G 0x27
56 #define LSM6DSR_ACC_GYRO_OUTX_L_XL 0x28
57 #define LSM6DSR_ACC_GYRO_OUTX_H_XL 0x29
58 #define LSM6DSR_ACC_GYRO_OUTY_L_XL 0x2A
59 #define LSM6DSR_ACC_GYRO_OUTY_H_XL 0x2B
60 #define LSM6DSR_ACC_GYRO_OUTZ_L_XL 0x2C
61 #define LSM6DSR_ACC_GYRO_OUTZ_H_XL 0x2D
62
63 #define LSM6DSR_ACC_GYRO_FIFO_STATUS1 0x3A
64 #define LSM6DSR_ACC_GYRO_FIFO_STATUS2 0x3B
65
66 #define LSM6DSR_ACC_GYRO_TIMESTAMP0_REG 0x40
67 #define LSM6DSR_ACC_GYRO_TIMESTAMP1_REG 0x41
68 #define LSM6DSR_ACC_GYRO_TIMESTAMP2_REG 0x42
69 #define LSM6DSR_ACC_GYRO_TIMESTAMP3_REG 0x43
70
71 #define LSM6DSR_ACC_GYRO_TAP_CFG0 0x56
72 #define LSM6DSR_ACC_GYRO_TAP_CFG1 0x57
73 #define LSM6DSR_ACC_GYRO_TAP_CFG2 0x58
74 #define LSM6DSR_ACC_GYRO_TAP_THS_6D 0x59
75 #define LSM6DSR_ACC_GYRO_INT_DUR2 0x5A
76 #define LSM6DSR_ACC_GYRO_WAKE_UP_THS 0x5B
77 #define LSM6DSR_ACC_GYRO_WAKE_UP_DUR 0x5C
78 #define LSM6DSR_ACC_GYRO_FREE_FALL 0x5D
79 #define LSM6DSR_ACC_GYRO_MD1_CFG 0x5E
80 #define LSM6DSR_ACC_GYRO_MD2_CFG 0x5F
81
82 #define LSM6DSR_ACC_GYRO_X_OFS_USR 0x73
83 #define LSM6DSR_ACC_GYRO_Y_OFS_USR 0x74
84 #define LSM6DSR_ACC_GYRO_Z_OFS_USR 0x75
85
86 #define LSM6DSR_CHIP_ID_VALUE (0x6B)
87
88 #define LSM6DSR_RESET_VALUE (0x1)
89 #define LSM6DSR_RESET_MSK (0X1)
90 #define LSM6DSR_RESET_POS (0)
91
92 #define LSM6DSR_BDU_VALUE (0x40)
93 #define LSM6DSR_BDU_MSK (0X40)
94 #define LSM6DSR_BDU_POS (6)
95
96 #define LSM6DSR_ACC_ODR_POWER_DOWN (0X00)
97 #define LSM6DSR_ACC_ODR_1_6_HZ (0X0B)
98 #define LSM6DSR_ACC_ODR_12_5_HZ (0x01)
99 #define LSM6DSR_ACC_ODR_26_HZ (0x02)
100 #define LSM6DSR_ACC_ODR_52_HZ (0x03)
101 #define LSM6DSR_ACC_ODR_104_HZ (0x04)
102 #define LSM6DSR_ACC_ODR_208_HZ (0x05)
103 #define LSM6DSR_ACC_ODR_416_HZ (0x06)
104 #define LSM6DSR_ACC_ODR_833_HZ (0x07)
105 #define LSM6DSR_ACC_ODR_1_66_KHZ (0x08)
106 #define LSM6DSR_ACC_ODR_3_33_KHZ (0x09)
107 #define LSM6DSR_ACC_ODR_6_66_KHZ (0x0A)
108 #define LSM6DSR_ACC_ODR_MSK (0XF0)
109 #define LSM6DSR_ACC_ODR_POS (4)
110
111 #define LSM6DSR_GYRO_ODR_POWER_DOWN (0X00)
112 #define LSM6DSR_GYRO_ODR_12_5_HZ (0x01)
113 #define LSM6DSR_GYRO_ODR_26_HZ (0x02)
114 #define LSM6DSR_GYRO_ODR_52_HZ (0x03)
115 #define LSM6DSR_GYRO_ODR_104_HZ (0x04)
116 #define LSM6DSR_GYRO_ODR_208_HZ (0x05)
117 #define LSM6DSR_GYRO_ODR_416_HZ (0x06)
118 #define LSM6DSR_GYRO_ODR_833_HZ (0x07)
119 #define LSM6DSR_GYRO_ODR_1_66_KHZ (0x08)
120 #define LSM6DSR_GYRO_ODR_3_33_KHZ (0x09)
121 #define LSM6DSR_GYRO_ODR_6_66_KHZ (0x0A)
122 #define LSM6DSR_GYRO_ODR_MSK (0XF0)
123 #define LSM6DSR_GYRO_ODR_POS (4)
124
125 #define LSM6DSR_ACC_RANGE_2G (0x0)
126 #define LSM6DSR_ACC_RANGE_4G (0x2)
127 #define LSM6DSR_ACC_RANGE_8G (0x3)
128 #define LSM6DSR_ACC_RANGE_16G (0x1)
129 #define LSM6DSR_ACC_RANGE_MSK (0X0C)
130 #define LSM6DSR_ACC_RANGE_POS (2)
131
132 #define LSM6DSR_ACC_SENSITIVITY_2G (61)
133 #define LSM6DSR_ACC_SENSITIVITY_4G (122)
134 #define LSM6DSR_ACC_SENSITIVITY_8G (244)
135 #define LSM6DSR_ACC_SENSITIVITY_16G (488)
136
137 #define LSM6DSR_GYRO_RANGE_245 (0x0)
138 #define LSM6DSR_GYRO_RANGE_500 (0x1)
139 #define LSM6DSR_GYRO_RANGE_1000 (0x2)
140 #define LSM6DSR_GYRO_RANGE_2000 (0x3)
141 #define LSM6DSR_GYRO_RANGE_MSK (0X0C)
142 #define LSM6DSR_GYRO_RANGE_POS (2)
143
144 #define LSM6DSR_GYRO_SENSITIVITY_245DPS (8750)
145 #define LSM6DSR_GYRO_SENSITIVITY_500DPS (17500)
146 #define LSM6DSR_GYRO_SENSITIVITY_1000DPS (35000)
147 #define LSM6DSR_GYRO_SENSITIVITY_2000DPS (70000)
148
149 #define LSM6DSR_SHIFT_EIGHT_BITS (8)
150 #define LSM6DSR_16_BIT_SHIFT (0xFF)
151 #define LSM6DSR_ACC_MUL (1000)
152 #define LSM6DSR_GYRO_MUL (1)
153
154 #define LSM6DSR_ACC_DEFAULT_ODR_100HZ (100)
155 #define LSM6DSR_GYRO_DEFAULT_ODR_100HZ (100)
156
157 #define LSM6DSR_GET_BITSLICE(regvar, bitname)\
158 ((regvar & bitname##_MSK) >> bitname##_POS)
159
160 #define LSM6DSR_SET_BITSLICE(regvar, bitname, val)\
161 ((regvar & ~bitname##_MSK) | ((val<<bitname##_POS)&bitname##_MSK))
162
163 static int32_t lsm6dsr_acc_factor[ACC_RANGE_MAX] = { LSM6DSR_ACC_SENSITIVITY_2G, LSM6DSR_ACC_SENSITIVITY_4G,
164 LSM6DSR_ACC_SENSITIVITY_8G, LSM6DSR_ACC_SENSITIVITY_16G };
165 static int32_t lsm6dsr_gyro_factor[GYRO_RANGE_MAX] = {0, LSM6DSR_GYRO_SENSITIVITY_245DPS, LSM6DSR_GYRO_SENSITIVITY_500DPS,
166 LSM6DSR_GYRO_SENSITIVITY_1000DPS, LSM6DSR_GYRO_SENSITIVITY_2000DPS };
167 static int32_t cur_acc_factor = 0;
168 static int32_t cur_gyro_factor = 0;
169 static int32_t g_lsm6dsrflag = 0;
170
171 i2c_dev_t lsm6dsr_ctx = {
172 //.port = 4,
173 .port = 3,
174 .config.address_width = 8,
175 .config.freq = 400000,
176 .config.dev_addr = LSM6DSR_I2C_ADDR,
177 };
178
drv_acc_gyro_st_lsm6dsr_soft_reset(i2c_dev_t * drv)179 static int drv_acc_gyro_st_lsm6dsr_soft_reset(i2c_dev_t* drv)
180 {
181 int ret = 0;
182 uint8_t value = 0x00;
183
184 ret = sensor_i2c_read(drv, LSM6DSR_ACC_GYRO_CTRL3_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
185 if(unlikely(ret)){
186 return ret;
187 }
188
189 value |= LSM6DSR_RESET_VALUE;
190
191 ret = sensor_i2c_write(drv, LSM6DSR_ACC_GYRO_CTRL3_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
192 if(unlikely(ret)){
193 return -1;
194 }
195 return 0;
196 }
197
drv_acc_gyro_st_lsm6dsr_validate_id(i2c_dev_t * drv,uint8_t id_value)198 static int drv_acc_gyro_st_lsm6dsr_validate_id(i2c_dev_t* drv, uint8_t id_value)
199 {
200 uint8_t value = 0x00;
201 int ret = 0;
202
203 if(drv == NULL){
204 return -1;
205 }
206
207 ret = sensor_i2c_read(drv, LSM6DSR_ACC_GYRO_WHO_AM_I_REG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
208 if(unlikely(ret)){
209 return ret;
210 }
211
212 LOG("%s %s right id (0x%02x), read id(0x%02x)\n", SENSOR_STR, __func__, id_value, value);
213 if (id_value != value){
214 return -1;
215 }
216
217 return 0;
218 }
219
drv_acc_st_lsm6dsr_set_power_mode(i2c_dev_t * drv,dev_power_mode_e mode)220 static int drv_acc_st_lsm6dsr_set_power_mode(i2c_dev_t* drv, dev_power_mode_e mode)
221 {
222 uint8_t value = 0x00;
223 int ret = 0;
224
225 ret = sensor_i2c_read(drv, LSM6DSR_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
226 if(unlikely(ret)){
227 return ret;
228 }
229
230 switch(mode){
231 case DEV_POWER_ON:{
232
233 value = LSM6DSR_SET_BITSLICE(value,LSM6DSR_ACC_ODR,LSM6DSR_ACC_ODR_12_5_HZ);
234 ret = sensor_i2c_write(drv, LSM6DSR_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
235 if(unlikely(ret)){
236 return ret;
237 }
238 }break;
239
240 case DEV_POWER_OFF:{
241 value = LSM6DSR_SET_BITSLICE(value,LSM6DSR_ACC_ODR,LSM6DSR_ACC_ODR_POWER_DOWN);
242 ret = sensor_i2c_write(drv, LSM6DSR_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
243 if(unlikely(ret)){
244 return ret;
245 }
246 }break;
247
248 case DEV_SLEEP:{
249 value = LSM6DSR_SET_BITSLICE(value,LSM6DSR_ACC_ODR,LSM6DSR_ACC_ODR_12_5_HZ);
250 ret = sensor_i2c_write(drv, LSM6DSR_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
251 if(unlikely(ret)){
252 return ret;
253 }
254
255 }break;
256
257 default:break;
258 }
259 return 0;
260 }
261
drv_acc_gyro_st_lsm6dsr_set_bdu(i2c_dev_t * drv)262 static int drv_acc_gyro_st_lsm6dsr_set_bdu(i2c_dev_t* drv)
263 {
264 int ret = 0;
265 uint8_t value = 0x00;
266
267 ret = sensor_i2c_read(drv, LSM6DSR_ACC_GYRO_CTRL3_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
268 if(unlikely(ret)){
269 return ret;
270 }
271
272 if (value & LSM6DSR_BDU_VALUE)
273 return 0;
274
275 value |= LSM6DSR_BDU_VALUE;
276
277 ret = sensor_i2c_write(drv, LSM6DSR_ACC_GYRO_CTRL3_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
278 if(unlikely(ret)){
279 return -1;
280 }
281 return 0;
282 }
283
drv_acc_st_lsm6dsr_hz2odr(uint32_t hz)284 static uint8_t drv_acc_st_lsm6dsr_hz2odr(uint32_t hz)
285 {
286 if(hz > 3330)
287 return LSM6DSR_ACC_ODR_6_66_KHZ;
288 else if(hz > 1660)
289 return LSM6DSR_ACC_ODR_3_33_KHZ;
290 else if(hz > 833)
291 return LSM6DSR_ACC_ODR_1_66_KHZ;
292 else if(hz > 416)
293 return LSM6DSR_ACC_ODR_833_HZ;
294 else if(hz > 208)
295 return LSM6DSR_ACC_ODR_416_HZ;
296 else if(hz > 104)
297 return LSM6DSR_ACC_ODR_208_HZ;
298 else if(hz > 52)
299 return LSM6DSR_ACC_ODR_104_HZ;
300 else if(hz > 26)
301 return LSM6DSR_ACC_ODR_52_HZ;
302 else if(hz > 13)
303 return LSM6DSR_ACC_ODR_26_HZ;
304 else if(hz >= 2)
305 return LSM6DSR_ACC_ODR_12_5_HZ;
306 else
307 return LSM6DSR_ACC_ODR_1_6_HZ;
308
309 }
310
311
drv_acc_st_lsm6dsr_set_odr(i2c_dev_t * drv,uint32_t hz)312 static int drv_acc_st_lsm6dsr_set_odr(i2c_dev_t* drv, uint32_t hz)
313 {
314 int ret = 0;
315 uint8_t value = 0x00;
316 uint8_t odr = drv_acc_st_lsm6dsr_hz2odr(hz);
317
318 ret = sensor_i2c_read(drv, LSM6DSR_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
319 if(unlikely(ret)){
320 return ret;
321 }
322
323 value = LSM6DSR_SET_BITSLICE(value,LSM6DSR_ACC_ODR,odr);
324
325 ret = sensor_i2c_write(drv, LSM6DSR_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
326 if(unlikely(ret)){
327 return ret;
328 }
329 return 0;
330 }
331
drv_acc_st_lsm6dsr_set_range(i2c_dev_t * drv,uint32_t range)332 static int drv_acc_st_lsm6dsr_set_range(i2c_dev_t* drv, uint32_t range)
333 {
334 int ret = 0;
335 uint8_t value = 0x00;
336 uint8_t tmp = 0;
337
338 ret = sensor_i2c_read(drv, LSM6DSR_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
339 if(unlikely(ret)){
340 return ret;
341 }
342
343 switch (range){
344 case ACC_RANGE_2G:{
345 tmp = LSM6DSR_ACC_RANGE_2G;
346 }break;
347
348 case ACC_RANGE_4G:{
349 tmp = LSM6DSR_ACC_RANGE_4G;
350 }break;
351
352 case ACC_RANGE_8G:{
353 tmp = LSM6DSR_ACC_RANGE_8G;
354 }break;
355
356 case ACC_RANGE_16G:{
357 tmp = LSM6DSR_ACC_RANGE_16G;
358 }break;
359
360 default:break;
361 }
362
363 value = LSM6DSR_SET_BITSLICE(value,LSM6DSR_ACC_RANGE,tmp);
364 ret = sensor_i2c_write(drv, LSM6DSR_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
365 if(unlikely(ret)){
366 return ret;
367 }
368
369 if((range >= ACC_RANGE_2G)&&(range <= ACC_RANGE_16G)){
370 cur_acc_factor = lsm6dsr_acc_factor[range];
371 }
372
373 return 0;
374 }
375
376
drv_acc_st_lsm6dsr_irq_handle(void)377 static void drv_acc_st_lsm6dsr_irq_handle(void)
378 {
379 /* no handle so far */
380 }
381
drv_acc_st_lsm6dsr_open(void)382 static int drv_acc_st_lsm6dsr_open(void)
383 {
384 int ret = 0;
385
386 ret = drv_acc_st_lsm6dsr_set_power_mode(&lsm6dsr_ctx, DEV_POWER_ON);
387 if(unlikely(ret)){
388 return -1;
389 }
390
391 ret = drv_acc_gyro_st_lsm6dsr_set_bdu(&lsm6dsr_ctx);
392 if(unlikely(ret)){
393 return -1;
394 }
395
396 ret = drv_acc_st_lsm6dsr_set_range(&lsm6dsr_ctx, ACC_RANGE_8G);
397 if(unlikely(ret)){
398 return -1;
399 }
400
401 ret = drv_acc_st_lsm6dsr_set_odr(&lsm6dsr_ctx, LSM6DSR_ACC_DEFAULT_ODR_100HZ);
402 if(unlikely(ret)){
403 return -1;
404 }
405
406 return 0;
407
408 }
409
drv_acc_st_lsm6dsr_close(void)410 static int drv_acc_st_lsm6dsr_close(void)
411 {
412 int ret = 0;
413 ret = drv_acc_st_lsm6dsr_set_power_mode(&lsm6dsr_ctx, DEV_POWER_OFF);
414 if(unlikely(ret)){
415 return -1;
416 }
417 return 0;
418 }
419
drv_acc_st_lsm6dsr_read(void * buf,size_t len)420 static int drv_acc_st_lsm6dsr_read(void *buf, size_t len)
421 {
422 int ret = 0;
423 size_t size;
424 uint8_t reg[6];
425 accel_data_t *accel = (accel_data_t *)buf;
426 if(buf == NULL){
427 return -1;
428 }
429
430 size = sizeof(accel_data_t);
431 if(len < size){
432 return -1;
433 }
434
435 ret = sensor_i2c_read(&lsm6dsr_ctx, LSM6DSR_ACC_GYRO_OUTX_L_XL, ®[0], I2C_REG_LEN, I2C_OP_RETRIES);
436 ret |= sensor_i2c_read(&lsm6dsr_ctx, LSM6DSR_ACC_GYRO_OUTX_H_XL, ®[1], I2C_REG_LEN, I2C_OP_RETRIES);
437 ret |= sensor_i2c_read(&lsm6dsr_ctx, LSM6DSR_ACC_GYRO_OUTY_L_XL, ®[2], I2C_REG_LEN, I2C_OP_RETRIES);
438 ret |= sensor_i2c_read(&lsm6dsr_ctx, LSM6DSR_ACC_GYRO_OUTY_H_XL, ®[3], I2C_REG_LEN, I2C_OP_RETRIES);
439 ret |= sensor_i2c_read(&lsm6dsr_ctx, LSM6DSR_ACC_GYRO_OUTZ_L_XL, ®[4], I2C_REG_LEN, I2C_OP_RETRIES);
440 ret |= sensor_i2c_read(&lsm6dsr_ctx, LSM6DSR_ACC_GYRO_OUTZ_H_XL, ®[5], I2C_REG_LEN, I2C_OP_RETRIES);
441 if(unlikely(ret)){
442 return -1;
443 }
444 accel->data[DATA_AXIS_X] = (int16_t)((((int16_t)((int8_t)reg[1]))<< LSM6DSR_SHIFT_EIGHT_BITS)|(reg[0]));
445 accel->data[DATA_AXIS_Y] = (int16_t)((((int16_t)((int8_t)reg[3]))<< LSM6DSR_SHIFT_EIGHT_BITS)|(reg[2]));
446 accel->data[DATA_AXIS_Z] = (int16_t)((((int16_t)((int8_t)reg[5]))<< LSM6DSR_SHIFT_EIGHT_BITS)|(reg[4]));
447
448 if(cur_acc_factor != 0){
449 accel->data[DATA_AXIS_X] = (accel->data[DATA_AXIS_X] * cur_acc_factor)/LSM6DSR_ACC_MUL;
450 accel->data[DATA_AXIS_Y] = (accel->data[DATA_AXIS_Y] * cur_acc_factor)/LSM6DSR_ACC_MUL;
451 accel->data[DATA_AXIS_Z] = (accel->data[DATA_AXIS_Z] * cur_acc_factor)/LSM6DSR_ACC_MUL;
452 }
453
454 accel->timestamp = aos_now_ms();
455
456 return (int)size;
457 }
458
drv_acc_st_lsm6dsr_ioctl(int cmd,unsigned long arg)459 static int drv_acc_st_lsm6dsr_ioctl(int cmd, unsigned long arg)
460 {
461 int ret = 0;
462
463 switch(cmd){
464 case SENSOR_IOCTL_ODR_SET:{
465 ret = drv_acc_st_lsm6dsr_set_odr(&lsm6dsr_ctx, arg);
466 if(unlikely(ret)){
467 return -1;
468 }
469 }break;
470 case SENSOR_IOCTL_RANGE_SET:{
471 ret = drv_acc_st_lsm6dsr_set_range(&lsm6dsr_ctx, arg);
472 if(unlikely(ret)){
473 return -1;
474 }
475 }break;
476 case SENSOR_IOCTL_SET_POWER:{
477 ret = drv_acc_st_lsm6dsr_set_power_mode(&lsm6dsr_ctx, arg);
478 if(unlikely(ret)){
479 return -1;
480 }
481 }break;
482 case SENSOR_IOCTL_GET_INFO:{
483 /* fill the dev info here */
484 dev_sensor_info_t *info = (dev_sensor_info_t*)arg;
485 info->model = "LSM6DSR";
486 info->range_max = 16;
487 info->range_min = 2;
488 info->unit = mg;
489 }break;
490
491 default:break;
492 }
493
494 return 0;
495 }
496
497
drv_acc_st_lsm6dsr_init(void)498 int drv_acc_st_lsm6dsr_init(void){
499 int ret = 0;
500 sensor_obj_t sensor;
501 memset(&sensor, 0, sizeof(sensor));
502
503 /* fill the sensor obj parameters here */
504 sensor.io_port = I2C_PORT;
505 sensor.tag = TAG_DEV_ACC;
506 sensor.path = dev_acc_path;
507 sensor.open = drv_acc_st_lsm6dsr_open;
508 sensor.close = drv_acc_st_lsm6dsr_close;
509 sensor.read = drv_acc_st_lsm6dsr_read;
510 sensor.write = NULL;
511 sensor.ioctl = drv_acc_st_lsm6dsr_ioctl;
512 sensor.irq_handle = drv_acc_st_lsm6dsr_irq_handle;
513
514 ret = sensor_create_obj(&sensor);
515 if(unlikely(ret)){
516 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
517 return -1;
518 }
519 ret = drv_acc_gyro_st_lsm6dsr_validate_id(&lsm6dsr_ctx, LSM6DSR_CHIP_ID_VALUE);
520 if(unlikely(ret)){
521 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
522 return -1;
523 }
524
525 if(0 == g_lsm6dsrflag)
526 {
527 ret = drv_acc_gyro_st_lsm6dsr_soft_reset(&lsm6dsr_ctx);
528 if(unlikely(ret)){
529 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
530 return -1;
531 }
532 ret = drv_acc_st_lsm6dsr_set_power_mode(&lsm6dsr_ctx, DEV_POWER_OFF);
533 if(unlikely(ret)){
534 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
535 return -1;
536 }
537 g_lsm6dsrflag = 1;
538 }
539 else
540 {
541 LOG("%s %s acc do not need reset\n", SENSOR_STR, __func__);
542 }
543
544 LOG("%s %s successfully \n", SENSOR_STR, __func__);
545 return 0;
546 }
547
548
drv_gyro_st_lsm6dsr_set_power_mode(i2c_dev_t * drv,dev_power_mode_e mode)549 static int drv_gyro_st_lsm6dsr_set_power_mode(i2c_dev_t* drv, dev_power_mode_e mode)
550 {
551 uint8_t value = 0x00;
552 int ret = 0;
553
554 ret = sensor_i2c_read(drv, LSM6DSR_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
555 if(unlikely(ret)){
556 return ret;
557 }
558
559 switch(mode){
560 case DEV_POWER_ON:{
561
562 value = LSM6DSR_SET_BITSLICE(value,LSM6DSR_GYRO_ODR,LSM6DSR_GYRO_ODR_12_5_HZ);
563 ret = sensor_i2c_write(drv, LSM6DSR_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
564 if(unlikely(ret)){
565 return ret;
566 }
567 }break;
568
569 case DEV_POWER_OFF:{
570 value = LSM6DSR_SET_BITSLICE(value,LSM6DSR_GYRO_ODR,LSM6DSR_GYRO_ODR_POWER_DOWN);
571 ret = sensor_i2c_write(drv, LSM6DSR_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
572 if(unlikely(ret)){
573 return ret;
574 }
575 }break;
576
577 case DEV_SLEEP:{
578 value = LSM6DSR_SET_BITSLICE(value,LSM6DSR_GYRO_ODR,LSM6DSR_GYRO_ODR_12_5_HZ);
579 ret = sensor_i2c_write(drv, LSM6DSR_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
580 if(unlikely(ret)){
581 return ret;
582 }
583
584 }break;
585
586 default:break;
587 }
588 return 0;
589 }
590
drv_gyro_st_lsm6dsr_hz2odr(uint32_t hz)591 static uint8_t drv_gyro_st_lsm6dsr_hz2odr(uint32_t hz)
592 {
593 if(hz > 3330)
594 return LSM6DSR_GYRO_ODR_6_66_KHZ;
595 else if(hz > 1660)
596 return LSM6DSR_GYRO_ODR_3_33_KHZ;
597 else if(hz > 833)
598 return LSM6DSR_GYRO_ODR_1_66_KHZ;
599 else if(hz > 416)
600 return LSM6DSR_GYRO_ODR_833_HZ;
601 else if(hz > 208)
602 return LSM6DSR_GYRO_ODR_416_HZ;
603 else if(hz > 104)
604 return LSM6DSR_GYRO_ODR_208_HZ;
605 else if(hz > 52)
606 return LSM6DSR_GYRO_ODR_104_HZ;
607 else if(hz > 26)
608 return LSM6DSR_GYRO_ODR_52_HZ;
609 else if(hz > 13)
610 return LSM6DSR_GYRO_ODR_26_HZ;
611 else
612 return LSM6DSR_GYRO_ODR_12_5_HZ;
613
614 }
615
616
drv_gyro_st_lsm6dsr_set_odr(i2c_dev_t * drv,uint32_t hz)617 static int drv_gyro_st_lsm6dsr_set_odr(i2c_dev_t* drv, uint32_t hz)
618 {
619 int ret = 0;
620 uint8_t value = 0x00;
621 uint8_t odr = drv_gyro_st_lsm6dsr_hz2odr(hz);
622
623 ret = sensor_i2c_read(drv, LSM6DSR_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
624 if(unlikely(ret)){
625 return ret;
626 }
627
628 value = LSM6DSR_SET_BITSLICE(value,LSM6DSR_GYRO_ODR,odr);
629
630 ret = sensor_i2c_write(drv, LSM6DSR_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
631 if(unlikely(ret)){
632 return ret;
633 }
634 return 0;
635 }
636
drv_gyro_st_lsm6dsr_set_range(i2c_dev_t * drv,uint32_t range)637 static int drv_gyro_st_lsm6dsr_set_range(i2c_dev_t* drv, uint32_t range)
638 {
639 int ret = 0;
640 uint8_t value = 0x00;
641 uint8_t tmp = 0;
642
643 ret = sensor_i2c_read(drv, LSM6DSR_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
644 if(unlikely(ret)){
645 return ret;
646 }
647
648 switch (range){
649 case GYRO_RANGE_250DPS:{
650 tmp = LSM6DSR_GYRO_RANGE_245;
651 }break;
652
653 case GYRO_RANGE_500DPS:{
654 tmp = LSM6DSR_GYRO_RANGE_500;
655 }break;
656
657 case GYRO_RANGE_1000DPS:{
658 tmp = LSM6DSR_GYRO_RANGE_1000;
659 }break;
660
661 case GYRO_RANGE_2000DPS:{
662 tmp = LSM6DSR_GYRO_RANGE_2000;
663 }break;
664
665 default:break;
666 }
667
668 value = LSM6DSR_SET_BITSLICE(value,LSM6DSR_GYRO_RANGE,tmp);
669 ret = sensor_i2c_write(drv, LSM6DSR_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
670 if(unlikely(ret)){
671 return ret;
672 }
673
674 if((range >= GYRO_RANGE_250DPS)&&(range <= GYRO_RANGE_2000DPS)){
675 cur_gyro_factor = lsm6dsr_gyro_factor[range];
676 }
677
678 return 0;
679 }
680
681
drv_gyro_st_lsm6dsr_irq_handle(void)682 static void drv_gyro_st_lsm6dsr_irq_handle(void)
683 {
684 /* no handle so far */
685 }
686
drv_gyro_st_lsm6dsr_open(void)687 static int drv_gyro_st_lsm6dsr_open(void)
688 {
689 int ret = 0;
690 ret = drv_gyro_st_lsm6dsr_set_power_mode(&lsm6dsr_ctx, DEV_POWER_ON);
691 if(unlikely(ret)){
692 return -1;
693 }
694
695 ret = drv_acc_gyro_st_lsm6dsr_set_bdu(&lsm6dsr_ctx);
696 if(unlikely(ret)){
697 return -1;
698 }
699
700 ret = drv_gyro_st_lsm6dsr_set_range(&lsm6dsr_ctx, GYRO_RANGE_1000DPS);
701 if(unlikely(ret)){
702 return -1;
703 }
704
705 ret = drv_gyro_st_lsm6dsr_set_odr(&lsm6dsr_ctx, LSM6DSR_GYRO_DEFAULT_ODR_100HZ);
706 if(unlikely(ret)){
707 return -1;
708 }
709
710 return 0;
711
712 }
713
drv_gyro_st_lsm6dsr_close(void)714 static int drv_gyro_st_lsm6dsr_close(void)
715 {
716 int ret = 0;
717 ret = drv_gyro_st_lsm6dsr_set_power_mode(&lsm6dsr_ctx, DEV_POWER_OFF);
718 if(unlikely(ret)){
719 return -1;
720 }
721 return 0;
722 }
723
drv_gyro_st_lsm6dsr_read(void * buf,size_t len)724 static int drv_gyro_st_lsm6dsr_read(void *buf, size_t len)
725 {
726 int ret = 0;
727 size_t size;
728 uint8_t reg[6];
729 gyro_data_t *gyro = (gyro_data_t *)buf;
730 if(buf == NULL){
731 return -1;
732 }
733
734 size = sizeof(gyro_data_t);
735 if(len < size){
736 return -1;
737 }
738
739 ret = sensor_i2c_read(&lsm6dsr_ctx, LSM6DSR_ACC_GYRO_OUTX_L_G, ®[0], I2C_REG_LEN, I2C_OP_RETRIES);
740 ret |= sensor_i2c_read(&lsm6dsr_ctx, LSM6DSR_ACC_GYRO_OUTX_H_G, ®[1], I2C_REG_LEN, I2C_OP_RETRIES);
741 ret |= sensor_i2c_read(&lsm6dsr_ctx, LSM6DSR_ACC_GYRO_OUTY_L_G, ®[2], I2C_REG_LEN, I2C_OP_RETRIES);
742 ret |= sensor_i2c_read(&lsm6dsr_ctx, LSM6DSR_ACC_GYRO_OUTY_H_G, ®[3], I2C_REG_LEN, I2C_OP_RETRIES);
743 ret |= sensor_i2c_read(&lsm6dsr_ctx, LSM6DSR_ACC_GYRO_OUTZ_L_G, ®[4], I2C_REG_LEN, I2C_OP_RETRIES);
744 ret |= sensor_i2c_read(&lsm6dsr_ctx, LSM6DSR_ACC_GYRO_OUTZ_H_G, ®[5], I2C_REG_LEN, I2C_OP_RETRIES);
745 if(unlikely(ret)){
746 return -1;
747 }
748 gyro->data[DATA_AXIS_X] = (int16_t)((((int32_t)((int8_t)reg[1]))<< LSM6DSR_SHIFT_EIGHT_BITS)|(reg[0]));
749 gyro->data[DATA_AXIS_Y] = (int16_t)((((int32_t)((int8_t)reg[3]))<< LSM6DSR_SHIFT_EIGHT_BITS)|(reg[2]));
750 gyro->data[DATA_AXIS_Z] = (int16_t)((((int32_t)((int8_t)reg[5]))<< LSM6DSR_SHIFT_EIGHT_BITS)|(reg[4]));
751
752 if(cur_gyro_factor != 0){
753 gyro->data[DATA_AXIS_X] = (gyro->data[DATA_AXIS_X] * cur_gyro_factor)/LSM6DSR_GYRO_MUL;
754 gyro->data[DATA_AXIS_Y] = (gyro->data[DATA_AXIS_Y] * cur_gyro_factor)/LSM6DSR_GYRO_MUL;
755 gyro->data[DATA_AXIS_Z] = (gyro->data[DATA_AXIS_Z] * cur_gyro_factor)/LSM6DSR_GYRO_MUL;
756 }
757 gyro->timestamp = aos_now_ms();
758
759 return (int)size;
760 }
761
drv_gyro_st_lsm6dsr_ioctl(int cmd,unsigned long arg)762 static int drv_gyro_st_lsm6dsr_ioctl(int cmd, unsigned long arg)
763 {
764 int ret = 0;
765
766 switch(cmd){
767 case SENSOR_IOCTL_ODR_SET:{
768 ret = drv_gyro_st_lsm6dsr_set_odr(&lsm6dsr_ctx, arg);
769 if(unlikely(ret)){
770 return -1;
771 }
772 }break;
773 case SENSOR_IOCTL_RANGE_SET:{
774 ret = drv_gyro_st_lsm6dsr_set_range(&lsm6dsr_ctx, arg);
775 if(unlikely(ret)){
776 return -1;
777 }
778 }break;
779 case SENSOR_IOCTL_SET_POWER:{
780 ret = drv_gyro_st_lsm6dsr_set_power_mode(&lsm6dsr_ctx, arg);
781 if(unlikely(ret)){
782 return -1;
783 }
784 }break;
785 case SENSOR_IOCTL_GET_INFO:{
786 /* fill the dev info here */
787 dev_sensor_info_t *info = (dev_sensor_info_t *)arg;
788 info->model = "LSM6DSR";
789 info->range_max = 2000;
790 info->range_min = 125;
791 info->unit = udps;
792 }break;
793
794 default:break;
795 }
796
797 return 0;
798 }
799
drv_gyro_st_lsm6dsr_init(void)800 int drv_gyro_st_lsm6dsr_init(void){
801 int ret = 0;
802 sensor_obj_t sensor;
803 memset(&sensor, 0, sizeof(sensor));
804
805 /* fill the sensor obj parameters here */
806 sensor.io_port = I2C_PORT;
807 sensor.tag = TAG_DEV_GYRO;
808 sensor.path = dev_gyro_path;
809 sensor.open = drv_gyro_st_lsm6dsr_open;
810 sensor.close = drv_gyro_st_lsm6dsr_close;
811 sensor.read = drv_gyro_st_lsm6dsr_read;
812 sensor.write = NULL;
813 sensor.ioctl = drv_gyro_st_lsm6dsr_ioctl;
814 sensor.irq_handle = drv_gyro_st_lsm6dsr_irq_handle;
815
816 ret = sensor_create_obj(&sensor);
817 if(unlikely(ret)){
818 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
819 return -1;
820 }
821
822 ret = drv_acc_gyro_st_lsm6dsr_validate_id(&lsm6dsr_ctx, LSM6DSR_CHIP_ID_VALUE);
823 if(unlikely(ret)){
824 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
825 return -1;
826 }
827
828 if(0 == g_lsm6dsrflag){
829 ret = drv_acc_gyro_st_lsm6dsr_soft_reset(&lsm6dsr_ctx);
830 if(unlikely(ret)){
831 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
832 return -1;
833 }
834
835 ret = drv_gyro_st_lsm6dsr_set_power_mode(&lsm6dsr_ctx, DEV_POWER_OFF);
836 if(unlikely(ret)){
837 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
838 return -1;
839 }
840 g_lsm6dsrflag = 1;
841 }
842 else{
843 LOG("%s %s gyro do not need reset\n", SENSOR_STR, __func__);
844 }
845
846 /* update the phy sensor info to sensor hal */
847 LOG("%s %s successfully \n", SENSOR_STR, __func__);
848 return 0;
849 }
850
851
852 SENSOR_DRV_ADD(drv_acc_st_lsm6dsr_init);
853 SENSOR_DRV_ADD(drv_gyro_st_lsm6dsr_init);
854
855
856