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 LSM303AGR_MAG_I2C_ADDR1 0x1E
16 #define LSM303AGR_MAG_I2C_ADDR_TRANS(n) ((n)<<1)
17 #define LSM303AGR_MAG_I2C_ADDR LSM303AGR_MAG_I2C_ADDR_TRANS(LSM303AGR_MAG_I2C_ADDR1)
18 #define LSM303AGR_MAG_BIT(x) (x)
19
20 #define LSM303AGR_ACC_I2C_ADDR1 (0x19)
21 #define LSM303AGR_ACC_I2C_ADDR_TRANS(n) ((n)<<1)
22 #define LSM303AGR_ACC_I2C_ADDR LSM303AGR_ACC_I2C_ADDR_TRANS(LSM303AGR_ACC_I2C_ADDR1)
23
24 #define LSM303AGR_ACC_STATUS_REG_AUX 0x07
25
26 #define LSM303AGR_ACC_OUT_TEMP_L 0x0C
27 #define LSM303AGR_ACC_OUT_TEMP_H 0x0D
28 #define LSM303AGR_ACC_WHO_AM_I 0x0F
29 #define LSM303AGR_ACC_CTRL_REG0 0x1E
30 #define LSM303AGR_ACC_TEMP_CFG_REG 0x1F
31
32 #define LSM303AGR_ACC_CTRL_REG1 0x20
33 #define LSM303AGR_ACC_CTRL_REG2 0x21
34 #define LSM303AGR_ACC_CTRL_REG3 0x22
35 #define LSM303AGR_ACC_CTRL_REG4 0x23
36 #define LSM303AGR_ACC_CTRL_REG5 0x24
37 #define LSM303AGR_ACC_CTRL_REG6 0x25
38 #define LSM303AGR_ACC_REFERENCE 0x26
39 #define LSM303AGR_ACC_STATUS_REG 0x27
40 #define LSM303AGR_ACC_OUT_X_L 0x28
41 #define LSM303AGR_ACC_OUT_X_H 0x29
42 #define LSM303AGR_ACC_OUT_Y_L 0x2A
43 #define LSM303AGR_ACC_OUT_Y_H 0x2B
44 #define LSM303AGR_ACC_OUT_Z_L 0x2C
45 #define LSM303AGR_ACC_OUT_Z_H 0x2D
46 #define LSM303AGR_ACC_FIFO_CTRL_REG 0x2E
47 #define LSM303AGR_ACC_FIFO_SRC_REG 0x2F
48 #define LSM303AGR_ACC_INT1_CFG 0x30
49 #define LSM303AGR_ACC_INT1_SRC 0x31
50 #define LSM303AGR_ACC_INT1_THS 0x32
51 #define LSM303AGR_ACC_INT1_DURATION 0x33
52 #define LSM303AGR_ACC_INT2_CFG 0x34
53 #define LSM303AGR_ACC_INT2_SRC 0x35
54 #define LSM303AGR_ACC_INT2_THS 0x36
55 #define LSM303AGR_ACC_INT2_DURATION 0x37
56 #define LSM303AGR_ACC_CLICK_CFG 0x38
57 #define LSM303AGR_ACC_CLICK_SRC 0x39
58 #define LSM303AGR_ACC_CLICK_THS 0x3A
59 #define LSM303AGR_ACC_TIME_LIMIT 0x3B
60 #define LSM303AGR_ACC_TIME_LATENCY 0x3C
61 #define LSM303AGR_ACC_TIME_WINDOW 0x3D
62 #define LSM303AGR_ACC_ACT_THS 0x3E
63 #define LSM303AGR_ACC_ACT_DUR 0x3F
64
65
66 #define LSM303AGR_ACC_SELFTESTDISABLE (0x0)
67 #define LSM303AGR_ACC_SELFTESTENABLE (0x2)
68 #define LSM303AGR_ACC_SELFTEST_MSK (0x06)
69 #define LSM303AGR_ACC_SELFTEST_POS (2)
70
71 #define LSM303AGR_ACC_RANGE_2G (0x0)
72 #define LSM303AGR_ACC_RANGE_4G (0x1)
73 #define LSM303AGR_ACC_RANGE_8G (0x2)
74 #define LSM303AGR_ACC_RANGE_16G (0x3)
75 #define LSM303AGR_ACC_RANGE_MSK (0X30)
76 #define LSM303AGR_ACC_RANGE_POS (4)
77
78 #define LSM303AGR_ACC_SENSITIVITY_2G (1)
79 #define LSM303AGR_ACC_SENSITIVITY_4G (2)
80 #define LSM303AGR_ACC_SENSITIVITY_8G (4)
81 #define LSM303AGR_ACC_SENSITIVITY_16G (12)
82
83 #define LSM303AGR_ACC_CHIP_ID_VALUE (0x33)
84
85 #define LSM303AGR_ACC_SHIFT_EIGHT_BITS (8)
86 #define LSM303AGR_ACC_SHIFT_FOUR_BITS (4)
87
88 #define LSM303AGR_ACC_16_BIT_SHIFT (0xFF)
89 #define LSM303AGR_ACC_MUL (1000)
90
91 #define LSM303AGR_ACC_ODR_POWER_DOWN (0x00)
92 #define LSM303AGR_ACC_ODR_1_HZ (0x01)
93 #define LSM303AGR_ACC_ODR_10_HZ (0x02)
94 #define LSM303AGR_ACC_ODR_25_HZ (0x03)
95 #define LSM303AGR_ACC_ODR_50_HZ (0x04)
96 #define LSM303AGR_ACC_ODR_100_HZ (0x05)
97 #define LSM303AGR_ACC_ODR_200_HZ (0x06)
98 #define LSM303AGR_ACC_ODR_400_HZ (0x07)
99 #define LSM303AGR_ACC_ODR_1_62_KHZ (0x08)
100 #define LSM303AGR_ACC_ODR_5_376_KHZ (0x09)
101 #define LSM303AGR_ACC_ODR_1_344_HZ (0x09)
102 #define LSM303AGR_ACC_ODR_MSK (0XF0)
103 #define LSM303AGR_ACC_ODR_POS (4)
104
105 #define LSM303AGR_ACC_DEFAULT_ODR_100HZ (100)
106
107 #define LSM303AGR_ACC_BDU_ENABLE (0x80)
108
109 #define LSM303AGR_ACC_STATUS_ZYXDA (0x08)
110
111 #define LSM303AGR_ACC_DEFAULT_ODR_100HZ (100)
112
113 #define LSM303AGR_ACC_SELF_TEST_MIN_X (17 * 4) // 17 counts, per count 4mg
114 #define LSM303AGR_ACC_SELF_TEST_MIN_Y (17 * 4) // 17 counts, per count 4mg
115 #define LSM303AGR_ACC_SELF_TEST_MIN_Z (17 * 4) // 17 counts, per count 4mg
116 #define LSM303AGR_ACC_SELF_TEST_MAX_X (360 * 4) // 360 counts, per count 4mg
117 #define LSM303AGR_ACC_SELF_TEST_MAX_Y (360 * 4) // 360 counts, per count 4mg
118 #define LSM303AGR_ACC_SELF_TEST_MAX_Z (360 * 4) // 360 counts, per count 4mg
119
120 #define LSM303AGR_ACC_SELF_TEST_DRY_WAIT_CNT 5
121 #define LSM303AGR_ACC_SELF_TEST_AVG_SAMPLE_CNT 5
122
123 #define LSM303AGR_ACC_GET_BITSLICE(regvar, bitname)\
124 ((regvar & bitname##_MSK) >> bitname##_POS)
125
126 #define LSM303AGR_ACC_SET_BITSLICE(regvar, bitname, val)\
127 ((regvar & ~bitname##_MSK) | ((val<<bitname##_POS)&bitname##_MSK))
128
129
130 static int32_t lsm303agr_acc_factor[ACC_RANGE_MAX] = { LSM303AGR_ACC_SENSITIVITY_2G, LSM303AGR_ACC_SENSITIVITY_4G,
131 LSM303AGR_ACC_SENSITIVITY_8G, LSM303AGR_ACC_SENSITIVITY_16G };
132 static int32_t cur_acc_factor = 0;
133
134 #define LSM303AGR_BIT(x) (x)
135
136 #define LSM303AGR_MAG_OFFSET_X_REG_L 0x45
137 #define LSM303AGR_MAG_OFFSET_X_REG_H 0x46
138 #define LSM303AGR_MAG_OFFSET_Y_REG_L 0x47
139 #define LSM303AGR_MAG_OFFSET_Y_REG_H 0x48
140 #define LSM303AGR_MAG_OFFSET_Z_REG_L 0x49
141 #define LSM303AGR_MAG_OFFSET_Z_REG_H 0x4A
142 #define LSM303AGR_MAG_WHO_AM_I 0x4F
143 #define LSM303AGR_MAG_CFG_REG_A 0x60
144 #define LSM303AGR_MAG_CFG_REG_B 0x61
145 #define LSM303AGR_MAG_CFG_REG_C 0x62
146 #define LSM303AGR_MAG_INT_CRTL_REG 0x63
147 #define LSM303AGR_MAG_INT_SOURCE_REG 0x64
148 #define LSM303AGR_MAG_INT_THS_L_REG 0x65
149 #define LSM303AGR_MAG_INT_THS_H_REG 0x66
150 #define LSM303AGR_MAG_STATUS_REG 0x67
151 #define LSM303AGR_MAG_OUTX_L_REG 0x68
152 #define LSM303AGR_MAG_OUTX_H_REG 0x69
153 #define LSM303AGR_MAG_OUTY_L_REG 0x6A
154 #define LSM303AGR_MAG_OUTY_H_REG 0x6B
155 #define LSM303AGR_MAG_OUTZ_L_REG 0x6C
156 #define LSM303AGR_MAG_OUTZ_H_REG 0x6D
157 #define LSM303AGR_MAG_TEMP_OUT_L_REG 0x6E
158 #define LSM303AGR_MAG_TEMP_OUT_H_REG 0x6F
159
160 #define I_AM_LSM303AGR 0x40
161
162 #define LSM303AGR_MAG_ODR_BIT LSM303AGR_BIT(0x0C)
163 #define LSM303AGR_MAG_ODR_10_HZ 0x00
164 #define LSM303AGR_MAG_ODR_20_HZ 0x04
165 #define LSM303AGR_MAG_ODR_50_HZ 0x08
166 #define LSM303AGR_MAG_ODR_100_HZ 0x0C
167
168 #define LSM303AGR_MAG_SELFTEST_DISABLE 0x00
169 #define LSM303AGR_MAG_SELFTEST_ENABLE 0x02
170
171 #define LSM303AGR_MAG_FS_50_GA 0x00
172
173 #define LSM303AGR_MAG_REBOOT_DEFAULT 0x00
174 #define LSM303AGR_MAG_REBOOT_ENABLE 0x40
175
176 #define LSM303AGR_MAG_SOFT_RESET_DEFAULT 0x00
177 #define LSM303AGR_MAG_SOFT_RESET_ENABLE 0x02
178
179 #define LSM303AGR_MAG_CONFIG_LOWPOWER_BIT LSM303AGR_BIT(0x10)
180 #define LSM303AGR_MAG_CONFIG_NORMAL_MODE 0x00
181 #define LSM303AGR_MAG_CONFIG_LOWPOWER_MODE 0x10
182
183 #define LSM303AGR_MAG_POWERMODE_BIT LSM303AGR_BIT(0x03)
184 #define LSM303AGR_MAG_CONTINUOUS_MODE 0x00
185 #define LSM303AGR_MAG_SINGLE_MODE 0x01
186 #define LSM303AGR_MAG_POWERDOWN1_MODE 0x02
187 #define LSM303AGR_MAG_POWERDOWN2_MODE 0x03
188
189 #define LSM303AGR_MAG_COMP_TEMP_EN 0x80
190
191 #define LSM303AGR_MAG_OFF_CANC 0x02
192
193 #define LSM303AGR_MAG_BLE_BIT LSM303AGR_BIT(0x08)
194 #define LSM303AGR_MAG_BLE_LSB 0x00
195 #define LSM303AGR_MAG_BLE_MSB 0x08
196
197 #define LSM303AGR_MAG_BDU_BIT LSM303AGR_BIT(0x10)
198 #define LSM303AGR_MAG_BDU_CONTINUOUS 0x00
199 #define LSM303AGR_MAG_BDU_MSBLSB 0x10
200
201 //#define LSM303AGR_MAG_SENSITIVITY_FOR_FS_50GA 15/10
202 #define LSM303AGR_MAG_SENSITIVITY_FOR_FS_50GA 1500
203
204 #define LSM303AGR_MAG_SELF_TEST_MIN_X (15) // 15mGuass
205 #define LSM303AGR_MAG_SELF_TEST_MIN_Y (15) // 15mGuass
206 #define LSM303AGR_MAG_SELF_TEST_MIN_Z (15) // 15mGuass
207 #define LSM303AGR_MAG_SELF_TEST_MAX_X (500) // 500mGuass
208 #define LSM303AGR_MAG_SELF_TEST_MAX_Y (500) // 500mGuass
209 #define LSM303AGR_MAG_SELF_TEST_MAX_Z (500) // 500mGuass
210
211 #define LSM303AGR_MAG_SELF_TEST_DRY_WAIT_CNT 5
212 #define LSM303AGR_MAG_SELF_TEST_AVG_SAMPLE_CNT 50
213
214 i2c_dev_t lsm303agr_mag_ctx = {
215 .port = 3,
216 .config.address_width = 8,
217 .config.freq = 400000,
218 .config.dev_addr = LSM303AGR_MAG_I2C_ADDR,
219 };
220
221 i2c_dev_t lsm303agr_acc_ctx = {
222 .port = 3,
223 .config.address_width = 8,
224 .config.freq = 400000,
225 .config.dev_addr = LSM303AGR_ACC_I2C_ADDR,
226 };
227
drv_acc_st_lsm303agr_validate_id(i2c_dev_t * drv,uint8_t id_value)228 static int drv_acc_st_lsm303agr_validate_id(i2c_dev_t* drv, uint8_t id_value)
229 {
230 uint8_t value = 0x00;
231 int ret = 0;
232
233 if(drv == NULL){
234 return -1;
235 }
236
237 ret = sensor_i2c_read(drv, LSM303AGR_ACC_WHO_AM_I, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
238 if(unlikely(ret)){
239 return ret;
240 }
241
242 if (id_value != value){
243 return -1;
244 }
245
246 return 0;
247 }
248
drv_acc_st_lsm303agr_set_power_mode(i2c_dev_t * drv,dev_power_mode_e mode)249 static int drv_acc_st_lsm303agr_set_power_mode(i2c_dev_t* drv, dev_power_mode_e mode)
250 {
251 uint8_t value = 0x00;
252 int ret = 0;
253
254 ret = sensor_i2c_read(drv, LSM303AGR_ACC_CTRL_REG1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
255 if(unlikely(ret)){
256 return ret;
257 }
258
259 switch(mode){
260 case DEV_POWER_ON:{
261 value = LSM303AGR_ACC_SET_BITSLICE(value,LSM303AGR_ACC_ODR,LSM303AGR_ACC_ODR_10_HZ);
262 ret = sensor_i2c_write(drv, LSM303AGR_ACC_CTRL_REG1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
263 if(unlikely(ret)){
264 return ret;
265 }
266 }break;
267
268 case DEV_POWER_OFF:{
269 value = LSM303AGR_ACC_SET_BITSLICE(value,LSM303AGR_ACC_ODR,LSM303AGR_ACC_ODR_POWER_DOWN);
270 ret = sensor_i2c_write(drv, LSM303AGR_ACC_CTRL_REG1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
271 if(unlikely(ret)){
272 return ret;
273 }
274 }break;
275
276 case DEV_SLEEP:{
277 value = LSM303AGR_ACC_SET_BITSLICE(value,LSM303AGR_ACC_ODR,LSM303AGR_ACC_ODR_10_HZ);
278 ret = sensor_i2c_write(drv, LSM303AGR_ACC_CTRL_REG1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
279 if(unlikely(ret)){
280 return ret;
281 }
282
283 }break;
284
285 default:break;
286 }
287 return 0;
288 }
289
drv_acc_st_lsm303agr_hz2odr(uint32_t hz)290 static uint8_t drv_acc_st_lsm303agr_hz2odr(uint32_t hz)
291 {
292 if(hz > 1620)
293 return LSM303AGR_ACC_ODR_5_376_KHZ;
294 else if(hz > 1344)
295 return LSM303AGR_ACC_ODR_1_62_KHZ;
296 else if(hz > 400)
297 return LSM303AGR_ACC_ODR_1_344_HZ;
298 else if(hz > 200)
299 return LSM303AGR_ACC_ODR_400_HZ;
300 else if(hz > 100)
301 return LSM303AGR_ACC_ODR_200_HZ;
302 else if(hz > 50)
303 return LSM303AGR_ACC_ODR_100_HZ;
304 else if(hz > 25)
305 return LSM303AGR_ACC_ODR_50_HZ;
306 else if(hz > 10)
307 return LSM303AGR_ACC_ODR_25_HZ;
308 else if(hz >= 1)
309 return LSM303AGR_ACC_ODR_10_HZ;
310 else
311 return LSM303AGR_ACC_ODR_1_HZ;
312
313 }
314
drv_acc_st_lsm303agr_set_odr(i2c_dev_t * drv,uint32_t hz)315 static int drv_acc_st_lsm303agr_set_odr(i2c_dev_t* drv, uint32_t hz)
316 {
317 int ret = 0;
318 uint8_t value = 0x00;
319 uint8_t odr = drv_acc_st_lsm303agr_hz2odr(hz);
320
321 ret = sensor_i2c_read(drv, LSM303AGR_ACC_CTRL_REG1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
322 if(unlikely(ret)){
323 return ret;
324 }
325
326 value = LSM303AGR_ACC_SET_BITSLICE(value,LSM303AGR_ACC_ODR,odr);
327
328 ret = sensor_i2c_write(drv, LSM303AGR_ACC_CTRL_REG1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
329 if(unlikely(ret)){
330 return ret;
331 }
332 return 0;
333 }
334
drv_acc_st_lsm303agr_set_range(i2c_dev_t * drv,uint32_t range)335 static int drv_acc_st_lsm303agr_set_range(i2c_dev_t* drv, uint32_t range)
336 {
337
338 int ret = 0;
339 uint8_t value = 0x00;
340 uint8_t tmp = 0;
341
342 ret = sensor_i2c_read(drv, LSM303AGR_ACC_CTRL_REG4, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
343 if(unlikely(ret)){
344 return ret;
345 }
346
347 switch (range){
348 case ACC_RANGE_2G:{
349 tmp = LSM303AGR_ACC_RANGE_2G;
350 }break;
351
352 case ACC_RANGE_4G:{
353 tmp = LSM303AGR_ACC_RANGE_4G;
354 }break;
355
356 case ACC_RANGE_8G:{
357 tmp = LSM303AGR_ACC_RANGE_8G;
358 }break;
359
360 case ACC_RANGE_16G:{
361 tmp = LSM303AGR_ACC_RANGE_16G;
362 }break;
363
364 default:break;
365 }
366
367 value = LSM303AGR_ACC_SET_BITSLICE(value,LSM303AGR_ACC_RANGE,tmp);
368
369 ret = sensor_i2c_write(drv, LSM303AGR_ACC_CTRL_REG4, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
370 if(unlikely(ret)){
371 return ret;
372 }
373
374 if((range >= ACC_RANGE_2G)&&(range <= ACC_RANGE_16G)){
375 cur_acc_factor = lsm303agr_acc_factor[range];
376 }
377
378 return 0;
379 }
380
381
drv_acc_st_lsm303agr_set_bdu(i2c_dev_t * drv)382 static int drv_acc_st_lsm303agr_set_bdu(i2c_dev_t* drv)
383 {
384 uint8_t value = 0x00;
385 int ret = 0;
386
387 ret = sensor_i2c_read(drv, LSM303AGR_ACC_CTRL_REG4, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
388 if(unlikely(ret)){
389 return ret;
390 }
391 value |= LSM303AGR_ACC_BDU_ENABLE;
392
393 ret = sensor_i2c_write(drv, LSM303AGR_ACC_CTRL_REG4, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
394 if(unlikely(ret)){
395 return ret;
396 }
397 return 0;
398 }
399
drv_acc_st_lsm303agr_st_discard(i2c_dev_t * drv)400 static int drv_acc_st_lsm303agr_st_discard(i2c_dev_t* drv)
401 {
402 uint8_t i;
403 uint8_t value = 0x00;
404 int ret = 0;
405 uint8_t buffer[6];
406
407 for (i = 0; i < LSM303AGR_ACC_SELF_TEST_DRY_WAIT_CNT; i ++) {
408 ret = sensor_i2c_read(drv, LSM303AGR_ACC_STATUS_REG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
409 if(unlikely(ret)){
410 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
411 return ret;
412 }
413 if (value & 0x08)
414 break;
415
416 aos_msleep(20);
417 }
418
419 if (i >= LSM303AGR_ACC_SELF_TEST_DRY_WAIT_CNT) {
420 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
421 return -1;
422 }
423
424 ret = sensor_i2c_read(drv, (LSM303AGR_ACC_OUT_X_L | 0x80), buffer, 6, I2C_OP_RETRIES);
425 if(unlikely(ret)){
426 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
427 return ret;
428 }
429
430 return ret;
431 }
432
drv_acc_st_lsm303agr_st_data(i2c_dev_t * drv,int32_t * data)433 static int drv_acc_st_lsm303agr_st_data(i2c_dev_t* drv,int32_t* data)
434 {
435 uint8_t i, j;
436 int16_t x_raw, y_raw, z_raw;
437 int32_t x_mg, y_mg, z_mg;
438 int32_t x_sum, y_sum, z_sum;
439 uint8_t value = 0x00;
440 int ret = 0;
441 uint8_t buffer[6];
442
443 x_sum = 0; y_sum = 0; z_sum = 0;
444
445 for (i = 0; i < LSM303AGR_ACC_SELF_TEST_AVG_SAMPLE_CNT; i ++) {
446 for (j = 0; j < LSM303AGR_ACC_SELF_TEST_DRY_WAIT_CNT; j ++) {
447 ret = sensor_i2c_read(drv, LSM303AGR_ACC_STATUS_REG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
448 if(unlikely(ret)){
449 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
450 return ret;
451 }
452 if (value & 0x08)
453 break;
454
455 aos_msleep(20);
456 }
457
458 if (j >= LSM303AGR_ACC_SELF_TEST_DRY_WAIT_CNT) {
459 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
460 return -1;
461 }
462
463 ret = sensor_i2c_read(drv, (LSM303AGR_ACC_OUT_X_L | 0x80), buffer, 6, I2C_OP_RETRIES);
464 if(unlikely(ret)){
465 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
466 return ret;
467 }
468
469 x_raw = (buffer[1] << 8) + buffer[0];
470 y_raw = (buffer[3] << 8) + buffer[2];
471 z_raw = (buffer[5] << 8) + buffer[4];
472 //LOG("%s %s %d: i(%d), raw(%d, %d, %d)\n", SENSOR_STR, __func__, __LINE__, i, x_raw, y_raw, z_raw);
473
474 x_mg = x_raw >> 4;
475 y_mg = y_raw >> 4;
476 z_mg = z_raw >> 4;
477 //LOG("%s %s %d: i(%d), mg(%d, %d, %d)\n", SENSOR_STR, __func__, __LINE__, i, x_mg, y_mg, z_mg);
478
479 x_sum += x_mg;
480 y_sum += y_mg;
481 z_sum += z_mg;
482 }
483
484 data[0] = x_sum / LSM303AGR_ACC_SELF_TEST_AVG_SAMPLE_CNT;
485 data[1] = y_sum / LSM303AGR_ACC_SELF_TEST_AVG_SAMPLE_CNT;
486 data[2] = z_sum / LSM303AGR_ACC_SELF_TEST_AVG_SAMPLE_CNT;
487 return ret;
488 }
489
drv_acc_st_lsm303agr_self_test(i2c_dev_t * drv,int32_t * data)490 static int drv_acc_st_lsm303agr_self_test(i2c_dev_t* drv,int32_t* data)
491 {
492 uint8_t i;
493 uint8_t value = 0x00;
494 int ret = 0;
495 uint8_t ctrl_reg[4];
496 int32_t out_nost[3];
497 int32_t out_st[3];
498 int32_t out_diff[3];
499
500 // Save cfg registers which will be modified during self-test
501 ret = sensor_i2c_read(drv, (LSM303AGR_ACC_CTRL_REG1 | 0x80), ctrl_reg, 4, I2C_OP_RETRIES);
502 if(unlikely(ret)){
503 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
504 return ret;
505 }
506
507 // Initialize Sensor, turn on sensor, enable X/Y/Z axes
508 // Set BDU=1, FS=2G, Normal mode, ODR = 50Hz
509 value = 0;
510 ret = sensor_i2c_write(drv, LSM303AGR_ACC_CTRL_REG2, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
511 if(unlikely(ret)){
512 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
513 goto restore;
514 }
515 value = 0;
516 ret = sensor_i2c_write(drv, LSM303AGR_ACC_CTRL_REG3, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
517 if(unlikely(ret)){
518 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
519 goto restore;
520 }
521
522 value = 0x80;
523 ret = sensor_i2c_write(drv, LSM303AGR_ACC_CTRL_REG4, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
524 if(unlikely(ret)){
525 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
526 goto restore;
527 }
528
529 value = 0x47;
530 ret = sensor_i2c_write(drv, LSM303AGR_ACC_CTRL_REG1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
531 if(unlikely(ret)){
532 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
533 goto restore;
534 }
535
536 aos_msleep(90);
537
538 // Discard the first sample
539 ret = drv_acc_st_lsm303agr_st_discard(drv);
540 if(unlikely(ret)){
541 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
542 goto restore;
543 }
544
545 // Read some samples, and averate them
546 ret = drv_acc_st_lsm303agr_st_data(drv, out_nost);
547 if(unlikely(ret)){
548 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
549 goto restore;
550 }
551
552 // Enable seft-test
553 value = 0x82;
554 ret = sensor_i2c_write(drv, LSM303AGR_ACC_CTRL_REG4, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
555 if(unlikely(ret)){
556 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
557 goto restore;
558 }
559 aos_msleep(90);
560
561 // Discard the first sample
562 ret = drv_acc_st_lsm303agr_st_discard(drv);
563 if(unlikely(ret)){
564 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
565 goto restore;
566 }
567
568 // Read some samples, and average them
569 ret = drv_acc_st_lsm303agr_st_data(drv, out_st);
570 if(unlikely(ret)){
571 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
572 goto restore;
573 }
574
575 // Check if the differences are between min and max
576 for (i = 0; i < 3; i ++) {
577 out_diff[i] = abs(out_st[i] - out_nost[i]);
578 data[i] = out_diff[i];
579 }
580
581 if ((LSM303AGR_ACC_SELF_TEST_MIN_X > out_diff[0]) || (out_diff[0] > LSM303AGR_ACC_SELF_TEST_MAX_X)) {
582 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
583 ret = -1;
584 goto restore;
585 }
586 if ((LSM303AGR_ACC_SELF_TEST_MIN_Y > out_diff[1]) || (out_diff[1] > LSM303AGR_ACC_SELF_TEST_MAX_Y)) {
587 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
588 ret = -1;
589 goto restore;
590 }
591 if ((LSM303AGR_ACC_SELF_TEST_MIN_Z > out_diff[2]) || (out_diff[2] > LSM303AGR_ACC_SELF_TEST_MAX_Z)) {
592 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
593 ret = -1;
594 goto restore;
595 }
596
597 restore:
598 ret += sensor_i2c_write(drv, LSM303AGR_ACC_CTRL_REG1 | 0x80, ctrl_reg, 4, I2C_OP_RETRIES);
599 return ret;
600 }
601
drv_acc_st_lsm303agr_irq_handle(void)602 static void drv_acc_st_lsm303agr_irq_handle(void)
603 {
604 /* no handle so far */
605 }
606
drv_acc_st_lsm303agr_open(void)607 static int drv_acc_st_lsm303agr_open(void)
608 {
609 int ret = 0;
610 ret = drv_acc_st_lsm303agr_set_power_mode(&lsm303agr_acc_ctx, DEV_POWER_ON);
611 if(unlikely(ret)){
612 return -1;
613 }
614
615 ret = drv_acc_st_lsm303agr_set_range(&lsm303agr_acc_ctx, ACC_RANGE_8G);
616 if(unlikely(ret)){
617 return -1;
618 }
619
620 ret = drv_acc_st_lsm303agr_set_odr(&lsm303agr_acc_ctx, LSM303AGR_ACC_DEFAULT_ODR_100HZ);
621 if(unlikely(ret)){
622 return -1;
623 }
624
625 return 0;
626
627 }
628
drv_acc_st_lsm303agr_close(void)629 static int drv_acc_st_lsm303agr_close(void)
630 {
631 int ret = 0;
632 ret = drv_acc_st_lsm303agr_set_power_mode(&lsm303agr_acc_ctx, DEV_POWER_OFF);
633 if(unlikely(ret)){
634 return -1;
635 }
636 return 0;
637 }
638
drv_acc_st_lsm303agr_read(void * buf,size_t len)639 static int drv_acc_st_lsm303agr_read(void *buf, size_t len)
640 {
641
642 int ret = 0;
643 size_t size;
644 uint8_t reg[6];
645 accel_data_t *accel = (accel_data_t *)buf;
646 if(buf == NULL){
647 return -1;
648 }
649
650 size = sizeof(accel_data_t);
651 if(len < size){
652 return -1;
653 }
654
655 ret = sensor_i2c_read(&lsm303agr_acc_ctx, (LSM303AGR_ACC_OUT_X_L | 0x80), reg, 6, I2C_OP_RETRIES);
656
657 if(unlikely(ret)){
658 return -1;
659 }
660 accel->data[DATA_AXIS_X] = (int16_t)((((int16_t)((int8_t)reg[1]))<< LSM303AGR_ACC_SHIFT_EIGHT_BITS)|(reg[0]));
661 accel->data[DATA_AXIS_X] = accel->data[DATA_AXIS_X] >> LSM303AGR_ACC_SHIFT_FOUR_BITS;
662 accel->data[DATA_AXIS_Y] = (int16_t)((((int16_t)((int8_t)reg[3]))<< LSM303AGR_ACC_SHIFT_EIGHT_BITS)|(reg[2]));
663 accel->data[DATA_AXIS_Y] = accel->data[DATA_AXIS_Y] >> LSM303AGR_ACC_SHIFT_FOUR_BITS;
664 accel->data[DATA_AXIS_Z] = (int16_t)((((int16_t)((int8_t)reg[5]))<< LSM303AGR_ACC_SHIFT_EIGHT_BITS)|(reg[4]));
665 accel->data[DATA_AXIS_Z] = accel->data[DATA_AXIS_Z] >> LSM303AGR_ACC_SHIFT_FOUR_BITS;
666
667 if(cur_acc_factor != 0){
668 accel->data[DATA_AXIS_X] = accel->data[DATA_AXIS_X] * cur_acc_factor;
669 accel->data[DATA_AXIS_Y] = accel->data[DATA_AXIS_Y] * cur_acc_factor;
670 accel->data[DATA_AXIS_Z] = accel->data[DATA_AXIS_Z] * cur_acc_factor;
671 }
672
673 accel->timestamp = aos_now_ms();
674
675 return (int)size;
676 }
677
678
drv_acc_st_lsm303agr_ioctl(int cmd,unsigned long arg)679 static int drv_acc_st_lsm303agr_ioctl(int cmd, unsigned long arg)
680 {
681 int ret = 0;
682 dev_sensor_info_t *info = (dev_sensor_info_t *)arg;
683
684 switch(cmd){
685 case SENSOR_IOCTL_ODR_SET:{
686 ret = drv_acc_st_lsm303agr_set_odr(&lsm303agr_acc_ctx, arg);
687 if(unlikely(ret)){
688 return -1;
689 }
690 }break;
691 case SENSOR_IOCTL_RANGE_SET:{
692 ret = drv_acc_st_lsm303agr_set_range(&lsm303agr_acc_ctx, arg);
693 if(unlikely(ret)){
694 return -1;
695 }
696 }break;
697 case SENSOR_IOCTL_SET_POWER:{
698 ret = drv_acc_st_lsm303agr_set_power_mode(&lsm303agr_acc_ctx, arg);
699 if(unlikely(ret)){
700 return -1;
701 }
702 }break;
703 case SENSOR_IOCTL_GET_INFO:{
704 /* fill the dev info here */
705 info->model = "LSM303AGR_ACC";
706 info->range_max = 16;
707 info->range_min = 2;
708 info->unit = mg;
709
710 }break;
711 case SENSOR_IOCTL_SELF_TEST:{
712 ret = drv_acc_st_lsm303agr_self_test(&lsm303agr_acc_ctx, (int32_t*)info->data);
713 //printf("%d %d %d\n",info->data[0],info->data[1],info->data[2]);
714 LOG("%s %s: %d, %d, %d\n", SENSOR_STR, __func__, info->data[0],info->data[1],info->data[2]);
715 return ret;
716 }
717 default:break;
718 }
719
720 return 0;
721 }
722
drv_acc_st_lsm303agr_init(void)723 int drv_acc_st_lsm303agr_init(void){
724 int ret = 0;
725 sensor_obj_t sensor;
726 memset(&sensor, 0, sizeof(sensor));
727
728 /* fill the sensor obj parameters here */
729 sensor.io_port = I2C_PORT;
730 sensor.tag = TAG_DEV_ACC;
731 sensor.path = dev_acc_path;
732 sensor.open = drv_acc_st_lsm303agr_open;
733 sensor.close = drv_acc_st_lsm303agr_close;
734 sensor.read = drv_acc_st_lsm303agr_read;
735 sensor.write = NULL;
736 sensor.ioctl = drv_acc_st_lsm303agr_ioctl;
737 sensor.irq_handle = drv_acc_st_lsm303agr_irq_handle;
738
739 ret = sensor_create_obj(&sensor);
740 if(unlikely(ret)){
741 return -1;
742 }
743
744 ret = drv_acc_st_lsm303agr_validate_id(&lsm303agr_acc_ctx, LSM303AGR_ACC_CHIP_ID_VALUE);
745 if(unlikely(ret)){
746 return -1;
747 }
748
749 ret = drv_acc_st_lsm303agr_set_range(&lsm303agr_acc_ctx, ACC_RANGE_8G);
750 if(unlikely(ret)){
751 return -1;
752 }
753
754 //set odr is 100hz, and will update
755 ret = drv_acc_st_lsm303agr_set_odr(&lsm303agr_acc_ctx, LSM303AGR_ACC_DEFAULT_ODR_100HZ);
756 if(unlikely(ret)){
757 return -1;
758 }
759
760 //set bdu
761 ret = drv_acc_st_lsm303agr_set_bdu(&lsm303agr_acc_ctx);
762 if(unlikely(ret)){
763 return -1;
764 }
765
766 /* update the phy sensor info to sensor hal */
767 LOG("%s %s successfully \n", SENSOR_STR, __func__);
768 return 0;
769 }
770
drv_mag_st_lsm303agr_soft_reset(i2c_dev_t * drv)771 static int drv_mag_st_lsm303agr_soft_reset(i2c_dev_t* drv)
772 {
773 int ret = 0;
774 uint8_t value = LSM303AGR_MAG_SOFT_RESET_ENABLE;
775 //ret = sensor_i2c_write(drv, LSM303AGR_MAG_CFG_REG_A, LSM303AGR_MAG_SOFT_RESET_ENABLE, I2C_DATA_LEN, I2C_OP_RETRIES);
776 ret = sensor_i2c_write(drv, LSM303AGR_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
777 if(unlikely(ret)){
778 return -1;
779 }
780 return 0;
781 }
782
drv_mag_st_lsm303agr_selftest(i2c_dev_t * drv)783 UNUSED static int drv_mag_st_lsm303agr_selftest(i2c_dev_t* drv)
784 {
785 int ret = 0;
786 uint8_t value = LSM303AGR_MAG_SELFTEST_ENABLE;
787 //ret = sensor_i2c_write(drv, LSM303AGR_MAG_CFG_REG_C, LSM303AGR_MAG_SELFTEST_ENABLE, I2C_DATA_LEN, I2C_OP_RETRIES);
788 ret = sensor_i2c_write(drv, LSM303AGR_MAG_CFG_REG_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
789 if(unlikely(ret)){
790 return -1;
791 }
792 return 0;
793 }
794
drv_mag_st_lsm303agr_reboot(i2c_dev_t * drv)795 UNUSED static int drv_mag_st_lsm303agr_reboot(i2c_dev_t* drv)
796 {
797 int ret = 0;
798 uint8_t value = LSM303AGR_MAG_REBOOT_ENABLE;
799 //ret = sensor_i2c_write(drv, LSM303AGR_MAG_CFG_REG_A, LSM303AGR_MAG_REBOOT_ENABLE, I2C_DATA_LEN, I2C_OP_RETRIES);
800 ret = sensor_i2c_write(drv, LSM303AGR_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
801 if(unlikely(ret)){
802 return -1;
803 }
804 return 0;
805 }
806
drv_mag_st_lsm303agr_validate_id(i2c_dev_t * drv,uint8_t id_value)807 static int drv_mag_st_lsm303agr_validate_id(i2c_dev_t* drv, uint8_t id_value)
808 {
809 uint8_t value = 0x00;
810 int ret = 0;
811
812 if(drv == NULL){
813 return -1;
814 }
815
816 ret = sensor_i2c_read(drv, LSM303AGR_MAG_WHO_AM_I, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
817 LOG("%s %s right id (0x%02x), read id(0x%02x)\n", SENSOR_STR, __func__, id_value, value);
818 if(unlikely(ret)){
819 return ret;
820 }
821
822 if (id_value != value){
823 return -1;
824 }
825
826 return 0;
827 }
828
drv_mag_st_lsm303agr_set_power_mode(i2c_dev_t * drv,dev_power_mode_e mode)829 static int drv_mag_st_lsm303agr_set_power_mode(i2c_dev_t* drv, dev_power_mode_e mode)
830 {
831 uint8_t value = 0x00;
832 int ret = 0;
833
834 ret = sensor_i2c_read(drv, LSM303AGR_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
835 if(unlikely(ret)){
836 return ret;
837 }
838
839 if (mode == DEV_POWER_ON)
840 {
841 value &= ~LSM303AGR_MAG_POWERMODE_BIT;
842 value |= LSM303AGR_MAG_CONTINUOUS_MODE;
843 }
844 else{
845 value &= ~LSM303AGR_MAG_POWERMODE_BIT;
846 value |= LSM303AGR_MAG_POWERDOWN2_MODE;
847 }
848
849 ret = sensor_i2c_write(drv, LSM303AGR_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
850 if(unlikely(ret)){
851 return ret;
852 }
853
854 return 0;
855 }
856
drv_mag_st_lsm303agr_set_odr(i2c_dev_t * drv,uint8_t odr)857 static int drv_mag_st_lsm303agr_set_odr(i2c_dev_t* drv, uint8_t odr)
858 {
859 uint8_t value = 0x00;
860 int ret = 0;
861
862 ret = sensor_i2c_read(drv, LSM303AGR_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
863 if(unlikely(ret)){
864 return ret;
865 }
866 value &= ~LSM303AGR_MAG_ODR_BIT;
867 value |= (uint8_t)odr;
868
869 ret = sensor_i2c_write(drv, LSM303AGR_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
870 if(unlikely(ret)){
871 return ret;
872 }
873 return 0;
874 }
875
drv_mag_st_lsm303agr_enable_off_canc(i2c_dev_t * drv)876 static int drv_mag_st_lsm303agr_enable_off_canc(i2c_dev_t* drv)
877 {
878 uint8_t value = 0x00;
879 int ret = 0;
880
881 ret = sensor_i2c_read(drv, LSM303AGR_MAG_CFG_REG_B, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
882 if(unlikely(ret)){
883 return ret;
884 }
885 value |= LSM303AGR_MAG_OFF_CANC;
886 ret = sensor_i2c_write(drv, LSM303AGR_MAG_CFG_REG_B, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
887 if(unlikely(ret)){
888 return ret;
889 }
890 return 0;
891 }
892
drv_mag_st_lsm303agr_enable_temp(i2c_dev_t * drv)893 static int drv_mag_st_lsm303agr_enable_temp(i2c_dev_t* drv)
894 {
895 uint8_t value = 0x00;
896 int ret = 0;
897
898 ret = sensor_i2c_read(drv, LSM303AGR_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
899 if(unlikely(ret)){
900 return ret;
901 }
902 value |= LSM303AGR_MAG_COMP_TEMP_EN;
903 ret = sensor_i2c_write(drv, LSM303AGR_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
904 if(unlikely(ret)){
905 return ret;
906 }
907 return 0;
908 }
909
drv_mag_st_lsm303agr_lowpower_mode(i2c_dev_t * drv,uint8_t lowpower_mode)910 UNUSED static int drv_mag_st_lsm303agr_lowpower_mode(i2c_dev_t* drv, uint8_t lowpower_mode)
911 {
912 int ret = 0;
913 uint8_t value = 0x00;
914
915 ret = sensor_i2c_read(drv, LSM303AGR_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
916 if(unlikely(ret)){
917 return ret;
918 }
919
920 if (lowpower_mode == LSM303AGR_MAG_CONFIG_LOWPOWER_MODE)
921 {
922 value &= ~LSM303AGR_MAG_CONFIG_LOWPOWER_BIT;
923 value |= LSM303AGR_MAG_CONFIG_LOWPOWER_MODE;
924 }
925 else{
926 value &= ~LSM303AGR_MAG_CONFIG_LOWPOWER_BIT;
927 value |= LSM303AGR_MAG_CONFIG_NORMAL_MODE;
928 }
929
930 ret = sensor_i2c_write(drv, LSM303AGR_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
931 if(unlikely(ret)){
932 return ret;
933 }
934
935 return 0;
936 }
937
drv_mag_st_lsm303agr_set_range(i2c_dev_t * drv,uint32_t range)938 static int drv_mag_st_lsm303agr_set_range(i2c_dev_t* drv, uint32_t range)
939 {
940 // default FS +/-50Gauss
941 return 0;
942 }
943
drv_mag_st_lsm303agr_set_ble(i2c_dev_t * drv,uint8_t ble)944 static int drv_mag_st_lsm303agr_set_ble(i2c_dev_t* drv, uint8_t ble)
945 {
946 uint8_t value = 0x00;
947 int ret = 0;
948
949 ret = sensor_i2c_read(drv, LSM303AGR_MAG_CFG_REG_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
950 if(unlikely(ret)){
951 return ret;
952 }
953
954 if (ble == LSM303AGR_MAG_BLE_LSB)
955 {
956 value &= ~LSM303AGR_MAG_BLE_BIT;
957 value |= LSM303AGR_MAG_BLE_LSB;
958 }
959 else{
960 value &= ~LSM303AGR_MAG_BLE_BIT;
961 value |= LSM303AGR_MAG_BLE_MSB;
962 }
963
964 ret = sensor_i2c_write(drv, LSM303AGR_MAG_CFG_REG_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
965 if(unlikely(ret)){
966 return ret;
967 }
968 return 0;
969 }
970
drv_mag_st_lsm303agr_set_bdu(i2c_dev_t * drv,uint8_t bdu)971 static int drv_mag_st_lsm303agr_set_bdu(i2c_dev_t* drv, uint8_t bdu)
972 {
973 uint8_t value = 0x00;
974 int ret = 0;
975
976 ret = sensor_i2c_read(drv, LSM303AGR_MAG_CFG_REG_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
977 if(unlikely(ret)){
978 return ret;
979 }
980
981 if (bdu == LSM303AGR_MAG_BDU_CONTINUOUS)
982 {
983 value &= ~LSM303AGR_MAG_BDU_BIT;
984 value |= LSM303AGR_MAG_BDU_CONTINUOUS;
985 }
986 else{
987 value &= ~LSM303AGR_MAG_BDU_BIT;
988 value |= LSM303AGR_MAG_BDU_MSBLSB;
989 }
990
991 ret = sensor_i2c_write(drv, LSM303AGR_MAG_CFG_REG_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
992 if(unlikely(ret)){
993 return ret;
994 }
995 return 0;
996 }
997
drv_mag_st_lsm303agr_set_default_config(i2c_dev_t * drv)998 static int drv_mag_st_lsm303agr_set_default_config(i2c_dev_t* drv)
999 {
1000 int ret = 0;
1001 ret = drv_mag_st_lsm303agr_set_power_mode(drv, DEV_POWER_OFF);
1002 if(unlikely(ret)){
1003 return ret;
1004 }
1005 ret = drv_mag_st_lsm303agr_set_odr(drv, LSM303AGR_MAG_ODR_10_HZ);
1006 if(unlikely(ret)){
1007 return ret;
1008 }
1009 ret = drv_mag_st_lsm303agr_set_range(drv, LSM303AGR_MAG_FS_50_GA);
1010 if(unlikely(ret)){
1011 return -1;
1012 }
1013 ret = drv_mag_st_lsm303agr_set_ble(drv, LSM303AGR_MAG_BLE_LSB);
1014 if(unlikely(ret)){
1015 return -1;
1016 }
1017 ret = drv_mag_st_lsm303agr_set_bdu(drv, LSM303AGR_MAG_BDU_MSBLSB);
1018 if(unlikely(ret)){
1019 return -1;
1020 }
1021
1022 return 0;
1023 }
1024
drv_mag_st_lsm303agr_st_discard(i2c_dev_t * drv)1025 static int drv_mag_st_lsm303agr_st_discard(i2c_dev_t* drv)
1026 {
1027 uint8_t i;
1028 uint8_t value = 0x00;
1029 int ret = 0;
1030 uint8_t buffer[6];
1031
1032 for (i = 0; i < LSM303AGR_MAG_SELF_TEST_DRY_WAIT_CNT; i ++) {
1033 ret = sensor_i2c_read(drv, LSM303AGR_MAG_STATUS_REG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
1034 if(unlikely(ret)){
1035 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1036 return ret;
1037 }
1038 if (value & 0x08)
1039 break;
1040
1041 aos_msleep(10);
1042 }
1043
1044 if (i >= LSM303AGR_MAG_SELF_TEST_DRY_WAIT_CNT) {
1045 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1046 return -1;
1047 }
1048
1049 ret = sensor_i2c_read(drv, (LSM303AGR_MAG_OUTX_L_REG | 0x80), buffer, 6, I2C_OP_RETRIES);
1050 if(unlikely(ret)){
1051 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1052 return ret;
1053 }
1054
1055 return ret;
1056 }
1057
drv_mag_st_lsm303agr_st_data(i2c_dev_t * drv,int32_t * data)1058 static int drv_mag_st_lsm303agr_st_data(i2c_dev_t* drv,int32_t* data)
1059 {
1060 uint8_t i, j;
1061 int16_t x_raw, y_raw, z_raw;
1062 int32_t x_mg, y_mg, z_mg;
1063 int32_t x_sum, y_sum, z_sum;
1064 uint8_t value = 0x00;
1065 int ret = 0;
1066 uint8_t buffer[6];
1067
1068 x_sum = 0; y_sum = 0; z_sum = 0;
1069
1070 for (i = 0; i < LSM303AGR_MAG_SELF_TEST_AVG_SAMPLE_CNT; i ++) {
1071 for (j = 0; j < LSM303AGR_MAG_SELF_TEST_DRY_WAIT_CNT; j ++) {
1072 ret = sensor_i2c_read(drv, LSM303AGR_MAG_STATUS_REG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
1073 if(unlikely(ret)){
1074 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1075 return ret;
1076 }
1077 if (value & 0x08)
1078 break;
1079
1080 aos_msleep(10);
1081 }
1082
1083 if (j >= LSM303AGR_MAG_SELF_TEST_DRY_WAIT_CNT) {
1084 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1085 return -1;
1086 }
1087
1088 ret = sensor_i2c_read(drv, (LSM303AGR_MAG_OUTX_L_REG | 0x80), buffer, 6, I2C_OP_RETRIES);
1089 if(unlikely(ret)){
1090 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1091 return ret;
1092 }
1093
1094 x_raw = (buffer[1] << 8) + buffer[0];
1095 y_raw = (buffer[3] << 8) + buffer[2];
1096 z_raw = (buffer[5] << 8) + buffer[4];
1097
1098 x_mg = x_raw * LSM303AGR_MAG_SENSITIVITY_FOR_FS_50GA / 1000;
1099 y_mg = y_raw * LSM303AGR_MAG_SENSITIVITY_FOR_FS_50GA / 1000;
1100 z_mg = z_raw * LSM303AGR_MAG_SENSITIVITY_FOR_FS_50GA / 1000;
1101
1102 x_sum += x_mg;
1103 y_sum += y_mg;
1104 z_sum += z_mg;
1105 }
1106
1107 data[0] = x_sum / LSM303AGR_MAG_SELF_TEST_AVG_SAMPLE_CNT;
1108 data[1] = y_sum / LSM303AGR_MAG_SELF_TEST_AVG_SAMPLE_CNT;
1109 data[2] = z_sum / LSM303AGR_MAG_SELF_TEST_AVG_SAMPLE_CNT;
1110 return ret;
1111 }
1112
drv_mag_st_lsm303agr_self_test(i2c_dev_t * drv,int32_t * data)1113 static int drv_mag_st_lsm303agr_self_test(i2c_dev_t* drv,int32_t* data)
1114 {
1115 uint8_t i;
1116 uint8_t value = 0x00;
1117 int ret = 0;
1118 uint8_t cfg_reg[3];
1119 int32_t out_nost[3];
1120 int32_t out_st[3];
1121 int32_t out_diff[3];
1122
1123 // Save cfg registers which will be modified during self-test
1124 ret = sensor_i2c_read(drv, (LSM303AGR_MAG_CFG_REG_A | 0x80), cfg_reg, 3, I2C_OP_RETRIES);
1125 if(unlikely(ret)){
1126 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1127 return ret;
1128 }
1129
1130 // Initialize Sensor, turn on sensor, COMP_TEMP_EN, BDU, Continueous-Measurement,
1131 // Enable offset concellation, ODR = 100Hz
1132 value = 0x8c;
1133 ret = sensor_i2c_write(drv, LSM303AGR_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
1134 if(unlikely(ret)){
1135 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1136 goto restore;
1137 }
1138 value = 0x02;
1139 ret = sensor_i2c_write(drv, LSM303AGR_MAG_CFG_REG_B, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
1140 if(unlikely(ret)){
1141 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1142 goto restore;
1143 }
1144
1145 value = 0x10;
1146 ret = sensor_i2c_write(drv, LSM303AGR_MAG_CFG_REG_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
1147 if(unlikely(ret)){
1148 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1149 goto restore;
1150 }
1151
1152 aos_msleep(20);
1153
1154 // Discard the first sample
1155 ret = drv_mag_st_lsm303agr_st_discard(drv);
1156 if(unlikely(ret)){
1157 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1158 goto restore;
1159 }
1160
1161 // Read some samples, and averate them
1162 ret = drv_mag_st_lsm303agr_st_data(drv, out_nost);
1163 if(unlikely(ret)){
1164 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1165 goto restore;
1166 }
1167
1168 // Enable seft-test
1169 value = 0x12;
1170 ret = sensor_i2c_write(drv, LSM303AGR_MAG_CFG_REG_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
1171 if(unlikely(ret)){
1172 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1173 goto restore;
1174 }
1175 aos_msleep(60);
1176
1177 // Discard the first sample
1178 ret = drv_mag_st_lsm303agr_st_discard(drv);
1179 if(unlikely(ret)){
1180 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1181 goto restore;
1182 }
1183
1184 // Read some samples, and average them
1185 ret = drv_mag_st_lsm303agr_st_data(drv, out_st);
1186 if(unlikely(ret)){
1187 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1188 goto restore;
1189 }
1190
1191 // Check if the differences are between min and max
1192 for (i = 0; i < 3; i ++) {
1193 out_diff[i] = abs(out_st[i] - out_nost[i]);
1194 data[i] = out_diff[i];
1195 }
1196
1197 if ((LSM303AGR_MAG_SELF_TEST_MIN_X > out_diff[0]) || (out_diff[0] > LSM303AGR_MAG_SELF_TEST_MAX_X)) {
1198 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1199 ret = -1;
1200 goto restore;
1201 }
1202 if ((LSM303AGR_MAG_SELF_TEST_MIN_Y > out_diff[1]) || (out_diff[1] > LSM303AGR_MAG_SELF_TEST_MAX_Y)) {
1203 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1204 ret = -1;
1205 goto restore;
1206 }
1207 if ((LSM303AGR_MAG_SELF_TEST_MIN_Z > out_diff[2]) || (out_diff[2] > LSM303AGR_MAG_SELF_TEST_MAX_Z)) {
1208 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1209 ret = -1;
1210 goto restore;
1211 }
1212
1213 restore:
1214 ret += sensor_i2c_write(drv, LSM303AGR_MAG_CFG_REG_A | 0x80, cfg_reg, 3, I2C_OP_RETRIES);
1215 return ret;
1216 }
1217
drv_mag_st_lsm303agr_irq_handle(void)1218 static void drv_mag_st_lsm303agr_irq_handle(void)
1219 {
1220 /* no handle so far */
1221 }
1222
drv_mag_st_lsm303agr_open(void)1223 static int drv_mag_st_lsm303agr_open(void)
1224 {
1225 int ret = 0;
1226 ret = drv_mag_st_lsm303agr_set_power_mode(&lsm303agr_mag_ctx, DEV_POWER_ON);
1227 if(unlikely(ret)){
1228 return -1;
1229 }
1230 return 0;
1231
1232 }
1233
drv_mag_st_lsm303agr_close(void)1234 static int drv_mag_st_lsm303agr_close(void)
1235 {
1236 int ret = 0;
1237 ret = drv_mag_st_lsm303agr_set_power_mode(&lsm303agr_mag_ctx, DEV_POWER_OFF);
1238 if(unlikely(ret)){
1239 return -1;
1240 }
1241 return 0;
1242 }
1243
drv_mag_st_lsm303agr_read(void * buf,size_t len)1244 static int drv_mag_st_lsm303agr_read(void* buf, size_t len)
1245 {
1246 int ret = 0;
1247 size_t size;
1248 int16_t pnRawData[3];
1249 uint8_t buffer[6];
1250 uint8_t i = 0;
1251 uint16_t sensitivity = 0;
1252
1253 mag_data_t* pdata = (mag_data_t*)buf;
1254 if(buf == NULL){
1255 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1256 return -1;
1257 }
1258
1259 size = sizeof(mag_data_t);
1260 if(len < size){
1261 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1262 return -1;
1263 }
1264
1265 ret = sensor_i2c_read(&lsm303agr_mag_ctx, (LSM303AGR_MAG_OUTX_L_REG | 0x80), buffer, 6, I2C_OP_RETRIES);
1266 if(unlikely(ret)){
1267 return -1;
1268 }
1269 for(i=0; i<3; i++)
1270 {
1271 pnRawData[i]=((((uint16_t)buffer[2*i+1]) << 8) | (uint16_t)buffer[2*i]);
1272 }
1273 //LOG("%s %s: %d, %d, %d \n", SENSOR_STR, __func__, pnRawData[0], pnRawData[1], pnRawData[2]);
1274
1275 sensitivity = LSM303AGR_MAG_SENSITIVITY_FOR_FS_50GA;
1276
1277 for(i=0; i<3; i++)
1278 {
1279 //pdata->data[i] = ( int16_t )(pnRawData[i] * sensitivity);
1280 pdata->data[i] = pnRawData[i] * sensitivity / 1000;
1281 }
1282 pdata->timestamp = aos_now_ms();
1283
1284 return (int)size;
1285 }
1286
1287
drv_mag_st_lsm303agr_ioctl(int cmd,unsigned long arg)1288 static int drv_mag_st_lsm303agr_ioctl(int cmd, unsigned long arg)
1289 {
1290 int ret = 0;
1291 dev_sensor_info_t *info = (dev_sensor_info_t *)arg;
1292
1293 switch(cmd){
1294 case SENSOR_IOCTL_ODR_SET:{
1295 ret = drv_mag_st_lsm303agr_set_odr(&lsm303agr_mag_ctx, arg);
1296 if(unlikely(ret)){
1297 return -1;
1298 }
1299 }break;
1300 case SENSOR_IOCTL_RANGE_SET:{
1301 ret = drv_mag_st_lsm303agr_set_range(&lsm303agr_mag_ctx, arg);
1302 if(unlikely(ret)){
1303 return -1;
1304 }
1305 }break;
1306 case SENSOR_IOCTL_SET_POWER:{
1307 ret = drv_mag_st_lsm303agr_set_power_mode(&lsm303agr_mag_ctx, arg);
1308 if(unlikely(ret)){
1309 return -1;
1310 }
1311 }break;
1312 case SENSOR_IOCTL_GET_INFO:{
1313 /* fill the dev info here */
1314 //dev_sensor_info_t *info = (dev_sensor_info_t *)arg;
1315 info->model = "LSM303AGR";
1316 info->range_max = 50;
1317 info->range_min = 50;
1318 info->unit = mGauss;
1319 }break;
1320
1321 case SENSOR_IOCTL_SELF_TEST:{
1322 ret = drv_mag_st_lsm303agr_self_test(&lsm303agr_mag_ctx, (int32_t*)info->data);
1323 //printf("%d %d %d\n",info->data[0],info->data[1],info->data[2]);
1324 LOG("%s %s: %d, %d, %d\n", SENSOR_STR, __func__, info->data[0],info->data[1],info->data[2]);
1325 return ret;
1326 }
1327
1328 default:break;
1329 }
1330
1331 return 0;
1332 }
1333
drv_mag_st_lsm303agr_init(void)1334 int drv_mag_st_lsm303agr_init(void){
1335 int ret = 0;
1336 sensor_obj_t sensor;
1337 memset(&sensor, 0, sizeof(sensor));
1338 /* fill the sensor obj parameters here */
1339 sensor.io_port = I2C_PORT;
1340 sensor.tag = TAG_DEV_MAG;
1341 sensor.path = dev_mag_path;
1342 sensor.open = drv_mag_st_lsm303agr_open;
1343 sensor.close = drv_mag_st_lsm303agr_close;
1344 sensor.read = drv_mag_st_lsm303agr_read;
1345 sensor.write = NULL;
1346 sensor.ioctl = drv_mag_st_lsm303agr_ioctl;
1347 sensor.irq_handle = drv_mag_st_lsm303agr_irq_handle;
1348
1349 ret = sensor_create_obj(&sensor);
1350 if(unlikely(ret)){
1351 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1352 return -1;
1353 }
1354
1355 ret = drv_mag_st_lsm303agr_validate_id(&lsm303agr_mag_ctx, I_AM_LSM303AGR);
1356 if(unlikely(ret)){
1357 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1358 return -1;
1359 }
1360
1361 ret = drv_mag_st_lsm303agr_soft_reset(&lsm303agr_mag_ctx);
1362 if(unlikely(ret)){
1363 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1364 return -1;
1365 }
1366
1367 ret = drv_mag_st_lsm303agr_set_default_config(&lsm303agr_mag_ctx);
1368 if(unlikely(ret)){
1369 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1370 return -1;
1371 }
1372
1373 ret = drv_mag_st_lsm303agr_enable_temp(&lsm303agr_mag_ctx);
1374 if(unlikely(ret)){
1375 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1376 return -1;
1377 }
1378
1379 ret = drv_mag_st_lsm303agr_enable_off_canc(&lsm303agr_mag_ctx);
1380 if(unlikely(ret)){
1381 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
1382 return -1;
1383 }
1384 /* update the phy sensor info to sensor hal */
1385 LOG("%s %s successfully \n", SENSOR_STR, __func__);
1386 return 0;
1387 }
1388
1389
1390 SENSOR_DRV_ADD(drv_acc_st_lsm303agr_init);
1391 SENSOR_DRV_ADD(drv_mag_st_lsm303agr_init);
1392
1393
1394