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 LSM6DS3TR_C_I2C_ADDR1 (0x6A)
16 #define LSM6DS3TR_C_I2C_ADDR2 (0x6B)
17 #define LSM6DS3TR_C_I2C_ADDR_TRANS(n) ((n)<<1)
18 #define LSM6DS3TR_C_I2C_ADDR LSM6DS3TR_C_I2C_ADDR_TRANS(LSM6DS3TR_C_I2C_ADDR2)
19
20 #define LSM6DS3TR_C_ACC_GYRO_FUNC_CFG_ACCESS 0x01
21 #define LSM6DS3TR_C_ACC_GYRO_SENSOR_SYNC_TIME 0x04
22 #define LSM6DS3TR_C_ACC_GYRO_SENSOR_RES_RATIO 0x05
23 #define LSM6DS3TR_C_ACC_GYRO_FIFO_CTRL1 0x06
24 #define LSM6DS3TR_C_ACC_GYRO_FIFO_CTRL2 0x07
25 #define LSM6DS3TR_C_ACC_GYRO_FIFO_CTRL3 0x08
26 #define LSM6DS3TR_C_ACC_GYRO_FIFO_CTRL4 0x09
27 #define LSM6DS3TR_C_ACC_GYRO_FIFO_CTRL5 0x0A
28 #define LSM6DS3TR_C_ACC_GYRO_DRDY_PULSE_CFG_G 0x0B
29 #define LSM6DS3TR_C_ACC_GYRO_INT1_CTRL 0x0D
30 #define LSM6DS3TR_C_ACC_GYRO_INT2_CTRL 0x0E
31 #define LSM6DS3TR_C_ACC_GYRO_WHO_AM_I_REG 0x0F
32 #define LSM6DS3TR_C_ACC_GYRO_CTRL1_XL 0x10
33 #define LSM6DS3TR_C_ACC_GYRO_CTRL2_G 0x11
34 #define LSM6DS3TR_C_ACC_GYRO_CTRL3_C 0x12
35 #define LSM6DS3TR_C_ACC_GYRO_CTRL4_C 0x13
36 #define LSM6DS3TR_C_ACC_GYRO_CTRL5_C 0x14
37 #define LSM6DS3TR_C_ACC_GYRO_CTRL6_C 0x15
38 #define LSM6DS3TR_C_ACC_GYRO_CTRL7_G 0x16
39 #define LSM6DS3TR_C_ACC_GYRO_CTRL8_XL 0x17
40 #define LSM6DS3TR_C_ACC_GYRO_CTRL9_XL 0x18
41 #define LSM6DS3TR_C_ACC_GYRO_CTRL10_C 0x19
42
43
44 #define LSM6DS3TR_C_ACC_GYRO_MASTER_CONFIG 0x1A
45 #define LSM6DS3TR_C_ACC_GYRO_WAKE_UP_SRC 0x1B
46 #define LSM6DS3TR_C_ACC_GYRO_TAP_SRC 0x1C
47 #define LSM6DS3TR_C_ACC_GYRO_D6D_SRC 0x1D
48 #define LSM6DS3TR_C_ACC_GYRO_STATUS_REG 0x1E
49
50 #define LSM6DS3TR_C_ACC_GYRO_OUT_TEMP_L 0x20
51 #define LSM6DS3TR_C_ACC_GYRO_OUT_TEMP_H 0x21
52 #define LSM6DS3TR_C_ACC_GYRO_OUTX_L_G 0x22
53 #define LSM6DS3TR_C_ACC_GYRO_OUTX_H_G 0x23
54 #define LSM6DS3TR_C_ACC_GYRO_OUTY_L_G 0x24
55 #define LSM6DS3TR_C_ACC_GYRO_OUTY_H_G 0x25
56 #define LSM6DS3TR_C_ACC_GYRO_OUTZ_L_G 0x26
57 #define LSM6DS3TR_C_ACC_GYRO_OUTZ_H_G 0x27
58 #define LSM6DS3TR_C_ACC_GYRO_OUTX_L_XL 0x28
59 #define LSM6DS3TR_C_ACC_GYRO_OUTX_H_XL 0x29
60 #define LSM6DS3TR_C_ACC_GYRO_OUTY_L_XL 0x2A
61 #define LSM6DS3TR_C_ACC_GYRO_OUTY_H_XL 0x2B
62 #define LSM6DS3TR_C_ACC_GYRO_OUTZ_L_XL 0x2C
63 #define LSM6DS3TR_C_ACC_GYRO_OUTZ_H_XL 0x2D
64 #define LSM6DS3TR_C_ACC_GYRO_SENSORHUB1_REG 0x2E
65 #define LSM6DS3TR_C_ACC_GYRO_SENSORHUB2_REG 0x2F
66 #define LSM6DS3TR_C_ACC_GYRO_SENSORHUB3_REG 0x30
67 #define LSM6DS3TR_C_ACC_GYRO_SENSORHUB4_REG 0x31
68 #define LSM6DS3TR_C_ACC_GYRO_SENSORHUB5_REG 0x32
69 #define LSM6DS3TR_C_ACC_GYRO_SENSORHUB6_REG 0x33
70 #define LSM6DS3TR_C_ACC_GYRO_SENSORHUB7_REG 0x34
71 #define LSM6DS3TR_C_ACC_GYRO_SENSORHUB8_REG 0x35
72 #define LSM6DS3TR_C_ACC_GYRO_SENSORHUB9_REG 0x36
73 #define LSM6DS3TR_C_ACC_GYRO_SENSORHUB10_REG 0x37
74 #define LSM6DS3TR_C_ACC_GYRO_SENSORHUB11_REG 0x38
75 #define LSM6DS3TR_C_ACC_GYRO_SENSORHUB12_REG 0x39
76 #define LSM6DS3TR_C_ACC_GYRO_FIFO_STATUS1 0x3A
77 #define LSM6DS3TR_C_ACC_GYRO_FIFO_STATUS2 0x3B
78 #define LSM6DS3TR_C_ACC_GYRO_FIFO_STATUS3 0x3C
79 #define LSM6DS3TR_C_ACC_GYRO_FIFO_STATUS4 0x3D
80 #define LSM6DS3TR_C_ACC_GYRO_FIFO_DATA_OUT_L 0x3E
81 #define LSM6DS3TR_C_ACC_GYRO_FIFO_DATA_OUT_H 0x3F
82 #define LSM6DS3TR_C_ACC_GYRO_TIMESTAMP0_REG 0x40
83 #define LSM6DS3TR_C_ACC_GYRO_TIMESTAMP1_REG 0x41
84 #define LSM6DS3TR_C_ACC_GYRO_TIMESTAMP2_REG 0x42
85
86 #define LSM6DS3TR_C_ACC_GYRO_TIMESTAMP_L 0x49
87 #define LSM6DS3TR_C_ACC_GYRO_TIMESTAMP_H 0x4A
88
89 #define LSM6DS3TR_C_ACC_GYRO_STEP_COUNTER_L 0x4B
90 #define LSM6DS3TR_C_ACC_GYRO_STEP_COUNTER_H 0x4C
91
92 #define LSM6DS3TR_C_ACC_GYRO_SENSORHUB13_REG 0x4D
93 #define LSM6DS3TR_C_ACC_GYRO_SENSORHUB14_REG 0x4E
94 #define LSM6DS3TR_C_ACC_GYRO_SENSORHUB15_REG 0x4F
95 #define LSM6DS3TR_C_ACC_GYRO_SENSORHUB16_REG 0x50
96 #define LSM6DS3TR_C_ACC_GYRO_SENSORHUB17_REG 0x51
97 #define LSM6DS3TR_C_ACC_GYRO_SENSORHUB18_REG 0x52
98
99 #define LSM6DS3TR_C_ACC_GYRO_FUNC_SRC 0x53
100 #define LSM6DS3TR_C_ACC_GYRO_TAP_CFG1 0x58
101 #define LSM6DS3TR_C_ACC_GYRO_TAP_THS_6D 0x59
102 #define LSM6DS3TR_C_ACC_GYRO_INT_DUR2 0x5A
103 #define LSM6DS3TR_C_ACC_GYRO_WAKE_UP_THS 0x5B
104 #define LSM6DS3TR_C_ACC_GYRO_WAKE_UP_DUR 0x5C
105 #define LSM6DS3TR_C_ACC_GYRO_FREE_FALL 0x5D
106 #define LSM6DS3TR_C_ACC_GYRO_MD1_CFG 0x5E
107 #define LSM6DS3TR_C_ACC_GYRO_MD2_CFG 0x5F
108
109 #define LSM6DS3TR_C_ACC_GYRO_OUT_MAG_RAW_X_L 0x66
110 #define LSM6DS3TR_C_ACC_GYRO_OUT_MAG_RAW_X_H 0x67
111 #define LSM6DS3TR_C_ACC_GYRO_OUT_MAG_RAW_Y_L 0x68
112 #define LSM6DS3TR_C_ACC_GYRO_OUT_MAG_RAW_Y_H 0x69
113 #define LSM6DS3TR_C_ACC_GYRO_OUT_MAG_RAW_Z_L 0x6A
114 #define LSM6DS3TR_C_ACC_GYRO_OUT_MAG_RAW_Z_H 0x6B
115
116 #define LSM6DS3TR_C_ACC_GYRO_X_OFS_USR 0x73
117 #define LSM6DS3TR_C_ACC_GYRO_Y_OFS_USR 0x74
118 #define LSM6DS3TR_C_ACC_GYRO_Z_OFS_USR 0x75
119
120 #define LSM6DS3TR_C_CHIP_ID_VALUE (0x6A)
121
122 #define LSM6DS3TR_C_RESET_VALUE (0x1)
123 #define LSM6DS3TR_C_RESET_MSK (0X1)
124 #define LSM6DS3TR_C_RESET_POS (0)
125
126 #define LSM6DS3TR_C_BDU_VALUE (0x40)
127 #define LSM6DS3TR_C_BDU_MSK (0X40)
128 #define LSM6DS3TR_C_BDU_POS (6)
129
130 #define LSM6DS3TR_C_ACC_ODR_POWER_DOWN (0X00)
131 #define LSM6DS3TR_C_ACC_ODR_1_6_HZ (0X0B)
132 #define LSM6DS3TR_C_ACC_ODR_12_5_HZ (0x01)
133 #define LSM6DS3TR_C_ACC_ODR_26_HZ (0x02)
134 #define LSM6DS3TR_C_ACC_ODR_52_HZ (0x03)
135 #define LSM6DS3TR_C_ACC_ODR_104_HZ (0x04)
136 #define LSM6DS3TR_C_ACC_ODR_208_HZ (0x05)
137 #define LSM6DS3TR_C_ACC_ODR_416_HZ (0x06)
138 #define LSM6DS3TR_C_ACC_ODR_833_HZ (0x07)
139 #define LSM6DS3TR_C_ACC_ODR_1_66_KHZ (0x08)
140 #define LSM6DS3TR_C_ACC_ODR_3_33_KHZ (0x09)
141 #define LSM6DS3TR_C_ACC_ODR_6_66_KHZ (0x0A)
142 #define LSM6DS3TR_C_ACC_ODR_MSK (0XF0)
143 #define LSM6DS3TR_C_ACC_ODR_POS (4)
144
145 #define LSM6DS3TR_C_GYRO_ODR_POWER_DOWN (0X00)
146 #define LSM6DS3TR_C_GYRO_ODR_12_5_HZ (0x01)
147 #define LSM6DS3TR_C_GYRO_ODR_26_HZ (0x02)
148 #define LSM6DS3TR_C_GYRO_ODR_52_HZ (0x03)
149 #define LSM6DS3TR_C_GYRO_ODR_104_HZ (0x04)
150 #define LSM6DS3TR_C_GYRO_ODR_208_HZ (0x05)
151 #define LSM6DS3TR_C_GYRO_ODR_416_HZ (0x06)
152 #define LSM6DS3TR_C_GYRO_ODR_833_HZ (0x07)
153 #define LSM6DS3TR_C_GYRO_ODR_1_66_KHZ (0x08)
154 #define LSM6DS3TR_C_GYRO_ODR_3_33_KHZ (0x09)
155 #define LSM6DS3TR_C_GYRO_ODR_6_66_KHZ (0x0A)
156 #define LSM6DS3TR_C_GYRO_ODR_MSK (0XF0)
157 #define LSM6DS3TR_C_GYRO_ODR_POS (4)
158
159 #define LSM6DS3TR_C_ACC_RANGE_2G (0x0)
160 #define LSM6DS3TR_C_ACC_RANGE_4G (0x2)
161 #define LSM6DS3TR_C_ACC_RANGE_8G (0x3)
162 #define LSM6DS3TR_C_ACC_RANGE_16G (0x1)
163 #define LSM6DS3TR_C_ACC_RANGE_MSK (0X0C)
164 #define LSM6DS3TR_C_ACC_RANGE_POS (2)
165
166 #define LSM6DS3TR_C_ACC_SENSITIVITY_2G (61)
167 #define LSM6DS3TR_C_ACC_SENSITIVITY_4G (122)
168 #define LSM6DS3TR_C_ACC_SENSITIVITY_8G (244)
169 #define LSM6DS3TR_C_ACC_SENSITIVITY_16G (488)
170
171 #define LSM6DS3TR_C_GYRO_RANGE_245 (0x0)
172 #define LSM6DS3TR_C_GYRO_RANGE_500 (0x1)
173 #define LSM6DS3TR_C_GYRO_RANGE_1000 (0x2)
174 #define LSM6DS3TR_C_GYRO_RANGE_2000 (0x3)
175 #define LSM6DS3TR_C_GYRO_RANGE_MSK (0X0C)
176 #define LSM6DS3TR_C_GYRO_RANGE_POS (2)
177
178 #define LSM6DS3TR_C_GYRO_SENSITIVITY_245DPS (8750)
179 #define LSM6DS3TR_C_GYRO_SENSITIVITY_500DPS (17500)
180 #define LSM6DS3TR_C_GYRO_SENSITIVITY_1000DPS (35000)
181 #define LSM6DS3TR_C_GYRO_SENSITIVITY_2000DPS (70000)
182
183 #define LSM6DS3TR_C_SHIFT_EIGHT_BITS (8)
184 #define LSM6DS3TR_C_16_BIT_SHIFT (0xFF)
185 #define LSM6DS3TR_C_ACC_MUL (1000)
186 #define LSM6DS3TR_C_GYRO_MUL (1)
187
188 #define LSM6DS3TR_C_ACC_DEFAULT_ODR_100HZ (100)
189 #define LSM6DS3TR_C_GYRO_DEFAULT_ODR_100HZ (100)
190
191 #define LSM6DS3TR_C_ACC_SELF_TEST_MIN_X (90) // 90mg
192 #define LSM6DS3TR_C_ACC_SELF_TEST_MIN_Y (90) // 90mg
193 #define LSM6DS3TR_C_ACC_SELF_TEST_MIN_Z (90) // 90mg
194 #define LSM6DS3TR_C_ACC_SELF_TEST_MAX_X (1700) // 1700mg
195 #define LSM6DS3TR_C_ACC_SELF_TEST_MAX_Y (1700) // 1700mg
196 #define LSM6DS3TR_C_ACC_SELF_TEST_MAX_Z (1700) // 1700mg
197
198 #define LSM6DS3TR_C_ACC_SELF_TEST_DRY_WAIT_CNT 5
199 #define LSM6DS3TR_C_ACC_SELF_TEST_AVG_SAMPLE_CNT 5
200
201 #define LSM6DS3TR_C_GYRO_SELF_TEST_MIN_X (150) // 150dps
202 #define LSM6DS3TR_C_GYRO_SELF_TEST_MIN_Y (150) // 150dps
203 #define LSM6DS3TR_C_GYRO_SELF_TEST_MIN_Z (150) // 150dps
204 #define LSM6DS3TR_C_GYRO_SELF_TEST_MAX_X (700) // 700dps
205 #define LSM6DS3TR_C_GYRO_SELF_TEST_MAX_Y (700) // 700dps
206 #define LSM6DS3TR_C_GYRO_SELF_TEST_MAX_Z (700) // 700dps
207
208 #define LSM6DS3TR_C_GYRO_SELF_TEST_DRY_WAIT_CNT 5
209 #define LSM6DS3TR_C_GYRO_SELF_TEST_AVG_SAMPLE_CNT 5
210
211 #define LSM6DS3TR_C_GET_BITSLICE(regvar, bitname)\
212 ((regvar & bitname##_MSK) >> bitname##_POS)
213
214 #define LSM6DS3TR_C_SET_BITSLICE(regvar, bitname, val)\
215 ((regvar & ~bitname##_MSK) | ((val<<bitname##_POS)&bitname##_MSK))
216
217 static int32_t lsm6ds3tr_c_acc_factor[ACC_RANGE_MAX] = { LSM6DS3TR_C_ACC_SENSITIVITY_2G, LSM6DS3TR_C_ACC_SENSITIVITY_4G,
218 LSM6DS3TR_C_ACC_SENSITIVITY_8G, LSM6DS3TR_C_ACC_SENSITIVITY_16G };
219 static int32_t lsm6ds3tr_c_gyro_factor[GYRO_RANGE_MAX] = {0, LSM6DS3TR_C_GYRO_SENSITIVITY_245DPS, LSM6DS3TR_C_GYRO_SENSITIVITY_500DPS,
220 LSM6DS3TR_C_GYRO_SENSITIVITY_1000DPS, LSM6DS3TR_C_GYRO_SENSITIVITY_2000DPS };
221 static int32_t cur_acc_factor = 0;
222 static int32_t cur_gyro_factor = 0;
223 static int32_t g_lsm6ds3tr_cflag = 0;
224
225 static i2c_dev_t lsm6ds3tr_c_ctx = {
226 //.port = 4,
227 .port = 3,
228 .config.address_width = 8,
229 .config.freq = 400000,
230 .config.dev_addr = LSM6DS3TR_C_I2C_ADDR,
231 };
232
drv_acc_gyro_st_lsm6ds3tr_c_soft_reset(i2c_dev_t * drv)233 static int drv_acc_gyro_st_lsm6ds3tr_c_soft_reset(i2c_dev_t* drv)
234 {
235 int ret = 0;
236 uint8_t value = 0x00;
237
238 ret = sensor_i2c_read(drv, LSM6DS3TR_C_ACC_GYRO_CTRL3_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
239 if(unlikely(ret)){
240 return ret;
241 }
242
243 value |= LSM6DS3TR_C_RESET_VALUE;
244
245 ret = sensor_i2c_write(drv, LSM6DS3TR_C_ACC_GYRO_CTRL3_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
246 if(unlikely(ret)){
247 return -1;
248 }
249 return 0;
250 }
251
drv_acc_gyro_st_lsm6ds3tr_c_validate_id(i2c_dev_t * drv,uint8_t id_value)252 static int drv_acc_gyro_st_lsm6ds3tr_c_validate_id(i2c_dev_t* drv, uint8_t id_value)
253 {
254 uint8_t value = 0x00;
255 int ret = 0;
256
257 if(drv == NULL){
258 return -1;
259 }
260
261 ret = sensor_i2c_read(drv, LSM6DS3TR_C_ACC_GYRO_WHO_AM_I_REG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
262 if(unlikely(ret)){
263 return ret;
264 }
265
266 LOG("%s %s right id (0x%02x), read id(0x%02x)\n", SENSOR_STR, __func__, id_value, value);
267 if (id_value != value){
268 return -1;
269 }
270
271 return 0;
272 }
273
drv_acc_st_lsm6ds3tr_c_set_power_mode(i2c_dev_t * drv,dev_power_mode_e mode)274 static int drv_acc_st_lsm6ds3tr_c_set_power_mode(i2c_dev_t* drv, dev_power_mode_e mode)
275 {
276 uint8_t value = 0x00;
277 int ret = 0;
278
279 ret = sensor_i2c_read(drv, LSM6DS3TR_C_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
280 if(unlikely(ret)){
281 return ret;
282 }
283
284 switch(mode){
285 case DEV_POWER_ON:{
286
287 value = LSM6DS3TR_C_SET_BITSLICE(value,LSM6DS3TR_C_ACC_ODR,LSM6DS3TR_C_ACC_ODR_12_5_HZ);
288 ret = sensor_i2c_write(drv, LSM6DS3TR_C_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
289 if(unlikely(ret)){
290 return ret;
291 }
292 }break;
293
294 case DEV_POWER_OFF:{
295 value = LSM6DS3TR_C_SET_BITSLICE(value,LSM6DS3TR_C_ACC_ODR,LSM6DS3TR_C_ACC_ODR_POWER_DOWN);
296 ret = sensor_i2c_write(drv, LSM6DS3TR_C_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
297 if(unlikely(ret)){
298 return ret;
299 }
300 }break;
301
302 case DEV_SLEEP:{
303 value = LSM6DS3TR_C_SET_BITSLICE(value,LSM6DS3TR_C_ACC_ODR,LSM6DS3TR_C_ACC_ODR_12_5_HZ);
304 ret = sensor_i2c_write(drv, LSM6DS3TR_C_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
305 if(unlikely(ret)){
306 return ret;
307 }
308
309 }break;
310
311 default:break;
312 }
313 return 0;
314 }
315
drv_acc_gyro_st_lsm6ds3tr_c_set_bdu(i2c_dev_t * drv)316 static int drv_acc_gyro_st_lsm6ds3tr_c_set_bdu(i2c_dev_t* drv)
317 {
318 int ret = 0;
319 uint8_t value = 0x00;
320
321 ret = sensor_i2c_read(drv, LSM6DS3TR_C_ACC_GYRO_CTRL3_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
322 if(unlikely(ret)){
323 return ret;
324 }
325
326 if (value & LSM6DS3TR_C_BDU_VALUE)
327 return 0;
328
329 value |= LSM6DS3TR_C_BDU_VALUE;
330
331 ret = sensor_i2c_write(drv, LSM6DS3TR_C_ACC_GYRO_CTRL3_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
332 if(unlikely(ret)){
333 return -1;
334 }
335 return 0;
336 }
337
drv_acc_st_lsm6ds3tr_c_hz2odr(uint32_t hz)338 static uint8_t drv_acc_st_lsm6ds3tr_c_hz2odr(uint32_t hz)
339 {
340 if(hz > 3330)
341 return LSM6DS3TR_C_ACC_ODR_6_66_KHZ;
342 else if(hz > 1660)
343 return LSM6DS3TR_C_ACC_ODR_3_33_KHZ;
344 else if(hz > 833)
345 return LSM6DS3TR_C_ACC_ODR_1_66_KHZ;
346 else if(hz > 416)
347 return LSM6DS3TR_C_ACC_ODR_833_HZ;
348 else if(hz > 208)
349 return LSM6DS3TR_C_ACC_ODR_416_HZ;
350 else if(hz > 104)
351 return LSM6DS3TR_C_ACC_ODR_208_HZ;
352 else if(hz > 52)
353 return LSM6DS3TR_C_ACC_ODR_104_HZ;
354 else if(hz > 26)
355 return LSM6DS3TR_C_ACC_ODR_52_HZ;
356 else if(hz > 13)
357 return LSM6DS3TR_C_ACC_ODR_26_HZ;
358 else if(hz >= 2)
359 return LSM6DS3TR_C_ACC_ODR_12_5_HZ;
360 else
361 return LSM6DS3TR_C_ACC_ODR_1_6_HZ;
362
363 }
364
365
drv_acc_st_lsm6ds3tr_c_set_odr(i2c_dev_t * drv,uint32_t hz)366 static int drv_acc_st_lsm6ds3tr_c_set_odr(i2c_dev_t* drv, uint32_t hz)
367 {
368 int ret = 0;
369 uint8_t value = 0x00;
370 uint8_t odr = drv_acc_st_lsm6ds3tr_c_hz2odr(hz);
371
372 ret = sensor_i2c_read(drv, LSM6DS3TR_C_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
373 if(unlikely(ret)){
374 return ret;
375 }
376
377 value = LSM6DS3TR_C_SET_BITSLICE(value,LSM6DS3TR_C_ACC_ODR,odr);
378
379 ret = sensor_i2c_write(drv, LSM6DS3TR_C_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
380 if(unlikely(ret)){
381 return ret;
382 }
383 return 0;
384 }
385
drv_acc_st_lsm6ds3tr_c_set_range(i2c_dev_t * drv,uint32_t range)386 static int drv_acc_st_lsm6ds3tr_c_set_range(i2c_dev_t* drv, uint32_t range)
387 {
388 int ret = 0;
389 uint8_t value = 0x00;
390 uint8_t tmp = 0;
391
392 ret = sensor_i2c_read(drv, LSM6DS3TR_C_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
393 if(unlikely(ret)){
394 return ret;
395 }
396
397 switch (range){
398 case ACC_RANGE_2G:{
399 tmp = LSM6DS3TR_C_ACC_RANGE_2G;
400 }break;
401
402 case ACC_RANGE_4G:{
403 tmp = LSM6DS3TR_C_ACC_RANGE_4G;
404 }break;
405
406 case ACC_RANGE_8G:{
407 tmp = LSM6DS3TR_C_ACC_RANGE_8G;
408 }break;
409
410 case ACC_RANGE_16G:{
411 tmp = LSM6DS3TR_C_ACC_RANGE_16G;
412 }break;
413
414 default:break;
415 }
416
417 value = LSM6DS3TR_C_SET_BITSLICE(value,LSM6DS3TR_C_ACC_RANGE,tmp);
418 ret = sensor_i2c_write(drv, LSM6DS3TR_C_ACC_GYRO_CTRL1_XL, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
419 if(unlikely(ret)){
420 return ret;
421 }
422
423 if((range >= ACC_RANGE_2G)&&(range <= ACC_RANGE_16G)){
424 cur_acc_factor = lsm6ds3tr_c_acc_factor[range];
425 }
426
427 return 0;
428 }
429
drv_acc_st_lsm6ds3tr_c_st_discard(i2c_dev_t * drv)430 static int drv_acc_st_lsm6ds3tr_c_st_discard(i2c_dev_t* drv)
431 {
432 uint8_t i;
433 uint8_t value = 0x00;
434 int ret = 0;
435 uint8_t buffer[6];
436
437 for (i = 0; i < LSM6DS3TR_C_ACC_SELF_TEST_DRY_WAIT_CNT; i ++) {
438 ret = sensor_i2c_read(drv, LSM6DS3TR_C_ACC_GYRO_STATUS_REG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
439 if(unlikely(ret)){
440 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
441 return ret;
442 }
443 if (value & 0x01)
444 break;
445
446 aos_msleep(20);
447 }
448
449 if (i >= LSM6DS3TR_C_ACC_SELF_TEST_DRY_WAIT_CNT) {
450 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
451 return -1;
452 }
453
454 ret = sensor_i2c_read(drv, LSM6DS3TR_C_ACC_GYRO_OUTX_L_XL, buffer, 6, I2C_OP_RETRIES);
455 if(unlikely(ret)){
456 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
457 return ret;
458 }
459
460 return ret;
461 }
462
drv_acc_st_lsm6ds3tr_c_st_data(i2c_dev_t * drv,int32_t * data)463 static int drv_acc_st_lsm6ds3tr_c_st_data(i2c_dev_t* drv,int32_t* data)
464 {
465 uint8_t i, j;
466 int16_t x_raw, y_raw, z_raw;
467 int32_t x_mg, y_mg, z_mg;
468 int32_t x_sum, y_sum, z_sum;
469 uint8_t value = 0x00;
470 int ret = 0;
471 uint8_t buffer[6];
472
473 x_sum = 0; y_sum = 0; z_sum = 0;
474
475 for (i = 0; i < LSM6DS3TR_C_ACC_SELF_TEST_AVG_SAMPLE_CNT; i ++) {
476 for (j = 0; j < LSM6DS3TR_C_ACC_SELF_TEST_DRY_WAIT_CNT; j ++) {
477 ret = sensor_i2c_read(drv, LSM6DS3TR_C_ACC_GYRO_STATUS_REG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
478 if(unlikely(ret)){
479 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
480 return ret;
481 }
482 if (value & 0x01)
483 break;
484
485 aos_msleep(20);
486 }
487
488 if (j >= LSM6DS3TR_C_ACC_SELF_TEST_DRY_WAIT_CNT) {
489 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
490 return -1;
491 }
492
493 ret = sensor_i2c_read(drv, LSM6DS3TR_C_ACC_GYRO_OUTX_L_XL, buffer, 6, I2C_OP_RETRIES);
494 if(unlikely(ret)){
495 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
496 return ret;
497 }
498
499 x_raw = (buffer[1] << 8) + buffer[0];
500 y_raw = (buffer[3] << 8) + buffer[2];
501 z_raw = (buffer[5] << 8) + buffer[4];
502 //LOG("%s %s %d: i(%d), raw(%d, %d, %d)\n", SENSOR_STR, __func__, __LINE__, i, x_raw, y_raw, z_raw);
503
504 x_mg = x_raw * LSM6DS3TR_C_ACC_SENSITIVITY_4G / 1000;
505 y_mg = y_raw * LSM6DS3TR_C_ACC_SENSITIVITY_4G / 1000;
506 z_mg = z_raw * LSM6DS3TR_C_ACC_SENSITIVITY_4G / 1000;
507 //LOG("%s %s %d: i(%d), mg(%d, %d, %d)\n", SENSOR_STR, __func__, __LINE__, i, x_mg, y_mg, z_mg);
508
509 x_sum += x_mg;
510 y_sum += y_mg;
511 z_sum += z_mg;
512 }
513
514 data[0] = x_sum / LSM6DS3TR_C_ACC_SELF_TEST_AVG_SAMPLE_CNT;
515 data[1] = y_sum / LSM6DS3TR_C_ACC_SELF_TEST_AVG_SAMPLE_CNT;
516 data[2] = z_sum / LSM6DS3TR_C_ACC_SELF_TEST_AVG_SAMPLE_CNT;
517 return ret;
518 }
519
drv_acc_st_lsm6ds3tr_c_self_test(i2c_dev_t * drv,int32_t * data)520 static int drv_acc_st_lsm6ds3tr_c_self_test(i2c_dev_t* drv,int32_t* data)
521 {
522 uint8_t i;
523 uint8_t value = 0x00;
524 int ret = 0;
525 uint8_t ctrl_reg[10];
526 int32_t out_nost[3];
527 int32_t out_st[3];
528 int32_t out_diff[3];
529 const uint8_t ctrl_val[10] = { 0x38, 0, 0x44, 0, 0, 0, 0, 0, 0, 0 };
530
531 // Save cfg registers which will be modified during self-test
532 ret = sensor_i2c_read(drv, LSM6DS3TR_C_ACC_GYRO_CTRL1_XL, ctrl_reg, 10, I2C_OP_RETRIES);
533 if(unlikely(ret)){
534 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
535 return ret;
536 }
537
538 // Initialize Sensor, turn on sensor, enable X/Y/Z axes
539 // Set BDU=1, FS=4G, ODR = 52Hz
540 for (i = 0; i < 10; i ++) {
541 value = ctrl_val[i];
542 ret = sensor_i2c_write(drv, LSM6DS3TR_C_ACC_GYRO_CTRL1_XL + i, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
543 if(unlikely(ret)){
544 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
545 goto restore;
546 }
547 }
548
549 aos_msleep(100);
550
551 // Discard the first sample
552 ret = drv_acc_st_lsm6ds3tr_c_st_discard(drv);
553 if(unlikely(ret)){
554 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
555 goto restore;
556 }
557
558 // Read some samples, and averate them
559 ret = drv_acc_st_lsm6ds3tr_c_st_data(drv, out_nost);
560 if(unlikely(ret)){
561 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
562 goto restore;
563 }
564
565 // Enable seft-test
566 value = 0x01;
567 ret = sensor_i2c_write(drv, LSM6DS3TR_C_ACC_GYRO_CTRL5_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
568 if(unlikely(ret)){
569 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
570 goto restore;
571 }
572 aos_msleep(100);
573
574 // Discard the first sample
575 ret = drv_acc_st_lsm6ds3tr_c_st_discard(drv);
576 if(unlikely(ret)){
577 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
578 goto restore;
579 }
580
581 // Read some samples, and average them
582 ret = drv_acc_st_lsm6ds3tr_c_st_data(drv, out_st);
583 if(unlikely(ret)){
584 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
585 goto restore;
586 }
587
588 // Check if the differences are between min and max
589 for (i = 0; i < 3; i ++) {
590 out_diff[i] = abs(out_st[i] - out_nost[i]);
591 data[i] = out_diff[i];
592 }
593
594 if ((LSM6DS3TR_C_ACC_SELF_TEST_MIN_X > out_diff[0]) || (out_diff[0] > LSM6DS3TR_C_ACC_SELF_TEST_MAX_X)) {
595 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
596 ret = -1;
597 goto restore;
598 }
599 if ((LSM6DS3TR_C_ACC_SELF_TEST_MIN_Y > out_diff[1]) || (out_diff[1] > LSM6DS3TR_C_ACC_SELF_TEST_MAX_Y)) {
600 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
601 ret = -1;
602 goto restore;
603 }
604 if ((LSM6DS3TR_C_ACC_SELF_TEST_MIN_Z > out_diff[2]) || (out_diff[2] > LSM6DS3TR_C_ACC_SELF_TEST_MAX_Z)) {
605 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
606 ret = -1;
607 goto restore;
608 }
609
610 restore:
611 ret += sensor_i2c_write(drv, LSM6DS3TR_C_ACC_GYRO_CTRL1_XL, ctrl_reg, 10, I2C_OP_RETRIES);
612 return ret;
613 }
614
615
drv_acc_st_lsm6ds3tr_c_irq_handle(void)616 static void drv_acc_st_lsm6ds3tr_c_irq_handle(void)
617 {
618 /* no handle so far */
619 }
620
drv_acc_st_lsm6ds3tr_c_open(void)621 static int drv_acc_st_lsm6ds3tr_c_open(void)
622 {
623 int ret = 0;
624
625 ret = drv_acc_st_lsm6ds3tr_c_set_power_mode(&lsm6ds3tr_c_ctx, DEV_POWER_ON);
626 if(unlikely(ret)){
627 return -1;
628 }
629
630 ret = drv_acc_gyro_st_lsm6ds3tr_c_set_bdu(&lsm6ds3tr_c_ctx);
631 if(unlikely(ret)){
632 return -1;
633 }
634
635 ret = drv_acc_st_lsm6ds3tr_c_set_range(&lsm6ds3tr_c_ctx, ACC_RANGE_8G);
636 if(unlikely(ret)){
637 return -1;
638 }
639
640 ret = drv_acc_st_lsm6ds3tr_c_set_odr(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_ACC_DEFAULT_ODR_100HZ);
641 if(unlikely(ret)){
642 return -1;
643 }
644
645 return 0;
646
647 }
648
drv_acc_st_lsm6ds3tr_c_close(void)649 static int drv_acc_st_lsm6ds3tr_c_close(void)
650 {
651 int ret = 0;
652 ret = drv_acc_st_lsm6ds3tr_c_set_power_mode(&lsm6ds3tr_c_ctx, DEV_POWER_OFF);
653 if(unlikely(ret)){
654 return -1;
655 }
656 return 0;
657 }
658
drv_acc_st_lsm6ds3tr_c_read(void * buf,size_t len)659 static int drv_acc_st_lsm6ds3tr_c_read(void *buf, size_t len)
660 {
661 int ret = 0;
662 size_t size;
663 uint8_t reg[6];
664 accel_data_t *accel = (accel_data_t *)buf;
665 if(buf == NULL){
666 return -1;
667 }
668
669 size = sizeof(accel_data_t);
670 if(len < size){
671 return -1;
672 }
673
674 ret = sensor_i2c_read(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_ACC_GYRO_OUTX_L_XL, ®[0], I2C_REG_LEN, I2C_OP_RETRIES);
675 ret |= sensor_i2c_read(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_ACC_GYRO_OUTX_H_XL, ®[1], I2C_REG_LEN, I2C_OP_RETRIES);
676 ret |= sensor_i2c_read(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_ACC_GYRO_OUTY_L_XL, ®[2], I2C_REG_LEN, I2C_OP_RETRIES);
677 ret |= sensor_i2c_read(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_ACC_GYRO_OUTY_H_XL, ®[3], I2C_REG_LEN, I2C_OP_RETRIES);
678 ret |= sensor_i2c_read(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_ACC_GYRO_OUTZ_L_XL, ®[4], I2C_REG_LEN, I2C_OP_RETRIES);
679 ret |= sensor_i2c_read(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_ACC_GYRO_OUTZ_H_XL, ®[5], I2C_REG_LEN, I2C_OP_RETRIES);
680 if(unlikely(ret)){
681 return -1;
682 }
683 accel->data[DATA_AXIS_X] = (int16_t)((((int16_t)((int8_t)reg[1]))<< LSM6DS3TR_C_SHIFT_EIGHT_BITS)|(reg[0]));
684 accel->data[DATA_AXIS_Y] = (int16_t)((((int16_t)((int8_t)reg[3]))<< LSM6DS3TR_C_SHIFT_EIGHT_BITS)|(reg[2]));
685 accel->data[DATA_AXIS_Z] = (int16_t)((((int16_t)((int8_t)reg[5]))<< LSM6DS3TR_C_SHIFT_EIGHT_BITS)|(reg[4]));
686
687 if(cur_acc_factor != 0){
688 accel->data[DATA_AXIS_X] = (accel->data[DATA_AXIS_X] * cur_acc_factor)/LSM6DS3TR_C_ACC_MUL;
689 accel->data[DATA_AXIS_Y] = (accel->data[DATA_AXIS_Y] * cur_acc_factor)/LSM6DS3TR_C_ACC_MUL;
690 accel->data[DATA_AXIS_Z] = (accel->data[DATA_AXIS_Z] * cur_acc_factor)/LSM6DS3TR_C_ACC_MUL;
691 }
692
693 accel->timestamp = aos_now_ms();
694
695 return (int)size;
696 }
697
drv_acc_st_lsm6ds3tr_c_ioctl(int cmd,unsigned long arg)698 static int drv_acc_st_lsm6ds3tr_c_ioctl(int cmd, unsigned long arg)
699 {
700 int ret = 0;
701 dev_sensor_info_t *info = (dev_sensor_info_t *)arg;
702
703 switch(cmd){
704 case SENSOR_IOCTL_ODR_SET:{
705 ret = drv_acc_st_lsm6ds3tr_c_set_odr(&lsm6ds3tr_c_ctx, arg);
706 if(unlikely(ret)){
707 return -1;
708 }
709 }break;
710 case SENSOR_IOCTL_RANGE_SET:{
711 ret = drv_acc_st_lsm6ds3tr_c_set_range(&lsm6ds3tr_c_ctx, arg);
712 if(unlikely(ret)){
713 return -1;
714 }
715 }break;
716 case SENSOR_IOCTL_SET_POWER:{
717 ret = drv_acc_st_lsm6ds3tr_c_set_power_mode(&lsm6ds3tr_c_ctx, arg);
718 if(unlikely(ret)){
719 return -1;
720 }
721 }break;
722 case SENSOR_IOCTL_GET_INFO:{
723 /* fill the dev info here */
724 //dev_sensor_info_t *info = (dev_sensor_info_t*)arg;
725 info->model = "LSM6DS3TR_C";
726 info->range_max = 16;
727 info->range_min = 2;
728 info->unit = mg;
729 }break;
730 case SENSOR_IOCTL_SELF_TEST:{
731 ret = drv_acc_st_lsm6ds3tr_c_self_test(&lsm6ds3tr_c_ctx, (int32_t*)info->data);
732 //printf("%d %d %d\n",info->data[0],info->data[1],info->data[2]);
733 LOG("%s %s: %d, %d, %d\n", SENSOR_STR, __func__, info->data[0],info->data[1],info->data[2]);
734 return ret;
735 }
736 default:break;
737 }
738
739 return 0;
740 }
741
742
drv_acc_st_lsm6ds3tr_c_init(void)743 int drv_acc_st_lsm6ds3tr_c_init(void){
744 int ret = 0;
745 sensor_obj_t sensor;
746 memset(&sensor, 0, sizeof(sensor));
747
748 /* fill the sensor obj parameters here */
749 sensor.io_port = I2C_PORT;
750 sensor.tag = TAG_DEV_ACC;
751 sensor.path = dev_acc_path;
752 sensor.open = drv_acc_st_lsm6ds3tr_c_open;
753 sensor.close = drv_acc_st_lsm6ds3tr_c_close;
754 sensor.read = drv_acc_st_lsm6ds3tr_c_read;
755 sensor.write = NULL;
756 sensor.ioctl = drv_acc_st_lsm6ds3tr_c_ioctl;
757 sensor.irq_handle = drv_acc_st_lsm6ds3tr_c_irq_handle;
758
759 ret = sensor_create_obj(&sensor);
760 if(unlikely(ret)){
761 return -1;
762 }
763 ret = drv_acc_gyro_st_lsm6ds3tr_c_validate_id(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_CHIP_ID_VALUE);
764 if(unlikely(ret)){
765 return -1;
766 }
767
768 if(0 == g_lsm6ds3tr_cflag)
769 {
770 ret = drv_acc_gyro_st_lsm6ds3tr_c_soft_reset(&lsm6ds3tr_c_ctx);
771 if(unlikely(ret)){
772 return -1;
773 }
774 ret = drv_acc_st_lsm6ds3tr_c_set_power_mode(&lsm6ds3tr_c_ctx, DEV_POWER_OFF);
775 if(unlikely(ret)){
776 return -1;
777 }
778 g_lsm6ds3tr_cflag = 1;
779 }
780 else
781 {
782 LOG("%s %s acc do not need reset\n", SENSOR_STR, __func__);
783 }
784
785 LOG("%s %s successfully \n", SENSOR_STR, __func__);
786 return 0;
787 }
788
789
drv_gyro_st_lsm6ds3tr_c_set_power_mode(i2c_dev_t * drv,dev_power_mode_e mode)790 static int drv_gyro_st_lsm6ds3tr_c_set_power_mode(i2c_dev_t* drv, dev_power_mode_e mode)
791 {
792 uint8_t value = 0x00;
793 int ret = 0;
794
795 ret = sensor_i2c_read(drv, LSM6DS3TR_C_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
796 if(unlikely(ret)){
797 return ret;
798 }
799
800 switch(mode){
801 case DEV_POWER_ON:{
802
803 value = LSM6DS3TR_C_SET_BITSLICE(value,LSM6DS3TR_C_GYRO_ODR,LSM6DS3TR_C_GYRO_ODR_12_5_HZ);
804 ret = sensor_i2c_write(drv, LSM6DS3TR_C_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
805 if(unlikely(ret)){
806 return ret;
807 }
808 }break;
809
810 case DEV_POWER_OFF:{
811 value = LSM6DS3TR_C_SET_BITSLICE(value,LSM6DS3TR_C_GYRO_ODR,LSM6DS3TR_C_GYRO_ODR_POWER_DOWN);
812 ret = sensor_i2c_write(drv, LSM6DS3TR_C_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
813 if(unlikely(ret)){
814 return ret;
815 }
816 }break;
817
818 case DEV_SLEEP:{
819 value = LSM6DS3TR_C_SET_BITSLICE(value,LSM6DS3TR_C_GYRO_ODR,LSM6DS3TR_C_GYRO_ODR_12_5_HZ);
820 ret = sensor_i2c_write(drv, LSM6DS3TR_C_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
821 if(unlikely(ret)){
822 return ret;
823 }
824
825 }break;
826
827 default:break;
828 }
829 return 0;
830 }
831
drv_gyro_st_lsm6ds3tr_c_hz2odr(uint32_t hz)832 static uint8_t drv_gyro_st_lsm6ds3tr_c_hz2odr(uint32_t hz)
833 {
834 if(hz > 3330)
835 return LSM6DS3TR_C_GYRO_ODR_6_66_KHZ;
836 else if(hz > 1660)
837 return LSM6DS3TR_C_GYRO_ODR_3_33_KHZ;
838 else if(hz > 833)
839 return LSM6DS3TR_C_GYRO_ODR_1_66_KHZ;
840 else if(hz > 416)
841 return LSM6DS3TR_C_GYRO_ODR_833_HZ;
842 else if(hz > 208)
843 return LSM6DS3TR_C_GYRO_ODR_416_HZ;
844 else if(hz > 104)
845 return LSM6DS3TR_C_GYRO_ODR_208_HZ;
846 else if(hz > 52)
847 return LSM6DS3TR_C_GYRO_ODR_104_HZ;
848 else if(hz > 26)
849 return LSM6DS3TR_C_GYRO_ODR_52_HZ;
850 else if(hz > 13)
851 return LSM6DS3TR_C_GYRO_ODR_26_HZ;
852 else
853 return LSM6DS3TR_C_GYRO_ODR_12_5_HZ;
854
855 }
856
857
drv_gyro_st_lsm6ds3tr_c_set_odr(i2c_dev_t * drv,uint32_t hz)858 static int drv_gyro_st_lsm6ds3tr_c_set_odr(i2c_dev_t* drv, uint32_t hz)
859 {
860 int ret = 0;
861 uint8_t value = 0x00;
862 uint8_t odr = drv_gyro_st_lsm6ds3tr_c_hz2odr(hz);
863
864 ret = sensor_i2c_read(drv, LSM6DS3TR_C_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
865 if(unlikely(ret)){
866 return ret;
867 }
868
869 value = LSM6DS3TR_C_SET_BITSLICE(value,LSM6DS3TR_C_GYRO_ODR,odr);
870
871 ret = sensor_i2c_write(drv, LSM6DS3TR_C_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
872 if(unlikely(ret)){
873 return ret;
874 }
875 return 0;
876 }
877
drv_gyro_st_lsm6ds3tr_c_set_range(i2c_dev_t * drv,uint32_t range)878 static int drv_gyro_st_lsm6ds3tr_c_set_range(i2c_dev_t* drv, uint32_t range)
879 {
880 int ret = 0;
881 uint8_t value = 0x00;
882 uint8_t tmp = 0;
883
884 ret = sensor_i2c_read(drv, LSM6DS3TR_C_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
885 if(unlikely(ret)){
886 return ret;
887 }
888
889 switch (range){
890 case GYRO_RANGE_250DPS:{
891 tmp = LSM6DS3TR_C_GYRO_RANGE_245;
892 }break;
893
894 case GYRO_RANGE_500DPS:{
895 tmp = LSM6DS3TR_C_GYRO_RANGE_500;
896 }break;
897
898 case GYRO_RANGE_1000DPS:{
899 tmp = LSM6DS3TR_C_GYRO_RANGE_1000;
900 }break;
901
902 case GYRO_RANGE_2000DPS:{
903 tmp = LSM6DS3TR_C_GYRO_RANGE_2000;
904 }break;
905
906 default:break;
907 }
908
909 value = LSM6DS3TR_C_SET_BITSLICE(value,LSM6DS3TR_C_GYRO_RANGE,tmp);
910 ret = sensor_i2c_write(drv, LSM6DS3TR_C_ACC_GYRO_CTRL2_G, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
911 if(unlikely(ret)){
912 return ret;
913 }
914
915 if((range >= GYRO_RANGE_250DPS)&&(range <= GYRO_RANGE_2000DPS)){
916 cur_gyro_factor = lsm6ds3tr_c_gyro_factor[range];
917 }
918
919 return 0;
920 }
921
drv_gyro_st_lsm6ds3tr_c_st_discard(i2c_dev_t * drv)922 static int drv_gyro_st_lsm6ds3tr_c_st_discard(i2c_dev_t* drv)
923 {
924 uint8_t i;
925 uint8_t value = 0x00;
926 int ret = 0;
927 uint8_t buffer[6];
928
929 for (i = 0; i < LSM6DS3TR_C_GYRO_SELF_TEST_DRY_WAIT_CNT; i ++) {
930 ret = sensor_i2c_read(drv, LSM6DS3TR_C_ACC_GYRO_STATUS_REG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
931 if(unlikely(ret)){
932 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
933 return ret;
934 }
935 if (value & 0x02)
936 break;
937
938 aos_msleep(20);
939 }
940
941 if (i >= LSM6DS3TR_C_GYRO_SELF_TEST_DRY_WAIT_CNT) {
942 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
943 return -1;
944 }
945
946 ret = sensor_i2c_read(drv, LSM6DS3TR_C_ACC_GYRO_OUTX_L_G, buffer, 6, I2C_OP_RETRIES);
947 if(unlikely(ret)){
948 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
949 return ret;
950 }
951
952 return ret;
953 }
954
drv_gyro_st_lsm6ds3tr_c_st_data(i2c_dev_t * drv,int32_t * data)955 static int drv_gyro_st_lsm6ds3tr_c_st_data(i2c_dev_t* drv,int32_t* data)
956 {
957 uint8_t i, j;
958 int16_t x_raw, y_raw, z_raw;
959 int32_t x_dps, y_dps, z_dps;
960 int32_t x_sum, y_sum, z_sum;
961 uint8_t value = 0x00;
962 int ret = 0;
963 uint8_t buffer[6];
964
965 x_sum = 0; y_sum = 0; z_sum = 0;
966
967 for (i = 0; i < LSM6DS3TR_C_GYRO_SELF_TEST_AVG_SAMPLE_CNT; i ++) {
968 for (j = 0; j < LSM6DS3TR_C_GYRO_SELF_TEST_DRY_WAIT_CNT; j ++) {
969 ret = sensor_i2c_read(drv, LSM6DS3TR_C_ACC_GYRO_STATUS_REG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
970 if(unlikely(ret)){
971 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
972 return ret;
973 }
974 if (value & 0x02)
975 break;
976
977 aos_msleep(5);
978 }
979
980 if (j >= LSM6DS3TR_C_GYRO_SELF_TEST_DRY_WAIT_CNT) {
981 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
982 return -1;
983 }
984
985 ret = sensor_i2c_read(drv, LSM6DS3TR_C_ACC_GYRO_OUTX_L_G, buffer, 6, I2C_OP_RETRIES);
986 if(unlikely(ret)){
987 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
988 return ret;
989 }
990
991 x_raw = (buffer[1] << 8) + buffer[0];
992 y_raw = (buffer[3] << 8) + buffer[2];
993 z_raw = (buffer[5] << 8) + buffer[4];
994 //LOG("%s %s %d: i(%d), raw(%d, %d, %d)\n", SENSOR_STR, __func__, __LINE__, i, x_raw, y_raw, z_raw);
995
996 x_dps = x_raw * LSM6DS3TR_C_GYRO_SENSITIVITY_2000DPS / 1000000;
997 y_dps = y_raw * LSM6DS3TR_C_GYRO_SENSITIVITY_2000DPS / 1000000;
998 z_dps = z_raw * LSM6DS3TR_C_GYRO_SENSITIVITY_2000DPS / 1000000;
999 //LOG("%s %s %d: i(%d), dps(%d, %d, %d)\n", SENSOR_STR, __func__, __LINE__, i, x_dps, y_dps, z_dps);
1000
1001 x_sum += x_dps;
1002 y_sum += y_dps;
1003 z_sum += z_dps;
1004 }
1005
1006 data[0] = x_sum / LSM6DS3TR_C_ACC_SELF_TEST_AVG_SAMPLE_CNT;
1007 data[1] = y_sum / LSM6DS3TR_C_ACC_SELF_TEST_AVG_SAMPLE_CNT;
1008 data[2] = z_sum / LSM6DS3TR_C_ACC_SELF_TEST_AVG_SAMPLE_CNT;
1009 return ret;
1010 }
1011
drv_gyro_st_lsm6ds3tr_c_self_test(i2c_dev_t * drv,int32_t * data)1012 static int drv_gyro_st_lsm6ds3tr_c_self_test(i2c_dev_t* drv,int32_t* data)
1013 {
1014 uint8_t i;
1015 uint8_t value = 0x00;
1016 int ret = 0;
1017 uint8_t ctrl_reg[10];
1018 int32_t out_nost[3];
1019 int32_t out_st[3];
1020 int32_t out_diff[3];
1021 const uint8_t ctrl_val[10] = { 0, 0x5c, 0x44, 0, 0, 0, 0, 0, 0, 0 };
1022
1023 // Save cfg registers which will be modified during self-test
1024 ret = sensor_i2c_read(drv, LSM6DS3TR_C_ACC_GYRO_CTRL1_XL, ctrl_reg, 10, I2C_OP_RETRIES);
1025 if(unlikely(ret)){
1026 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1027 return ret;
1028 }
1029
1030 // Initialize Sensor, turn on sensor, enable P/R/Y
1031 // Set BDU=1, ODR = 208Hz, FS=2000dps
1032 for (i = 0; i < 10; i ++) {
1033 value = ctrl_val[i];
1034 ret = sensor_i2c_write(drv, LSM6DS3TR_C_ACC_GYRO_CTRL1_XL + i, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
1035 if(unlikely(ret)){
1036 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1037 goto restore;
1038 }
1039 }
1040
1041 aos_msleep(150);
1042
1043 // Discard the first sample
1044 ret = drv_gyro_st_lsm6ds3tr_c_st_discard(drv);
1045 if(unlikely(ret)){
1046 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1047 goto restore;
1048 }
1049
1050 // Read some samples, and averate them
1051 ret = drv_gyro_st_lsm6ds3tr_c_st_data(drv, out_nost);
1052 if(unlikely(ret)){
1053 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1054 goto restore;
1055 }
1056
1057 // Enable seft-test
1058 value = 0x04;
1059 ret = sensor_i2c_write(drv, LSM6DS3TR_C_ACC_GYRO_CTRL5_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
1060 if(unlikely(ret)){
1061 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1062 goto restore;
1063 }
1064 aos_msleep(50);
1065
1066 // Discard the first sample
1067 ret = drv_gyro_st_lsm6ds3tr_c_st_discard(drv);
1068 if(unlikely(ret)){
1069 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1070 goto restore;
1071 }
1072
1073 // Read some samples, and average them
1074 ret = drv_gyro_st_lsm6ds3tr_c_st_data(drv, out_st);
1075 if(unlikely(ret)){
1076 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1077 goto restore;
1078 }
1079
1080 // Check if the differences are between min and max
1081 for (i = 0; i < 3; i ++) {
1082 out_diff[i] = abs(out_st[i] - out_nost[i]);
1083 data[i] = out_diff[i];
1084 }
1085
1086 if ((LSM6DS3TR_C_GYRO_SELF_TEST_MIN_X > out_diff[0]) || (out_diff[0] > LSM6DS3TR_C_GYRO_SELF_TEST_MAX_X)) {
1087 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1088 ret = -1;
1089 goto restore;
1090 }
1091 if ((LSM6DS3TR_C_GYRO_SELF_TEST_MIN_Y > out_diff[1]) || (out_diff[1] > LSM6DS3TR_C_GYRO_SELF_TEST_MAX_Y)) {
1092 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1093 ret = -1;
1094 goto restore;
1095 }
1096 if ((LSM6DS3TR_C_GYRO_SELF_TEST_MIN_Z > out_diff[2]) || (out_diff[2] > LSM6DS3TR_C_GYRO_SELF_TEST_MAX_Z)) {
1097 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1098 ret = -1;
1099 goto restore;
1100 }
1101
1102 restore:
1103 ret += sensor_i2c_write(drv, LSM6DS3TR_C_ACC_GYRO_CTRL1_XL, ctrl_reg, 10, I2C_OP_RETRIES);
1104 return ret;
1105 }
1106
1107
drv_gyro_st_lsm6ds3tr_c_irq_handle(void)1108 static void drv_gyro_st_lsm6ds3tr_c_irq_handle(void)
1109 {
1110 /* no handle so far */
1111 }
1112
drv_gyro_st_lsm6ds3tr_c_open(void)1113 static int drv_gyro_st_lsm6ds3tr_c_open(void)
1114 {
1115 int ret = 0;
1116 ret = drv_gyro_st_lsm6ds3tr_c_set_power_mode(&lsm6ds3tr_c_ctx, DEV_POWER_ON);
1117 if(unlikely(ret)){
1118 return -1;
1119 }
1120
1121 ret = drv_acc_gyro_st_lsm6ds3tr_c_set_bdu(&lsm6ds3tr_c_ctx);
1122 if(unlikely(ret)){
1123 return -1;
1124 }
1125
1126 ret = drv_gyro_st_lsm6ds3tr_c_set_range(&lsm6ds3tr_c_ctx, GYRO_RANGE_1000DPS);
1127 if(unlikely(ret)){
1128 return -1;
1129 }
1130
1131 ret = drv_gyro_st_lsm6ds3tr_c_set_odr(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_GYRO_DEFAULT_ODR_100HZ);
1132 if(unlikely(ret)){
1133 return -1;
1134 }
1135
1136 return 0;
1137
1138 }
1139
drv_gyro_st_lsm6ds3tr_c_close(void)1140 static int drv_gyro_st_lsm6ds3tr_c_close(void)
1141 {
1142 int ret = 0;
1143 ret = drv_gyro_st_lsm6ds3tr_c_set_power_mode(&lsm6ds3tr_c_ctx, DEV_POWER_OFF);
1144 if(unlikely(ret)){
1145 return -1;
1146 }
1147 return 0;
1148 }
1149
drv_gyro_st_lsm6ds3tr_c_read(void * buf,size_t len)1150 static int drv_gyro_st_lsm6ds3tr_c_read(void *buf, size_t len)
1151 {
1152 int ret = 0;
1153 size_t size;
1154 uint8_t reg[6];
1155 gyro_data_t *gyro = (gyro_data_t *)buf;
1156 if(buf == NULL){
1157 return -1;
1158 }
1159
1160 size = sizeof(gyro_data_t);
1161 if(len < size){
1162 return -1;
1163 }
1164
1165 ret = sensor_i2c_read(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_ACC_GYRO_OUTX_L_G, ®[0], I2C_REG_LEN, I2C_OP_RETRIES);
1166 ret |= sensor_i2c_read(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_ACC_GYRO_OUTX_H_G, ®[1], I2C_REG_LEN, I2C_OP_RETRIES);
1167 ret |= sensor_i2c_read(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_ACC_GYRO_OUTY_L_G, ®[2], I2C_REG_LEN, I2C_OP_RETRIES);
1168 ret |= sensor_i2c_read(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_ACC_GYRO_OUTY_H_G, ®[3], I2C_REG_LEN, I2C_OP_RETRIES);
1169 ret |= sensor_i2c_read(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_ACC_GYRO_OUTZ_L_G, ®[4], I2C_REG_LEN, I2C_OP_RETRIES);
1170 ret |= sensor_i2c_read(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_ACC_GYRO_OUTZ_H_G, ®[5], I2C_REG_LEN, I2C_OP_RETRIES);
1171 if(unlikely(ret)){
1172 return -1;
1173 }
1174 gyro->data[DATA_AXIS_X] = (int16_t)((((int32_t)((int8_t)reg[1]))<< LSM6DS3TR_C_SHIFT_EIGHT_BITS)|(reg[0]));
1175 gyro->data[DATA_AXIS_Y] = (int16_t)((((int32_t)((int8_t)reg[3]))<< LSM6DS3TR_C_SHIFT_EIGHT_BITS)|(reg[2]));
1176 gyro->data[DATA_AXIS_Z] = (int16_t)((((int32_t)((int8_t)reg[5]))<< LSM6DS3TR_C_SHIFT_EIGHT_BITS)|(reg[4]));
1177
1178 if(cur_gyro_factor != 0){
1179 gyro->data[DATA_AXIS_X] = (gyro->data[DATA_AXIS_X] * cur_gyro_factor)/LSM6DS3TR_C_GYRO_MUL;
1180 gyro->data[DATA_AXIS_Y] = (gyro->data[DATA_AXIS_Y] * cur_gyro_factor)/LSM6DS3TR_C_GYRO_MUL;
1181 gyro->data[DATA_AXIS_Z] = (gyro->data[DATA_AXIS_Z] * cur_gyro_factor)/LSM6DS3TR_C_GYRO_MUL;
1182 }
1183 gyro->timestamp = aos_now_ms();
1184
1185 return (int)size;
1186 }
1187
drv_gyro_st_lsm6ds3tr_c_ioctl(int cmd,unsigned long arg)1188 static int drv_gyro_st_lsm6ds3tr_c_ioctl(int cmd, unsigned long arg)
1189 {
1190 int ret = 0;
1191
1192 dev_sensor_info_t *info = (dev_sensor_info_t *)arg;
1193
1194 switch(cmd){
1195 case SENSOR_IOCTL_ODR_SET:{
1196 ret = drv_gyro_st_lsm6ds3tr_c_set_odr(&lsm6ds3tr_c_ctx, arg);
1197 if(unlikely(ret)){
1198 return -1;
1199 }
1200 }break;
1201 case SENSOR_IOCTL_RANGE_SET:{
1202 ret = drv_gyro_st_lsm6ds3tr_c_set_range(&lsm6ds3tr_c_ctx, arg);
1203 if(unlikely(ret)){
1204 return -1;
1205 }
1206 }break;
1207 case SENSOR_IOCTL_SET_POWER:{
1208 ret = drv_gyro_st_lsm6ds3tr_c_set_power_mode(&lsm6ds3tr_c_ctx, arg);
1209 if(unlikely(ret)){
1210 return -1;
1211 }
1212 }break;
1213 case SENSOR_IOCTL_GET_INFO:{
1214 /* fill the dev info here */
1215 //dev_sensor_info_t *info = (dev_sensor_info_t *)arg;
1216 info->model = "LSM6DS3TR_C";
1217 info->range_max = 2000;
1218 info->range_min = 125;
1219 info->unit = udps;
1220 }break;
1221 case SENSOR_IOCTL_SELF_TEST:{
1222 ret = drv_gyro_st_lsm6ds3tr_c_self_test(&lsm6ds3tr_c_ctx, (int32_t*)info->data);
1223 //printf("%d %d %d\n",info->data[0],info->data[1],info->data[2]);
1224 LOG("%s %s: %d, %d, %d\n", SENSOR_STR, __func__, info->data[0],info->data[1],info->data[2]);
1225 return ret;
1226 }
1227
1228 default:break;
1229 }
1230
1231 return 0;
1232 }
1233
drv_gyro_st_lsm6ds3tr_c_init(void)1234 int drv_gyro_st_lsm6ds3tr_c_init(void){
1235 int ret = 0;
1236 sensor_obj_t sensor;
1237 memset(&sensor, 0, sizeof(sensor));
1238
1239 /* fill the sensor obj parameters here */
1240 sensor.io_port = I2C_PORT;
1241 sensor.tag = TAG_DEV_GYRO;
1242 sensor.path = dev_gyro_path;
1243 sensor.open = drv_gyro_st_lsm6ds3tr_c_open;
1244 sensor.close = drv_gyro_st_lsm6ds3tr_c_close;
1245 sensor.read = drv_gyro_st_lsm6ds3tr_c_read;
1246 sensor.write = NULL;
1247 sensor.ioctl = drv_gyro_st_lsm6ds3tr_c_ioctl;
1248 sensor.irq_handle = drv_gyro_st_lsm6ds3tr_c_irq_handle;
1249
1250 ret = sensor_create_obj(&sensor);
1251 if(unlikely(ret)){
1252 return -1;
1253 }
1254
1255 ret = drv_acc_gyro_st_lsm6ds3tr_c_validate_id(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_CHIP_ID_VALUE);
1256 if(unlikely(ret)){
1257 return -1;
1258 }
1259
1260 if(0 == g_lsm6ds3tr_cflag){
1261 ret = drv_acc_gyro_st_lsm6ds3tr_c_soft_reset(&lsm6ds3tr_c_ctx);
1262 if(unlikely(ret)){
1263 return -1;
1264 }
1265
1266 ret = drv_gyro_st_lsm6ds3tr_c_set_power_mode(&lsm6ds3tr_c_ctx, DEV_POWER_OFF);
1267 if(unlikely(ret)){
1268 return -1;
1269 }
1270 g_lsm6ds3tr_cflag = 1;
1271 }
1272 else{
1273 LOG("%s %s gyro do not need reset\n", SENSOR_STR, __func__);
1274 }
1275
1276 /* update the phy sensor info to sensor hal */
1277 LOG("%s %s successfully \n", SENSOR_STR, __func__);
1278 return 0;
1279 }
1280
1281
1282 SENSOR_DRV_ADD(drv_acc_st_lsm6ds3tr_c_init);
1283 SENSOR_DRV_ADD(drv_gyro_st_lsm6ds3tr_c_init);
1284
1285