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 LIS2MDL_I2C_ADDR1 0x1E
16 #define LIS2MDL_I2C_ADDR_TRANS(n) ((n)<<1)
17 #define LIS2MDL_I2C_ADDR LIS2MDL_I2C_ADDR_TRANS(LIS2MDL_I2C_ADDR1)
18 #define LIS2MDL_BIT(x) (x)
19
20 #define LIS2MDL_MAG_OFFSET_X_REG_L 0x45
21 #define LIS2MDL_MAG_OFFSET_X_REG_H 0x46
22 #define LIS2MDL_MAG_OFFSET_Y_REG_L 0x47
23 #define LIS2MDL_MAG_OFFSET_Y_REG_H 0x48
24 #define LIS2MDL_MAG_OFFSET_Z_REG_L 0x49
25 #define LIS2MDL_MAG_OFFSET_Z_REG_H 0x4A
26 #define LIS2MDL_MAG_WHO_AM_I 0x4F
27 #define LIS2MDL_MAG_CFG_REG_A 0x60
28 #define LIS2MDL_MAG_CFG_REG_B 0x61
29 #define LIS2MDL_MAG_CFG_REG_C 0x62
30 #define LIS2MDL_MAG_INT_CRTL_REG 0x63
31 #define LIS2MDL_MAG_INT_SOURCE_REG 0x64
32 #define LIS2MDL_MAG_INT_THS_L_REG 0x65
33 #define LIS2MDL_MAG_INT_THS_H_REG 0x66
34 #define LIS2MDL_MAG_STATUS_REG 0x67
35 #define LIS2MDL_MAG_OUTX_L_REG 0x68
36 #define LIS2MDL_MAG_OUTX_H_REG 0x69
37 #define LIS2MDL_MAG_OUTY_L_REG 0x6A
38 #define LIS2MDL_MAG_OUTY_H_REG 0x6B
39 #define LIS2MDL_MAG_OUTZ_L_REG 0x6C
40 #define LIS2MDL_MAG_OUTZ_H_REG 0x6D
41 #define LIS2MDL_MAG_TEMP_OUT_L_REG 0x6E
42 #define LIS2MDL_MAG_TEMP_OUT_H_REG 0x6F
43
44 #define I_AM_LIS2MDL 0x40
45
46 #define LIS2MDL_MAG_ODR_BIT LIS2MDL_BIT(0x0C)
47 #define LIS2MDL_MAG_ODR_10_HZ 0x00
48 #define LIS2MDL_MAG_ODR_20_HZ 0x04
49 #define LIS2MDL_MAG_ODR_50_HZ 0x08
50 #define LIS2MDL_MAG_ODR_100_HZ 0x0C
51
52 #define LIS2MDL_MAG_SELFTEST_DISABLE 0x00
53 #define LIS2MDL_MAG_SELFTEST_ENABLE 0x02
54
55 #define LIS2MDL_MAG_FS_50_GA 0x00
56
57 #define LIS2MDL_MAG_REBOOT_DEFAULT 0x00
58 #define LIS2MDL_MAG_REBOOT_ENABLE 0x40
59
60 #define LIS2MDL_MAG_SOFT_RESET_DEFAULT 0x00
61 #define LIS2MDL_MAG_SOFT_RESET_ENABLE 0x02
62
63 #define LIS2MDL_MAG_CONFIG_LOWPOWER_BIT LIS2MDL_BIT(0x10)
64 #define LIS2MDL_MAG_CONFIG_NORMAL_MODE 0x00
65 #define LIS2MDL_MAG_CONFIG_LOWPOWER_MODE 0x10
66
67 #define LIS2MDL_MAG_POWERMODE_BIT LIS2MDL_BIT(0x03)
68 #define LIS2MDL_MAG_CONTINUOUS_MODE 0x00
69 #define LIS2MDL_MAG_SINGLE_MODE 0x01
70 #define LIS2MDL_MAG_POWERDOWN1_MODE 0x02
71 #define LIS2MDL_MAG_POWERDOWN2_MODE 0x03
72
73 #define LIS2MDL_MAG_COMP_TEMP_EN 0x80
74
75 #define LIS2MDL_MAG_OFF_CANC 0x02
76
77 #define LIS2MDL_MAG_BLE_BIT LIS2MDL_BIT(0x08)
78 #define LIS2MDL_MAG_BLE_LSB 0x00
79 #define LIS2MDL_MAG_BLE_MSB 0x08
80
81 #define LIS2MDL_MAG_BDU_BIT LIS2MDL_BIT(0x10)
82 #define LIS2MDL_MAG_BDU_CONTINUOUS 0x00
83 #define LIS2MDL_MAG_BDU_MSBLSB 0x10
84
85 //#define LIS2MDL_MAG_SENSITIVITY_FOR_FS_50GA 15/10
86 #define LIS2MDL_MAG_SENSITIVITY_FOR_FS_50GA 1500
87
88 #define LIS2MDL_MAG_SELF_TEST_MIN_X (15) // 15mGuass
89 #define LIS2MDL_MAG_SELF_TEST_MIN_Y (15) // 15mGuass
90 #define LIS2MDL_MAG_SELF_TEST_MIN_Z (15) // 15mGuass
91 #define LIS2MDL_MAG_SELF_TEST_MAX_X (500) // 500mGuass
92 #define LIS2MDL_MAG_SELF_TEST_MAX_Y (500) // 500mGuass
93 #define LIS2MDL_MAG_SELF_TEST_MAX_Z (500) // 500mGuass
94
95 #define LIS2MDL_MAG_SELF_TEST_DRY_WAIT_CNT 5
96 #define LIS2MDL_MAG_SELF_TEST_AVG_SAMPLE_CNT 50
97
98 i2c_dev_t LIS2MDL_ctx = {
99 .port = 3,
100 .config.address_width = 8,
101 .config.freq = 400000,
102 .config.dev_addr = LIS2MDL_I2C_ADDR,
103 };
104
drv_mag_st_lis2mdl_soft_reset(i2c_dev_t * drv)105 UNUSED static int drv_mag_st_lis2mdl_soft_reset(i2c_dev_t* drv)
106 {
107 int ret = 0;
108 uint8_t value = LIS2MDL_MAG_SOFT_RESET_ENABLE;
109 //ret = sensor_i2c_write(drv, LIS2MDL_MAG_CFG_REG_A, LIS2MDL_MAG_SOFT_RESET_ENABLE, I2C_DATA_LEN, I2C_OP_RETRIES);
110 ret = sensor_i2c_write(drv, LIS2MDL_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
111 if(unlikely(ret)){
112 return -1;
113 }
114 return 0;
115 }
116
drv_mag_st_lis2mdl_selftest(i2c_dev_t * drv)117 UNUSED static int drv_mag_st_lis2mdl_selftest(i2c_dev_t* drv)
118 {
119 int ret = 0;
120 uint8_t value = LIS2MDL_MAG_SELFTEST_ENABLE;
121 //ret = sensor_i2c_write(drv, LIS2MDL_MAG_CFG_REG_C, LIS2MDL_MAG_SELFTEST_ENABLE, I2C_DATA_LEN, I2C_OP_RETRIES);
122 ret = sensor_i2c_write(drv, LIS2MDL_MAG_CFG_REG_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
123 if(unlikely(ret)){
124 return -1;
125 }
126 return 0;
127 }
128
drv_mag_st_lis2mdl_reboot(i2c_dev_t * drv)129 UNUSED static int drv_mag_st_lis2mdl_reboot(i2c_dev_t* drv)
130 {
131 int ret = 0;
132 uint8_t value = LIS2MDL_MAG_REBOOT_ENABLE;
133 //ret = sensor_i2c_write(drv, LIS2MDL_MAG_CFG_REG_A, LIS2MDL_MAG_REBOOT_ENABLE, I2C_DATA_LEN, I2C_OP_RETRIES);
134 ret = sensor_i2c_write(drv, LIS2MDL_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
135 if(unlikely(ret)){
136 return -1;
137 }
138 return 0;
139 }
140
drv_mag_st_lis2mdl_validate_id(i2c_dev_t * drv,uint8_t id_value)141 static int drv_mag_st_lis2mdl_validate_id(i2c_dev_t* drv, uint8_t id_value)
142 {
143 uint8_t value = 0x00;
144 int ret = 0;
145
146 if(drv == NULL){
147 return -1;
148 }
149
150 ret = sensor_i2c_read(drv, LIS2MDL_MAG_WHO_AM_I, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
151 LOG("%s %s right id (0x%02x), read id(0x%02x)\n", SENSOR_STR, __func__, id_value, value);
152 if(unlikely(ret)){
153 return ret;
154 }
155
156 if (id_value != value){
157 return -1;
158 }
159
160 return 0;
161 }
162
drv_mag_st_lis2mdl_set_power_mode(i2c_dev_t * drv,dev_power_mode_e mode)163 static int drv_mag_st_lis2mdl_set_power_mode(i2c_dev_t* drv, dev_power_mode_e mode)
164 {
165 uint8_t value = 0x00;
166 int ret = 0;
167
168 ret = sensor_i2c_read(drv, LIS2MDL_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
169 if(unlikely(ret)){
170 return ret;
171 }
172
173 if (mode == DEV_POWER_ON)
174 {
175 value &= ~LIS2MDL_MAG_POWERMODE_BIT;
176 value |= LIS2MDL_MAG_CONTINUOUS_MODE;
177 }
178 else{
179 value &= ~LIS2MDL_MAG_POWERMODE_BIT;
180 value |= LIS2MDL_MAG_POWERDOWN2_MODE;
181 }
182
183 ret = sensor_i2c_write(drv, LIS2MDL_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
184 if(unlikely(ret)){
185 return ret;
186 }
187
188 return 0;
189 }
190
drv_mag_st_lis2mdl_set_odr(i2c_dev_t * drv,uint8_t odr)191 static int drv_mag_st_lis2mdl_set_odr(i2c_dev_t* drv, uint8_t odr)
192 {
193 uint8_t value = 0x00;
194 int ret = 0;
195
196 ret = sensor_i2c_read(drv, LIS2MDL_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
197 if(unlikely(ret)){
198 return ret;
199 }
200 value &= ~LIS2MDL_MAG_ODR_BIT;
201 value |= (uint8_t)odr;
202
203 ret = sensor_i2c_write(drv, LIS2MDL_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
204 if(unlikely(ret)){
205 return ret;
206 }
207 return 0;
208 }
209
drv_mag_st_lis2mdl_enable_off_canc(i2c_dev_t * drv)210 static int drv_mag_st_lis2mdl_enable_off_canc(i2c_dev_t* drv)
211 {
212 uint8_t value = 0x00;
213 int ret = 0;
214
215 ret = sensor_i2c_read(drv, LIS2MDL_MAG_CFG_REG_B, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
216 if(unlikely(ret)){
217 return ret;
218 }
219 value |= LIS2MDL_MAG_OFF_CANC;
220 ret = sensor_i2c_write(drv, LIS2MDL_MAG_CFG_REG_B, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
221 if(unlikely(ret)){
222 return ret;
223 }
224 return 0;
225 }
226
drv_mag_st_lis2mdl_enable_temp(i2c_dev_t * drv)227 static int drv_mag_st_lis2mdl_enable_temp(i2c_dev_t* drv)
228 {
229 uint8_t value = 0x00;
230 int ret = 0;
231
232 ret = sensor_i2c_read(drv, LIS2MDL_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
233 if(unlikely(ret)){
234 return ret;
235 }
236 value |= LIS2MDL_MAG_COMP_TEMP_EN;
237 ret = sensor_i2c_write(drv, LIS2MDL_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
238 if(unlikely(ret)){
239 return ret;
240 }
241 return 0;
242 }
243
drv_mag_st_lis2mdl_lowpower_mode(i2c_dev_t * drv,uint8_t lowpower_mode)244 UNUSED static int drv_mag_st_lis2mdl_lowpower_mode(i2c_dev_t* drv, uint8_t lowpower_mode)
245 {
246 int ret = 0;
247 uint8_t value = 0x00;
248
249 ret = sensor_i2c_read(drv, LIS2MDL_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
250 if(unlikely(ret)){
251 return ret;
252 }
253
254 if (lowpower_mode == LIS2MDL_MAG_CONFIG_LOWPOWER_MODE)
255 {
256 value &= ~LIS2MDL_MAG_CONFIG_LOWPOWER_BIT;
257 value |= LIS2MDL_MAG_CONFIG_LOWPOWER_MODE;
258 }
259 else{
260 value &= ~LIS2MDL_MAG_CONFIG_LOWPOWER_BIT;
261 value |= LIS2MDL_MAG_CONFIG_NORMAL_MODE;
262 }
263
264 ret = sensor_i2c_write(drv, LIS2MDL_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
265 if(unlikely(ret)){
266 return ret;
267 }
268
269 return 0;
270 }
271
drv_mag_st_lis2mdl_set_range(i2c_dev_t * drv,uint32_t range)272 static int drv_mag_st_lis2mdl_set_range(i2c_dev_t* drv, uint32_t range)
273 {
274 // default FS +/-50Gauss
275 return 0;
276 }
277
drv_mag_st_lis2mdl_set_ble(i2c_dev_t * drv,uint8_t ble)278 static int drv_mag_st_lis2mdl_set_ble(i2c_dev_t* drv, uint8_t ble)
279 {
280 uint8_t value = 0x00;
281 int ret = 0;
282
283 ret = sensor_i2c_read(drv, LIS2MDL_MAG_CFG_REG_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
284 if(unlikely(ret)){
285 return ret;
286 }
287
288 if (ble == LIS2MDL_MAG_BLE_LSB)
289 {
290 value &= ~LIS2MDL_MAG_BLE_BIT;
291 value |= LIS2MDL_MAG_BLE_LSB;
292 }
293 else{
294 value &= ~LIS2MDL_MAG_BLE_BIT;
295 value |= LIS2MDL_MAG_BLE_MSB;
296 }
297
298 ret = sensor_i2c_write(drv, LIS2MDL_MAG_CFG_REG_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
299 if(unlikely(ret)){
300 return ret;
301 }
302 return 0;
303 }
304
drv_mag_st_lis2mdl_set_bdu(i2c_dev_t * drv,uint8_t bdu)305 static int drv_mag_st_lis2mdl_set_bdu(i2c_dev_t* drv, uint8_t bdu)
306 {
307 uint8_t value = 0x00;
308 int ret = 0;
309
310 ret = sensor_i2c_read(drv, LIS2MDL_MAG_CFG_REG_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
311 if(unlikely(ret)){
312 return ret;
313 }
314
315 if (bdu == LIS2MDL_MAG_BDU_CONTINUOUS)
316 {
317 value &= ~LIS2MDL_MAG_BDU_BIT;
318 value |= LIS2MDL_MAG_BDU_CONTINUOUS;
319 }
320 else{
321 value &= ~LIS2MDL_MAG_BDU_BIT;
322 value |= LIS2MDL_MAG_BDU_MSBLSB;
323 }
324
325 ret = sensor_i2c_write(drv, LIS2MDL_MAG_CFG_REG_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
326 if(unlikely(ret)){
327 return ret;
328 }
329 return 0;
330 }
331
drv_mag_st_lis2mdl_set_default_config(i2c_dev_t * drv)332 static int drv_mag_st_lis2mdl_set_default_config(i2c_dev_t* drv)
333 {
334 int ret = 0;
335 ret = drv_mag_st_lis2mdl_set_power_mode(drv, DEV_POWER_OFF);
336 if(unlikely(ret)){
337 return ret;
338 }
339 ret = drv_mag_st_lis2mdl_set_odr(drv, LIS2MDL_MAG_ODR_10_HZ);
340 if(unlikely(ret)){
341 return ret;
342 }
343 ret = drv_mag_st_lis2mdl_set_range(drv, LIS2MDL_MAG_FS_50_GA);
344 if(unlikely(ret)){
345 return -1;
346 }
347 ret = drv_mag_st_lis2mdl_set_ble(drv, LIS2MDL_MAG_BLE_LSB);
348 if(unlikely(ret)){
349 return -1;
350 }
351 ret = drv_mag_st_lis2mdl_set_bdu(drv, LIS2MDL_MAG_BDU_MSBLSB);
352 if(unlikely(ret)){
353 return -1;
354 }
355
356 return 0;
357 }
358
drv_mag_st_lis2mdl_st_discard(i2c_dev_t * drv)359 static int drv_mag_st_lis2mdl_st_discard(i2c_dev_t* drv)
360 {
361 uint8_t i;
362 uint8_t value = 0x00;
363 int ret = 0;
364 uint8_t buffer[6];
365
366 for (i = 0; i < LIS2MDL_MAG_SELF_TEST_DRY_WAIT_CNT; i ++) {
367 ret = sensor_i2c_read(drv, LIS2MDL_MAG_STATUS_REG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
368 if(unlikely(ret)){
369 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
370 return ret;
371 }
372 if (value & 0x08)
373 break;
374
375 aos_msleep(10);
376 }
377
378 if (i >= LIS2MDL_MAG_SELF_TEST_DRY_WAIT_CNT) {
379 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
380 return -1;
381 }
382
383 ret = sensor_i2c_read(drv, (LIS2MDL_MAG_OUTX_L_REG | 0x80), buffer, 6, I2C_OP_RETRIES);
384 if(unlikely(ret)){
385 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
386 return ret;
387 }
388
389 return ret;
390 }
391
drv_mag_st_lis2mdl_st_data(i2c_dev_t * drv,int32_t * data)392 static int drv_mag_st_lis2mdl_st_data(i2c_dev_t* drv,int32_t* data)
393 {
394 uint8_t i, j;
395 int16_t x_raw, y_raw, z_raw;
396 int32_t x_mg, y_mg, z_mg;
397 int32_t x_sum, y_sum, z_sum;
398 uint8_t value = 0x00;
399 int ret = 0;
400 uint8_t buffer[6];
401
402 x_sum = 0; y_sum = 0; z_sum = 0;
403
404 for (i = 0; i < LIS2MDL_MAG_SELF_TEST_AVG_SAMPLE_CNT; i ++) {
405 for (j = 0; j < LIS2MDL_MAG_SELF_TEST_DRY_WAIT_CNT; j ++) {
406 ret = sensor_i2c_read(drv, LIS2MDL_MAG_STATUS_REG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
407 if(unlikely(ret)){
408 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
409 return ret;
410 }
411 if (value & 0x08)
412 break;
413
414 aos_msleep(10);
415 }
416
417 if (j >= LIS2MDL_MAG_SELF_TEST_DRY_WAIT_CNT) {
418 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
419 return -1;
420 }
421
422 ret = sensor_i2c_read(drv, (LIS2MDL_MAG_OUTX_L_REG | 0x80), buffer, 6, I2C_OP_RETRIES);
423 if(unlikely(ret)){
424 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
425 return ret;
426 }
427
428 x_raw = (buffer[1] << 8) + buffer[0];
429 y_raw = (buffer[3] << 8) + buffer[2];
430 z_raw = (buffer[5] << 8) + buffer[4];
431
432 x_mg = x_raw * LIS2MDL_MAG_SENSITIVITY_FOR_FS_50GA / 1000;
433 y_mg = y_raw * LIS2MDL_MAG_SENSITIVITY_FOR_FS_50GA / 1000;
434 z_mg = z_raw * LIS2MDL_MAG_SENSITIVITY_FOR_FS_50GA / 1000;
435
436 x_sum += x_mg;
437 y_sum += y_mg;
438 z_sum += z_mg;
439 }
440
441 data[0] = x_sum / LIS2MDL_MAG_SELF_TEST_AVG_SAMPLE_CNT;
442 data[1] = y_sum / LIS2MDL_MAG_SELF_TEST_AVG_SAMPLE_CNT;
443 data[2] = z_sum / LIS2MDL_MAG_SELF_TEST_AVG_SAMPLE_CNT;
444 return ret;
445 }
446
drv_mag_st_lis2mdl_self_test(i2c_dev_t * drv,int32_t * data)447 static int drv_mag_st_lis2mdl_self_test(i2c_dev_t* drv,int32_t* data)
448 {
449 uint8_t i;
450 uint8_t value = 0x00;
451 int ret = 0;
452 uint8_t cfg_reg[3];
453 int32_t out_nost[3];
454 int32_t out_st[3];
455 int32_t out_diff[3];
456
457 // Save cfg registers which will be modified during self-test
458 ret = sensor_i2c_read(drv, (LIS2MDL_MAG_CFG_REG_A | 0x80), cfg_reg, 3, I2C_OP_RETRIES);
459 if(unlikely(ret)){
460 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
461 return ret;
462 }
463
464 // Initialize Sensor, turn on sensor, COMP_TEMP_EN, BDU, Continueous-Measurement,
465 // Enable offset concellation, ODR = 100Hz
466 value = 0x8c;
467 ret = sensor_i2c_write(drv, LIS2MDL_MAG_CFG_REG_A, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
468 if(unlikely(ret)){
469 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
470 goto restore;
471 }
472 value = 0x02;
473 ret = sensor_i2c_write(drv, LIS2MDL_MAG_CFG_REG_B, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
474 if(unlikely(ret)){
475 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
476 goto restore;
477 }
478
479 value = 0x10;
480 ret = sensor_i2c_write(drv, LIS2MDL_MAG_CFG_REG_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
481 if(unlikely(ret)){
482 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
483 goto restore;
484 }
485
486 aos_msleep(20);
487
488 // Discard the first sample
489 ret = drv_mag_st_lis2mdl_st_discard(drv);
490 if(unlikely(ret)){
491 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
492 goto restore;
493 }
494
495 // Read some samples, and averate them
496 ret = drv_mag_st_lis2mdl_st_data(drv, out_nost);
497 if(unlikely(ret)){
498 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
499 goto restore;
500 }
501
502 // Enable seft-test
503 value = 0x12;
504 ret = sensor_i2c_write(drv, LIS2MDL_MAG_CFG_REG_C, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
505 if(unlikely(ret)){
506 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
507 goto restore;
508 }
509 aos_msleep(60);
510
511 // Discard the first sample
512 ret = drv_mag_st_lis2mdl_st_discard(drv);
513 if(unlikely(ret)){
514 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
515 goto restore;
516 }
517
518 // Read some samples, and average them
519 ret = drv_mag_st_lis2mdl_st_data(drv, out_st);
520 if(unlikely(ret)){
521 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
522 goto restore;
523 }
524
525 // Check if the differences are between min and max
526 for (i = 0; i < 3; i ++) {
527 out_diff[i] = abs(out_st[i] - out_nost[i]);
528 data[i] = out_diff[i];
529 }
530
531 if ((LIS2MDL_MAG_SELF_TEST_MIN_X > out_diff[0]) || (out_diff[0] > LIS2MDL_MAG_SELF_TEST_MAX_X)) {
532 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
533 ret = -1;
534 goto restore;
535 }
536 if ((LIS2MDL_MAG_SELF_TEST_MIN_Y > out_diff[1]) || (out_diff[1] > LIS2MDL_MAG_SELF_TEST_MAX_Y)) {
537 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
538 ret = -1;
539 goto restore;
540 }
541 if ((LIS2MDL_MAG_SELF_TEST_MIN_Z > out_diff[2]) || (out_diff[2] > LIS2MDL_MAG_SELF_TEST_MAX_Z)) {
542 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
543 ret = -1;
544 goto restore;
545 }
546
547 restore:
548 ret += sensor_i2c_write(drv, LIS2MDL_MAG_CFG_REG_A | 0x80, cfg_reg, 3, I2C_OP_RETRIES);
549 return ret;
550 }
551
drv_mag_st_lis2mdl_irq_handle(void)552 static void drv_mag_st_lis2mdl_irq_handle(void)
553 {
554 /* no handle so far */
555 }
556
drv_mag_st_lis2mdl_open(void)557 static int drv_mag_st_lis2mdl_open(void)
558 {
559 int ret = 0;
560 ret = drv_mag_st_lis2mdl_set_power_mode(&LIS2MDL_ctx, DEV_POWER_ON);
561 if(unlikely(ret)){
562 return -1;
563 }
564 return 0;
565
566 }
567
drv_mag_st_lis2mdl_close(void)568 static int drv_mag_st_lis2mdl_close(void)
569 {
570 int ret = 0;
571 ret = drv_mag_st_lis2mdl_set_power_mode(&LIS2MDL_ctx, DEV_POWER_OFF);
572 if(unlikely(ret)){
573 return -1;
574 }
575 return 0;
576 }
577
drv_mag_st_lis2mdl_read(void * buf,size_t len)578 static int drv_mag_st_lis2mdl_read(void* buf, size_t len)
579 {
580 int ret = 0;
581 size_t size;
582 int16_t pnRawData[3];
583 uint8_t buffer[6];
584 uint8_t i = 0;
585 uint16_t sensitivity = 0;
586
587 mag_data_t* pdata = (mag_data_t*)buf;
588 if(buf == NULL){
589 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
590 return -1;
591 }
592
593 size = sizeof(mag_data_t);
594 if(len < size){
595 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
596 return -1;
597 }
598
599 ret = sensor_i2c_read(&LIS2MDL_ctx, (LIS2MDL_MAG_OUTX_L_REG | 0x80), buffer, 6, I2C_OP_RETRIES);
600 if(unlikely(ret)){
601 return -1;
602 }
603 for(i=0; i<3; i++)
604 {
605 pnRawData[i]=((((uint16_t)buffer[2*i+1]) << 8) | (uint16_t)buffer[2*i]);
606 }
607 //LOG("%s %s: %d, %d, %d \n", SENSOR_STR, __func__, pnRawData[0], pnRawData[1], pnRawData[2]);
608
609 sensitivity = LIS2MDL_MAG_SENSITIVITY_FOR_FS_50GA;
610
611 for(i=0; i<3; i++)
612 {
613 //pdata->data[i] = ( int16_t )(pnRawData[i] * sensitivity);
614 pdata->data[i] = pnRawData[i] * sensitivity / 1000;
615 }
616 pdata->timestamp = aos_now_ms();
617
618 return (int)size;
619 }
620
621
drv_mag_st_lis2mdl_ioctl(int cmd,unsigned long arg)622 static int drv_mag_st_lis2mdl_ioctl(int cmd, unsigned long arg)
623 {
624 int ret = 0;
625 dev_sensor_info_t *info = (dev_sensor_info_t *)arg;
626
627 switch(cmd){
628 case SENSOR_IOCTL_ODR_SET:{
629 ret = drv_mag_st_lis2mdl_set_odr(&LIS2MDL_ctx, arg);
630 if(unlikely(ret)){
631 return -1;
632 }
633 }break;
634 case SENSOR_IOCTL_RANGE_SET:{
635 ret = drv_mag_st_lis2mdl_set_range(&LIS2MDL_ctx, arg);
636 if(unlikely(ret)){
637 return -1;
638 }
639 }break;
640 case SENSOR_IOCTL_SET_POWER:{
641 ret = drv_mag_st_lis2mdl_set_power_mode(&LIS2MDL_ctx, arg);
642 if(unlikely(ret)){
643 return -1;
644 }
645 }break;
646 case SENSOR_IOCTL_GET_INFO:{
647 /* fill the dev info here */
648 //dev_sensor_info_t *info = (dev_sensor_info_t *)arg;
649 info->model = "LIS2MDL";
650 info->range_max = 50;
651 info->range_min = 50;
652 info->unit = mGauss;
653 }break;
654
655 case SENSOR_IOCTL_SELF_TEST:{
656 ret = drv_mag_st_lis2mdl_self_test(&LIS2MDL_ctx, (int32_t*)info->data);
657 //printf("%d %d %d\n",info->data[0],info->data[1],info->data[2]);
658 LOG("%s %s: %d, %d, %d\n", SENSOR_STR, __func__, info->data[0],info->data[1],info->data[2]);
659 return ret;
660 }
661
662 default:break;
663 }
664
665 return 0;
666 }
667
drv_mag_st_lis2mdl_init(void)668 int drv_mag_st_lis2mdl_init(void){
669 int ret = 0;
670 sensor_obj_t sensor;
671 memset(&sensor, 0, sizeof(sensor));
672 /* fill the sensor obj parameters here */
673 sensor.io_port = I2C_PORT;
674 sensor.tag = TAG_DEV_MAG;
675 sensor.path = dev_mag_path;
676 sensor.open = drv_mag_st_lis2mdl_open;
677 sensor.close = drv_mag_st_lis2mdl_close;
678 sensor.read = drv_mag_st_lis2mdl_read;
679 sensor.write = NULL;
680 sensor.ioctl = drv_mag_st_lis2mdl_ioctl;
681 sensor.irq_handle = drv_mag_st_lis2mdl_irq_handle;
682
683 ret = sensor_create_obj(&sensor);
684 if(unlikely(ret)){
685 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
686 return -1;
687 }
688
689 ret = drv_mag_st_lis2mdl_validate_id(&LIS2MDL_ctx, I_AM_LIS2MDL);
690 if(unlikely(ret)){
691 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
692 return -1;
693 }
694
695 ret = drv_mag_st_lis2mdl_soft_reset(&LIS2MDL_ctx);
696 if(unlikely(ret)){
697 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
698 return -1;
699 }
700
701 ret = drv_mag_st_lis2mdl_set_default_config(&LIS2MDL_ctx);
702 if(unlikely(ret)){
703 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
704 return -1;
705 }
706
707 ret = drv_mag_st_lis2mdl_enable_temp(&LIS2MDL_ctx);
708 if(unlikely(ret)){
709 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
710 return -1;
711 }
712
713 ret = drv_mag_st_lis2mdl_enable_off_canc(&LIS2MDL_ctx);
714 if(unlikely(ret)){
715 LOG("%s %s %s %d\n", SENSOR_STR, __func__, ERROR_LINE, __LINE__);
716 return -1;
717 }
718 /* update the phy sensor info to sensor hal */
719 LOG("%s %s successfully \n", SENSOR_STR, __func__);
720 return 0;
721 }
722
723 SENSOR_DRV_ADD(drv_mag_st_lis2mdl_init);
724
725
726