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