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 LIS3DH_I2C_ADDR1 (0x18)
16 #define LIS3DH_I2C_ADDR2 (0x19)
17 #define LIS3DH_I2C_ADDR_TRANS(n) ((n)<<1)
18 #define LIS3DH_I2C_ADDR LIS3DH_I2C_ADDR_TRANS(LIS3DH_I2C_ADDR2)
19
20 #define LIS3DH_STATUS_REG_AUX 0x07
21
22 #define LIS3DH_ACC_OUT_TEMP_L 0x0C
23 #define LIS3DH_ACC_OUT_TEMP_H 0x0D
24 #define LIS3DH_ACC_WHO_AM_I 0x0F
25 #define LIS3DH_ACC_CTRL_REG0 0x1E
26 #define LIS3DH_ACC_TEMP_CFG_REG 0x1F
27
28 #define LIS3DH_ACC_CTRL_REG1 0x20
29 #define LIS3DH_ACC_CTRL_REG2 0x21
30 #define LIS3DH_ACC_CTRL_REG3 0x22
31 #define LIS3DH_ACC_CTRL_REG4 0x23
32 #define LIS3DH_ACC_CTRL_REG5 0x24
33 #define LIS3DH_ACC_CTRL_REG6 0x25
34 #define LIS3DH_ACC_REFERENCE 0x26
35 #define LIS3DH_ACC_STATUS_REG 0x27
36 #define LIS3DH_ACC_OUT_X_L 0x28
37 #define LIS3DH_ACC_OUT_X_H 0x29
38 #define LIS3DH_ACC_OUT_Y_L 0x2A
39 #define LIS3DH_ACC_OUT_Y_H 0x2B
40 #define LIS3DH_ACC_OUT_Z_L 0x2C
41 #define LIS3DH_ACC_OUT_Z_H 0x2D
42 #define LIS3DH_ACC_FIFO_CTRL_REG 0x2E
43 #define LIS3DH_ACC_FIFO_SRC_REG 0x2F
44 #define LIS3DH_ACC_INT1_CFG 0x30
45 #define LIS3DH_ACC_INT1_SRC 0x31
46 #define LIS3DH_ACC_INT1_THS 0x32
47 #define LIS3DH_ACC_INT1_DURATION 0x33
48 #define LIS3DH_ACC_INT2_CFG 0x34
49 #define LIS3DH_ACC_INT2_SRC 0x35
50 #define LIS3DH_ACC_INT2_THS 0x36
51 #define LIS3DH_ACC_INT2_DURATION 0x37
52 #define LIS3DH_ACC_CLICK_CFG 0x38
53 #define LIS3DH_ACC_CLICK_SRC 0x39
54 #define LIS3DH_ACC_CLICK_THS 0x3A
55 #define LIS3DH_ACC_TIME_LIMIT 0x3B
56 #define LIS3DH_ACC_TIME_LATENCY 0x3C
57 #define LIS3DH_ACC_TIME_WINDOW 0x3D
58 #define LIS3DH_ACC_ACT_THS 0x3E
59 #define LIS3DH_ACC_ACT_DUR 0x3F
60
61
62 #define LIS3DH_ACC_SELFTESTDISABLE (0x0)
63 #define LIS3DH_ACC_SELFTESTENABLE (0x2)
64 #define LIS3DH_ACC_SELFTEST_MSK (0x06)
65 #define LIS3DH_ACC_SELFTEST_POS (2)
66
67 #define LIS3DH_ACC_RANGE_2G (0x0)
68 #define LIS3DH_ACC_RANGE_4G (0x1)
69 #define LIS3DH_ACC_RANGE_8G (0x2)
70 #define LIS3DH_ACC_RANGE_16G (0x3)
71 #define LIS3DH_ACC_RANGE_MSK (0X30)
72 #define LIS3DH_ACC_RANGE_POS (4)
73
74 #define LIS3DH_ACC_SENSITIVITY_2G (1)
75 #define LIS3DH_ACC_SENSITIVITY_4G (2)
76 #define LIS3DH_ACC_SENSITIVITY_8G (4)
77 #define LIS3DH_ACC_SENSITIVITY_16G (12)
78
79 #define LIS3DH_ACC_CHIP_ID_VALUE (0x33)
80
81 #define LIS3DH_SHIFT_EIGHT_BITS (8)
82 #define LIS3DH_SHIFT_FOUR_BITS (4)
83
84 #define LIS3DH_16_BIT_SHIFT (0xFF)
85 #define LIS3DH_ACC_MUL (1000)
86
87 #define LIS3DH_ACC_ODR_POWER_DOWN (0x00)
88 #define LIS3DH_ACC_ODR_1_HZ (0x01)
89 #define LIS3DH_ACC_ODR_10_HZ (0x02)
90 #define LIS3DH_ACC_ODR_25_HZ (0x03)
91 #define LIS3DH_ACC_ODR_50_HZ (0x04)
92 #define LIS3DH_ACC_ODR_100_HZ (0x05)
93 #define LIS3DH_ACC_ODR_200_HZ (0x06)
94 #define LIS3DH_ACC_ODR_400_HZ (0x07)
95 #define LIS3DH_ACC_ODR_1_62_KHZ (0x08)
96 #define LIS3DH_ACC_ODR_5_376_KHZ (0x09)
97 #define LIS3DH_ACC_ODR_1_344_HZ (0x09)
98 #define LIS3DH_ACC_ODR_MSK (0XF0)
99 #define LIS3DH_ACC_ODR_POS (4)
100
101 #define LIS3DH_ACC_DEFAULT_ODR_100HZ (100)
102
103 #define LIS3DH_BDU_ENABLE (0x80)
104
105 #define LIS3DH_ACC_STATUS_ZYXDA (0x08)
106
107 #define LIS3DH_DEFAULT_ODR_100HZ (100)
108
109 #define LIS3DH_ACC_SELF_TEST_MIN_X (17 * 4) // 17 counts, per count 4mg
110 #define LIS3DH_ACC_SELF_TEST_MIN_Y (17 * 4) // 17 counts, per count 4mg
111 #define LIS3DH_ACC_SELF_TEST_MIN_Z (17 * 4) // 17 counts, per count 4mg
112 #define LIS3DH_ACC_SELF_TEST_MAX_X (360 * 4) // 360 counts, per count 4mg
113 #define LIS3DH_ACC_SELF_TEST_MAX_Y (360 * 4) // 360 counts, per count 4mg
114 #define LIS3DH_ACC_SELF_TEST_MAX_Z (360 * 4) // 360 counts, per count 4mg
115
116 #define LIS3DH_ACC_SELF_TEST_DRY_WAIT_CNT 5
117 #define LIS3DH_ACC_SELF_TEST_AVG_SAMPLE_CNT 5
118
119 #define LIS3DH_DEFAULT_ODR_100HZ (100)
120
121 #define LIS3DH_GET_BITSLICE(regvar, bitname)\
122 ((regvar & bitname##_MSK) >> bitname##_POS)
123
124 #define LIS3DH_SET_BITSLICE(regvar, bitname, val)\
125 ((regvar & ~bitname##_MSK) | ((val<<bitname##_POS)&bitname##_MSK))
126
127
128 static int32_t lis3dh_acc_factor[ACC_RANGE_MAX] = { LIS3DH_ACC_SENSITIVITY_2G, LIS3DH_ACC_SENSITIVITY_4G,
129 LIS3DH_ACC_SENSITIVITY_8G, LIS3DH_ACC_SENSITIVITY_16G };
130 static int32_t cur_acc_factor = 0;
131
132
133 i2c_dev_t lis3dh_ctx = {
134 .port = 3,
135 .config.address_width = 8,
136 .config.freq = 400000,
137 .config.dev_addr = LIS3DH_I2C_ADDR,
138 };
139
drv_acc_st_lis3dh_validate_id(i2c_dev_t * drv,uint8_t id_value)140 static int drv_acc_st_lis3dh_validate_id(i2c_dev_t* drv, uint8_t id_value)
141 {
142 uint8_t value = 0x00;
143 int ret = 0;
144
145 if(drv == NULL){
146 return -1;
147 }
148
149 ret = sensor_i2c_read(drv, LIS3DH_ACC_WHO_AM_I, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
150 if(unlikely(ret)){
151 return ret;
152 }
153
154 if (id_value != value){
155 return -1;
156 }
157
158 return 0;
159 }
160
drv_acc_st_lis3dh_set_power_mode(i2c_dev_t * drv,dev_power_mode_e mode)161 static int drv_acc_st_lis3dh_set_power_mode(i2c_dev_t* drv, dev_power_mode_e mode)
162 {
163 uint8_t value = 0x00;
164 int ret = 0;
165
166 ret = sensor_i2c_read(drv, LIS3DH_ACC_CTRL_REG1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
167 if(unlikely(ret)){
168 return ret;
169 }
170
171 switch(mode){
172 case DEV_POWER_ON:{
173 value = LIS3DH_SET_BITSLICE(value,LIS3DH_ACC_ODR,LIS3DH_ACC_ODR_10_HZ);
174 ret = sensor_i2c_write(drv, LIS3DH_ACC_CTRL_REG1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
175 if(unlikely(ret)){
176 return ret;
177 }
178 }break;
179
180 case DEV_POWER_OFF:{
181 value = LIS3DH_SET_BITSLICE(value,LIS3DH_ACC_ODR,LIS3DH_ACC_ODR_POWER_DOWN);
182 ret = sensor_i2c_write(drv, LIS3DH_ACC_CTRL_REG1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
183 if(unlikely(ret)){
184 return ret;
185 }
186 }break;
187
188 case DEV_SLEEP:{
189 value = LIS3DH_SET_BITSLICE(value,LIS3DH_ACC_ODR,LIS3DH_ACC_ODR_10_HZ);
190 ret = sensor_i2c_write(drv, LIS3DH_ACC_CTRL_REG1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
191 if(unlikely(ret)){
192 return ret;
193 }
194
195 }break;
196
197 default:break;
198 }
199 return 0;
200 }
201
drv_acc_st_lis3dh_hz2odr(uint32_t hz)202 static uint8_t drv_acc_st_lis3dh_hz2odr(uint32_t hz)
203 {
204 if(hz > 1620)
205 return LIS3DH_ACC_ODR_5_376_KHZ;
206 else if(hz > 1344)
207 return LIS3DH_ACC_ODR_1_62_KHZ;
208 else if(hz > 400)
209 return LIS3DH_ACC_ODR_1_344_HZ;
210 else if(hz > 200)
211 return LIS3DH_ACC_ODR_400_HZ;
212 else if(hz > 100)
213 return LIS3DH_ACC_ODR_200_HZ;
214 else if(hz > 50)
215 return LIS3DH_ACC_ODR_100_HZ;
216 else if(hz > 25)
217 return LIS3DH_ACC_ODR_50_HZ;
218 else if(hz > 10)
219 return LIS3DH_ACC_ODR_25_HZ;
220 else if(hz >= 1)
221 return LIS3DH_ACC_ODR_10_HZ;
222 else
223 return LIS3DH_ACC_ODR_1_HZ;
224
225 }
226
drv_acc_st_lis3dh_set_odr(i2c_dev_t * drv,uint32_t hz)227 static int drv_acc_st_lis3dh_set_odr(i2c_dev_t* drv, uint32_t hz)
228 {
229 int ret = 0;
230 uint8_t value = 0x00;
231 uint8_t odr = drv_acc_st_lis3dh_hz2odr(hz);
232
233 ret = sensor_i2c_read(drv, LIS3DH_ACC_CTRL_REG1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
234 if(unlikely(ret)){
235 return ret;
236 }
237
238 value = LIS3DH_SET_BITSLICE(value,LIS3DH_ACC_ODR,odr);
239
240 ret = sensor_i2c_write(drv, LIS3DH_ACC_CTRL_REG1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
241 if(unlikely(ret)){
242 return ret;
243 }
244 return 0;
245 }
246
drv_acc_st_lis3dh_set_range(i2c_dev_t * drv,uint32_t range)247 static int drv_acc_st_lis3dh_set_range(i2c_dev_t* drv, uint32_t range)
248 {
249
250 int ret = 0;
251 uint8_t value = 0x00;
252 uint8_t tmp = 0;
253
254 ret = sensor_i2c_read(drv, LIS3DH_ACC_CTRL_REG4, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
255 if(unlikely(ret)){
256 return ret;
257 }
258
259 switch (range){
260 case ACC_RANGE_2G:{
261 tmp = LIS3DH_ACC_RANGE_2G;
262 }break;
263
264 case ACC_RANGE_4G:{
265 tmp = LIS3DH_ACC_RANGE_4G;
266 }break;
267
268 case ACC_RANGE_8G:{
269 tmp = LIS3DH_ACC_RANGE_8G;
270 }break;
271
272 case ACC_RANGE_16G:{
273 tmp = LIS3DH_ACC_RANGE_16G;
274 }break;
275
276 default:break;
277 }
278
279 value = LIS3DH_SET_BITSLICE(value,LIS3DH_ACC_RANGE,tmp);
280
281 ret = sensor_i2c_write(drv, LIS3DH_ACC_CTRL_REG4, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
282 if(unlikely(ret)){
283 return ret;
284 }
285
286 if((range >= ACC_RANGE_2G)&&(range <= ACC_RANGE_16G)){
287 cur_acc_factor = lis3dh_acc_factor[range];
288 }
289
290 return 0;
291 }
292
293
drv_acc_st_lis3dh_set_bdu(i2c_dev_t * drv)294 static int drv_acc_st_lis3dh_set_bdu(i2c_dev_t* drv)
295 {
296 uint8_t value = 0x00;
297 int ret = 0;
298
299 ret = sensor_i2c_read(drv, LIS3DH_ACC_CTRL_REG4, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
300 if(unlikely(ret)){
301 return ret;
302 }
303 value |= LIS3DH_BDU_ENABLE;
304
305 ret = sensor_i2c_write(drv, LIS3DH_ACC_CTRL_REG4, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
306 if(unlikely(ret)){
307 return ret;
308 }
309 return 0;
310 }
311
drv_acc_st_lis3dh_st_discard(i2c_dev_t * drv)312 static int drv_acc_st_lis3dh_st_discard(i2c_dev_t* drv)
313 {
314 uint8_t i;
315 uint8_t value = 0x00;
316 int ret = 0;
317 uint8_t buffer[6];
318
319 for (i = 0; i < LIS3DH_ACC_SELF_TEST_DRY_WAIT_CNT; i ++) {
320 ret = sensor_i2c_read(drv, LIS3DH_ACC_STATUS_REG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
321 if(unlikely(ret)){
322 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
323 return ret;
324 }
325 if (value & 0x08)
326 break;
327
328 aos_msleep(20);
329 }
330
331 if (i >= LIS3DH_ACC_SELF_TEST_DRY_WAIT_CNT) {
332 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
333 return -1;
334 }
335
336 ret = sensor_i2c_read(drv, (LIS3DH_ACC_OUT_X_L | 0x80), buffer, 6, I2C_OP_RETRIES);
337 if(unlikely(ret)){
338 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
339 return ret;
340 }
341
342 return ret;
343 }
344
drv_acc_st_lis3dh_st_data(i2c_dev_t * drv,int32_t * data)345 static int drv_acc_st_lis3dh_st_data(i2c_dev_t* drv,int32_t* data)
346 {
347 uint8_t i, j;
348 int16_t x_raw, y_raw, z_raw;
349 int32_t x_mg, y_mg, z_mg;
350 int32_t x_sum, y_sum, z_sum;
351 uint8_t value = 0x00;
352 int ret = 0;
353 uint8_t buffer[6];
354
355 x_sum = 0; y_sum = 0; z_sum = 0;
356
357 for (i = 0; i < LIS3DH_ACC_SELF_TEST_AVG_SAMPLE_CNT; i ++) {
358 for (j = 0; j < LIS3DH_ACC_SELF_TEST_DRY_WAIT_CNT; j ++) {
359 ret = sensor_i2c_read(drv, LIS3DH_ACC_STATUS_REG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
360 if(unlikely(ret)){
361 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
362 return ret;
363 }
364 if (value & 0x08)
365 break;
366
367 aos_msleep(20);
368 }
369
370 if (j >= LIS3DH_ACC_SELF_TEST_DRY_WAIT_CNT) {
371 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
372 return -1;
373 }
374
375 ret = sensor_i2c_read(drv, (LIS3DH_ACC_OUT_X_L | 0x80), buffer, 6, I2C_OP_RETRIES);
376 if(unlikely(ret)){
377 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
378 return ret;
379 }
380
381 x_raw = (buffer[1] << 8) + buffer[0];
382 y_raw = (buffer[3] << 8) + buffer[2];
383 z_raw = (buffer[5] << 8) + buffer[4];
384 //LOG("%s %s %d: i(%d), raw(%d, %d, %d)\n", SENSOR_STR, __func__, __LINE__, i, x_raw, y_raw, z_raw);
385
386 x_mg = x_raw >> 4;
387 y_mg = y_raw >> 4;
388 z_mg = z_raw >> 4;
389 //LOG("%s %s %d: i(%d), mg(%d, %d, %d)\n", SENSOR_STR, __func__, __LINE__, i, x_mg, y_mg, z_mg);
390
391 x_sum += x_mg;
392 y_sum += y_mg;
393 z_sum += z_mg;
394 }
395
396 data[0] = x_sum / LIS3DH_ACC_SELF_TEST_AVG_SAMPLE_CNT;
397 data[1] = y_sum / LIS3DH_ACC_SELF_TEST_AVG_SAMPLE_CNT;
398 data[2] = z_sum / LIS3DH_ACC_SELF_TEST_AVG_SAMPLE_CNT;
399 return ret;
400 }
401
drv_acc_st_lis3dh_self_test(i2c_dev_t * drv,int32_t * data)402 static int drv_acc_st_lis3dh_self_test(i2c_dev_t* drv,int32_t* data)
403 {
404 uint8_t i;
405 uint8_t value = 0x00;
406 int ret = 0;
407 uint8_t ctrl_reg[4];
408 int32_t out_nost[3];
409 int32_t out_st[3];
410 int32_t out_diff[3];
411
412 // Save cfg registers which will be modified during self-test
413 ret = sensor_i2c_read(drv, (LIS3DH_ACC_CTRL_REG1 | 0x80), ctrl_reg, 4, I2C_OP_RETRIES);
414 if(unlikely(ret)){
415 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
416 return ret;
417 }
418
419 // Initialize Sensor, turn on sensor, enable X/Y/Z axes
420 // Set BDU=1, FS=2G, Normal mode, ODR = 50Hz
421 value = 0;
422 ret = sensor_i2c_write(drv, LIS3DH_ACC_CTRL_REG2, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
423 if(unlikely(ret)){
424 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
425 goto restore;
426 }
427 value = 0;
428 ret = sensor_i2c_write(drv, LIS3DH_ACC_CTRL_REG3, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
429 if(unlikely(ret)){
430 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
431 goto restore;
432 }
433
434 value = 0x80;
435 ret = sensor_i2c_write(drv, LIS3DH_ACC_CTRL_REG4, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
436 if(unlikely(ret)){
437 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
438 goto restore;
439 }
440
441 value = 0x47;
442 ret = sensor_i2c_write(drv, LIS3DH_ACC_CTRL_REG1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
443 if(unlikely(ret)){
444 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
445 goto restore;
446 }
447
448 aos_msleep(90);
449
450 // Discard the first sample
451 ret = drv_acc_st_lis3dh_st_discard(drv);
452 if(unlikely(ret)){
453 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
454 goto restore;
455 }
456
457 // Read some samples, and averate them
458 ret = drv_acc_st_lis3dh_st_data(drv, out_nost);
459 if(unlikely(ret)){
460 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
461 goto restore;
462 }
463
464 // Enable seft-test
465 value = 0x82;
466 ret = sensor_i2c_write(drv, LIS3DH_ACC_CTRL_REG4, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
467 if(unlikely(ret)){
468 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
469 goto restore;
470 }
471 aos_msleep(90);
472
473 // Discard the first sample
474 ret = drv_acc_st_lis3dh_st_discard(drv);
475 if(unlikely(ret)){
476 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
477 goto restore;
478 }
479
480 // Read some samples, and average them
481 ret = drv_acc_st_lis3dh_st_data(drv, out_st);
482 if(unlikely(ret)){
483 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
484 goto restore;
485 }
486
487 // Check if the differences are between min and max
488 for (i = 0; i < 3; i ++) {
489 out_diff[i] = abs(out_st[i] - out_nost[i]);
490 data[i] = out_diff[i];
491 }
492
493 if ((LIS3DH_ACC_SELF_TEST_MIN_X > out_diff[0]) || (out_diff[0] > LIS3DH_ACC_SELF_TEST_MAX_X)) {
494 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
495 ret = -1;
496 goto restore;
497 }
498 if ((LIS3DH_ACC_SELF_TEST_MIN_Y > out_diff[1]) || (out_diff[1] > LIS3DH_ACC_SELF_TEST_MAX_Y)) {
499 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
500 ret = -1;
501 goto restore;
502 }
503 if ((LIS3DH_ACC_SELF_TEST_MIN_Z > out_diff[2]) || (out_diff[2] > LIS3DH_ACC_SELF_TEST_MAX_Z)) {
504 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
505 ret = -1;
506 goto restore;
507 }
508
509 restore:
510 ret += sensor_i2c_write(drv, LIS3DH_ACC_CTRL_REG1 | 0x80, ctrl_reg, 4, I2C_OP_RETRIES);
511 return ret;
512 }
513
drv_acc_st_lis3dh_irq_handle(void)514 static void drv_acc_st_lis3dh_irq_handle(void)
515 {
516 /* no handle so far */
517 }
518
drv_acc_st_lis3dh_open(void)519 static int drv_acc_st_lis3dh_open(void)
520 {
521 int ret = 0;
522 ret = drv_acc_st_lis3dh_set_power_mode(&lis3dh_ctx, DEV_POWER_ON);
523 if(unlikely(ret)){
524 return -1;
525 }
526
527 ret = drv_acc_st_lis3dh_set_range(&lis3dh_ctx, ACC_RANGE_8G);
528 if(unlikely(ret)){
529 return -1;
530 }
531
532 ret = drv_acc_st_lis3dh_set_odr(&lis3dh_ctx, LIS3DH_ACC_DEFAULT_ODR_100HZ);
533 if(unlikely(ret)){
534 return -1;
535 }
536
537 return 0;
538
539 }
540
drv_acc_st_lis3dh_close(void)541 static int drv_acc_st_lis3dh_close(void)
542 {
543 int ret = 0;
544 ret = drv_acc_st_lis3dh_set_power_mode(&lis3dh_ctx, DEV_POWER_OFF);
545 if(unlikely(ret)){
546 return -1;
547 }
548 return 0;
549 }
550
drv_acc_st_lis3dh_read(void * buf,size_t len)551 static int drv_acc_st_lis3dh_read(void *buf, size_t len)
552 {
553
554 int ret = 0;
555 size_t size;
556 uint8_t reg[6];
557 accel_data_t *accel = (accel_data_t *)buf;
558 if(buf == NULL){
559 return -1;
560 }
561
562 size = sizeof(accel_data_t);
563 if(len < size){
564 return -1;
565 }
566
567 ret = sensor_i2c_read(&lis3dh_ctx, (LIS3DH_ACC_OUT_X_L | 0x80), reg, 6, I2C_OP_RETRIES);
568
569 if(unlikely(ret)){
570 return -1;
571 }
572 accel->data[DATA_AXIS_X] = (int16_t)((((int16_t)((int8_t)reg[1]))<< LIS3DH_SHIFT_EIGHT_BITS)|(reg[0]));
573 accel->data[DATA_AXIS_X] = accel->data[DATA_AXIS_X] >> LIS3DH_SHIFT_FOUR_BITS;
574 accel->data[DATA_AXIS_Y] = (int16_t)((((int16_t)((int8_t)reg[3]))<< LIS3DH_SHIFT_EIGHT_BITS)|(reg[2]));
575 accel->data[DATA_AXIS_Y] = accel->data[DATA_AXIS_Y] >> LIS3DH_SHIFT_FOUR_BITS;
576 accel->data[DATA_AXIS_Z] = (int16_t)((((int16_t)((int8_t)reg[5]))<< LIS3DH_SHIFT_EIGHT_BITS)|(reg[4]));
577 accel->data[DATA_AXIS_Z] = accel->data[DATA_AXIS_Z] >> LIS3DH_SHIFT_FOUR_BITS;
578
579 if(cur_acc_factor != 0){
580 accel->data[DATA_AXIS_X] = accel->data[DATA_AXIS_X] * cur_acc_factor;
581 accel->data[DATA_AXIS_Y] = accel->data[DATA_AXIS_Y] * cur_acc_factor;
582 accel->data[DATA_AXIS_Z] = accel->data[DATA_AXIS_Z] * cur_acc_factor;
583 }
584
585 accel->timestamp = aos_now_ms();
586
587 return (int)size;
588 }
589
590
drv_acc_st_lis3dh_ioctl(int cmd,unsigned long arg)591 static int drv_acc_st_lis3dh_ioctl(int cmd, unsigned long arg)
592 {
593 int ret = 0;
594 dev_sensor_info_t *info = (dev_sensor_info_t *)arg;
595
596 switch(cmd){
597 case SENSOR_IOCTL_ODR_SET:{
598 ret = drv_acc_st_lis3dh_set_odr(&lis3dh_ctx, arg);
599 if(unlikely(ret)){
600 return -1;
601 }
602 }break;
603 case SENSOR_IOCTL_RANGE_SET:{
604 ret = drv_acc_st_lis3dh_set_range(&lis3dh_ctx, arg);
605 if(unlikely(ret)){
606 return -1;
607 }
608 }break;
609 case SENSOR_IOCTL_SET_POWER:{
610 ret = drv_acc_st_lis3dh_set_power_mode(&lis3dh_ctx, arg);
611 if(unlikely(ret)){
612 return -1;
613 }
614 }break;
615 case SENSOR_IOCTL_GET_INFO:{
616 /* fill the dev info here */
617 info->model = "LIS3DH";
618 info->range_max = 16;
619 info->range_min = 2;
620 info->unit = mg;
621
622 }break;
623 case SENSOR_IOCTL_SELF_TEST:{
624 ret = drv_acc_st_lis3dh_self_test(&lis3dh_ctx, (int32_t*)info->data);
625 //printf("%d %d %d\n",info->data[0],info->data[1],info->data[2]);
626 LOG("%s %s: %d, %d, %d\n", SENSOR_STR, __func__, info->data[0],info->data[1],info->data[2]);
627 return ret;
628 }
629 default:break;
630 }
631
632 return 0;
633 }
634
drv_acc_st_lis3dh_init(void)635 int drv_acc_st_lis3dh_init(void){
636 int ret = 0;
637 sensor_obj_t sensor;
638 memset(&sensor, 0, sizeof(sensor));
639
640 /* fill the sensor obj parameters here */
641 sensor.io_port = I2C_PORT;
642 sensor.tag = TAG_DEV_ACC;
643 sensor.path = dev_acc_path;
644 sensor.open = drv_acc_st_lis3dh_open;
645 sensor.close = drv_acc_st_lis3dh_close;
646 sensor.read = drv_acc_st_lis3dh_read;
647 sensor.write = NULL;
648 sensor.ioctl = drv_acc_st_lis3dh_ioctl;
649 sensor.irq_handle = drv_acc_st_lis3dh_irq_handle;
650
651 ret = sensor_create_obj(&sensor);
652 if(unlikely(ret)){
653 return -1;
654 }
655
656 ret = drv_acc_st_lis3dh_validate_id(&lis3dh_ctx, LIS3DH_ACC_CHIP_ID_VALUE);
657 if(unlikely(ret)){
658 return -1;
659 }
660
661 ret = drv_acc_st_lis3dh_set_range(&lis3dh_ctx, ACC_RANGE_8G);
662 if(unlikely(ret)){
663 return -1;
664 }
665
666 ret = drv_acc_st_lis3dh_set_bdu(&lis3dh_ctx);
667 if(unlikely(ret)){
668 return -1;
669 }
670
671 //set odr is 100hz, and will update
672 ret = drv_acc_st_lis3dh_set_odr(&lis3dh_ctx, LIS3DH_DEFAULT_ODR_100HZ);
673 if(unlikely(ret)){
674 return -1;
675 }
676
677 /* update the phy sensor info to sensor hal */
678 LOG("%s %s successfully \n", SENSOR_STR, __func__);
679 return 0;
680 }
681
682 SENSOR_DRV_ADD(drv_acc_st_lis3dh_init);
683
684
685