1 /* Core file for MiraMEMS 3-Axis Accelerometer's driver.
2 *
3 * mir3da_core.c - Linux kernel modules for 3-Axis Accelerometer
4 *
5 * Copyright (C) 2011-2013 MiraMEMS Sensing Technology Co., Ltd.
6 *
7 * This software is licensed under the terms of the GNU General Public
8 * License version 2, as published by the Free Software Foundation, and
9 * may be copied, distributed, and modified under those terms.
10 *
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 */
17 #include "da223_core.h"
18 #include "da223_cust.h"
19
20 #define MIR3DA_REG_ADDR(REG) ((REG)&0xFF)
21
22 #define MIR3DA_OFFSET_THRESHOLD 20
23 #define PEAK_LVL 800
24 #define STICK_LSB 2000
25 #define AIX_HISTORY_SIZE 20
26
27 typedef struct reg_obj_s {
28
29 short addr;
30 unsigned char mask;
31 unsigned char value;
32
33 } reg_obj_t;
34
35 struct gsensor_data_fmt_s {
36
37 unsigned char msbw;
38 unsigned char lsbw;
39 unsigned char endian; /* 0: little endian; 1: big endian */
40 };
41
42 struct gsensor_data_obj_s {
43
44 #define MIR3DA_DATA_LEN 6
45 reg_obj_t data_sect[MIR3DA_DATA_LEN];
46 struct gsensor_data_fmt_s data_fmt;
47 };
48
49 struct gsensor_obj_s {
50
51 char asic[10];
52
53 reg_obj_t chip_id;
54 reg_obj_t mod_id;
55 reg_obj_t soft_reset;
56 reg_obj_t power;
57
58 #define MIR3DA_INIT_SECT_LEN 11
59 #define MIR3DA_OFF_SECT_LEN MIR3DA_OFFSET_LEN
60 #define MIR3DA_ODR_SECT_LEN 3
61
62 reg_obj_t init_sect[MIR3DA_INIT_SECT_LEN];
63 reg_obj_t offset_sect[MIR3DA_OFF_SECT_LEN];
64 reg_obj_t odr_sect[MIR3DA_ODR_SECT_LEN];
65
66 struct gsensor_data_obj_s data;
67
68 int (*calibrate)(MIR_HANDLE handle, int z_dir);
69 int (*auto_calibrate)(MIR_HANDLE handle, int xyz[3]);
70 int (*int_ops)(MIR_HANDLE handle, mir_int_ops_t *ops);
71 int (*get_reg_data)(MIR_HANDLE handle, char *buf);
72 };
73
74 struct gsensor_drv_s {
75
76 struct general_op_s *method;
77
78 struct gsensor_obj_s *obj;
79 };
80
81 typedef enum _asic_type{
82 ASIC_NONE,
83 ASIC_2511,
84 ASIC_2512B,
85 ASIC_2513A,
86 ASIC_2516,
87 } asic_type;
88
89 typedef enum _mems_type{
90 MEMS_NONE,
91 MEMS_T4,
92 MEMS_T9,
93 MEMS_TV03,
94 MEMS_RTO3,
95 MEMS_GT2,
96 MEMS_GT3,
97 } mems_type;
98
99 typedef enum _package_type{
100 PACKAGE_NONE,
101 PACKAGE_2X2_12PIN,
102 PACKAGE_3X3_10PIN,
103 PACKAGE_3X3_16PIN,
104 } package_type;
105
106 struct chip_info_s{
107 unsigned char reg_value;
108 package_type package;
109 asic_type asic;
110 mems_type mems;
111 };
112
113 static struct chip_info_s gsensor_chip_info;
114
115 static struct chip_info_s mir3da_chip_info_list[]=
116 {
117 {0x00,PACKAGE_2X2_12PIN,ASIC_2512B,MEMS_TV03},
118 {0x01,PACKAGE_2X2_12PIN,ASIC_2511,MEMS_T4},
119 {0x02,PACKAGE_2X2_12PIN,ASIC_2511,MEMS_T9},
120 {0x03,PACKAGE_3X3_10PIN,ASIC_2511,MEMS_T4},
121 {0x04,PACKAGE_3X3_10PIN,ASIC_2511,MEMS_T9},
122 {0x05,PACKAGE_3X3_10PIN,ASIC_2511,MEMS_T4},
123 {0x06,PACKAGE_3X3_10PIN,ASIC_2511,MEMS_T9},
124 {0x07,PACKAGE_3X3_16PIN,ASIC_2511,MEMS_T4},
125 {0x08,PACKAGE_3X3_16PIN,ASIC_2511,MEMS_T9},
126 {0x09,PACKAGE_2X2_12PIN,ASIC_2511,MEMS_T4},
127 {0x0c,PACKAGE_2X2_12PIN,ASIC_2512B,MEMS_T9},
128 {0x33,PACKAGE_2X2_12PIN,ASIC_2511,MEMS_T9},
129 {0x34,PACKAGE_2X2_12PIN,ASIC_2511,MEMS_T9},
130 {0x35,PACKAGE_2X2_12PIN,ASIC_2511,MEMS_T9},
131 };
132
133 #define MIR3DA_NSA_INIT_SECTION { NSA_REG_G_RANGE, 0x03, 0x00 }, \
134 { NSA_REG_POWERMODE_BW, 0xFF, 0x3e }, \
135 { NSA_REG_ODR_AXIS_DISABLE, 0xFF, 0x09 }, \
136 { NSA_REG_INTERRUPT_SETTINGS2, 0xFF, 0x00 }, \
137 { NSA_REG_INTERRUPT_MAPPING2, 0xFF, 0x00 }, \
138 { NSA_REG_ENGINEERING_MODE, 0xFF, 0x83 }, \
139 { NSA_REG_ENGINEERING_MODE, 0xFF, 0x69 }, \
140 { NSA_REG_ENGINEERING_MODE, 0xFF, 0xBD }, \
141 { NSA_REG_INT_PIN_CONFIG, 0x0F, 0x05 }, \
142 { -1, 0x00, 0x00 }, \
143 { -1, 0x00, 0x00 }, \
144
145 #define MIR3DA_NSA_OFFSET_SECTION { NSA_REG_COARSE_OFFSET_TRIM_X, 0xFF, 0x00 }, \
146 { NSA_REG_COARSE_OFFSET_TRIM_Y, 0xFF, 0x00 }, \
147 { NSA_REG_COARSE_OFFSET_TRIM_Z, 0xFF, 0x00 }, \
148 { NSA_REG_FINE_OFFSET_TRIM_X, 0xFF, 0x00 }, \
149 { NSA_REG_FINE_OFFSET_TRIM_Y, 0xFF, 0x00 }, \
150 { NSA_REG_FINE_OFFSET_TRIM_Z, 0xFF, 0x00 }, \
151 { NSA_REG_CUSTOM_OFFSET_X, 0xFF, 0x00 }, \
152 { NSA_REG_CUSTOM_OFFSET_Y, 0xFF, 0x00 }, \
153 { NSA_REG_CUSTOM_OFFSET_Z, 0xFF, 0x00 }, \
154
155 #define MIR3DA_NSA_ODR_SECTION { NSA_REG_ODR_AXIS_DISABLE, 0x0F, 0x06 }, \
156 { NSA_REG_ODR_AXIS_DISABLE, 0x0F, 0x07 }, \
157 { NSA_REG_ODR_AXIS_DISABLE, 0x0F, 0x08 }, \
158
159
160 #define MIR3DA_NSA_DATA_SECTION { { NSA_REG_ACC_X_LSB, 0xFF, 0x00 }, \
161 { NSA_REG_ACC_X_MSB, 0xFF, 0x00 }, \
162 { NSA_REG_ACC_Y_LSB, 0xFF, 0x00 }, \
163 { NSA_REG_ACC_Y_MSB, 0xFF, 0x00 }, \
164 { NSA_REG_ACC_Z_LSB, 0xFF, 0x00 }, \
165 { NSA_REG_ACC_Z_MSB, 0xFF, 0x00 } }, \
166 { 8, 4, 0 }
167
168 static int NSA_NTO_calibrate(MIR_HANDLE handle, int z_dir);
169 static int NSA_NTO_auto_calibrate(MIR_HANDLE handle, int xyz[3]);
170 #if MIR3DA_AUTO_CALIBRATE
171 static int mir3da_auto_calibrate(MIR_HANDLE handle, int x, int y, int z);
172 #endif /* !MIR3DA_AUTO_CALIBRATE */
173 static int NSA_interrupt_ops(MIR_HANDLE handle, mir_int_ops_t *ops);
174 static int NSA_get_reg_data(MIR_HANDLE handle, char *buf);
175
176 #define MIR_NSA_NTO { "NSA_NTO", { NSA_REG_WHO_AM_I, 0xFF, 0x13 }, \
177 { NSA_REG_FIFO_CTRL, 0xFF, 0x00 }, \
178 { NSA_REG_SPI_I2C, 0x24, 0x24 }, \
179 { NSA_REG_POWERMODE_BW, 0x80, 0x80 }, \
180 { MIR3DA_NSA_INIT_SECTION }, \
181 { MIR3DA_NSA_OFFSET_SECTION }, \
182 { MIR3DA_NSA_ODR_SECTION }, \
183 { MIR3DA_NSA_DATA_SECTION }, \
184 NSA_NTO_calibrate , \
185 NSA_NTO_auto_calibrate , \
186 NSA_interrupt_ops , \
187 NSA_get_reg_data , \
188 }
189 /**************************************************************** COMMON ***************************************************************************/
190 #define MIR3DA_GSENSOR_SCHEME MIR3DA_SUPPORT_CHIP_LIST
191
192 #if YZ_CROSS_TALK_ENABLE
193 static short yzcross;
194 #endif
195
196 /* this level can be modified while runtime through system attribute */
197 int mir3da_Log_level = 0;//|DEBUG_ASSERT|DEBUG_MSG|DEBUG_FUNC|DEBUG_DATA;
198 static int gsensor_mod = -1; /* Initial value */
199 static int gsensor_type = -1; /* Initial value */
200 static struct gsensor_obj_s mir3da_gsensor[] = { MIR3DA_GSENSOR_SCHEME };
201 struct gsensor_drv_s mir3da_gsensor_drv;
202 static int is_da217 = -1;
203
204 #define MI_DATA(format, ...) if(DEBUG_DATA&mir3da_Log_level){mir3da_gsensor_drv.method->myprintf(MI_TAG format "\n", ## __VA_ARGS__);}
205 #define MI_MSG(format, ...) if(DEBUG_MSG&mir3da_Log_level){mir3da_gsensor_drv.method->myprintf(MI_TAG format "\n", ## __VA_ARGS__);}
206 #define MI_ERR(format, ...) if(DEBUG_ERR&mir3da_Log_level){mir3da_gsensor_drv.method->myprintf(MI_TAG format "\n", ## __VA_ARGS__);}
207 #define MI_FUN if(DEBUG_FUNC&mir3da_Log_level){mir3da_gsensor_drv.method->myprintf(MI_TAG "%s is called, line: %d\n", __FUNCTION__,__LINE__);}
208 #define MI_ASSERT(expr) \
209 if (!(expr)) {\
210 mir3da_gsensor_drv.method->myprintf("Assertion failed! %s,%d,%s,%s\n",\
211 __FILE__, __LINE__, __func__, #expr);\
212 }
213
214 //#define abs(x) ({ long __x = (x); (__x < 0) ? -__x : __x; })
215
216 #if FILTER_AVERAGE_ENHANCE
217 typedef struct FilterAverageContextTag{
218 int sample_l;
219 int sample_h;
220 int filter_param_l;
221 int filter_param_h;
222 int filter_threhold;
223
224 int refN_l;
225 int refN_h;
226
227 } FilterAverageContext;
228
229 typedef struct mir3da_core_ctx_s{
230 struct mir3da_filter_param_s filter_param;
231 FilterAverageContext tFac[3];
232 } mir3da_core_ctx;
233
234 static mir3da_core_ctx core_ctx;
235 #endif
236
237 #if MIR3DA_SENS_TEMP_SOLUTION
238 static int bSensZoom = 0;
239 #endif
240
241 #if MIR3DA_OFFSET_TEMP_SOLUTION
242 static int is_cali = 0;
243 static char bLoad = FILE_CHECKING;
244 static char readOffsetCnt=0;
245 static char readsubfileCnt=0;
246 static unsigned char original_offset[9];
247 static int mir3da_write_offset_to_file(unsigned char* offset);
248 static int mir3da_read_offset_from_file(unsigned char* offset);
249 static void manual_load_cali_file(MIR_HANDLE handle);
250 #endif /* !MIR3DA_OFFSET_TEMP_SOLUTION */
251
252 #if MIR3DA_STK_TEMP_SOLUTION
253 static short aixHistort[AIX_HISTORY_SIZE*3] = {0};
254 static short aixHistoryIndex = 0;
255 static char bxstk = 0;
256 static char bystk = 0;
257 static char bzstk = 0;
258
addAixHistory(short x,short y,short z)259 static void addAixHistory(short x,short y,short z){
260 aixHistort[aixHistoryIndex++] = x;
261 aixHistort[aixHistoryIndex++] = y;
262 aixHistort[aixHistoryIndex++] = z;
263 aixHistoryIndex = (aixHistoryIndex)%(AIX_HISTORY_SIZE*3);
264 }
265
isXStick(void)266 static char isXStick(void){
267 int i=0,j=0,temp=0;
268 for (i = 0; i < AIX_HISTORY_SIZE; i++){
269 if ((abs(aixHistort[i*3]) < STICK_LSB)&&(aixHistort[i*3] != 0)){
270 break;
271 }
272 }
273
274 for(j = 0; j< AIX_HISTORY_SIZE; j++){
275 temp |= aixHistort[j*3];
276 }
277
278 if(0 == temp)
279 return 1;
280
281 return i == AIX_HISTORY_SIZE;
282 }
283
isYStick(void)284 static char isYStick(void){
285 int i=0,j=0,temp=0;
286 for (i = 0; i < AIX_HISTORY_SIZE; i++){
287 if ((abs(aixHistort[i*3+1]) < STICK_LSB)&&(aixHistort[i*3+1] != 0)){
288 break;
289 }
290 }
291
292 for(j = 0; j < AIX_HISTORY_SIZE; j++){
293 temp |= aixHistort[j*3+1];
294 }
295
296 if(0 == temp)
297 return 1;
298
299 return i == AIX_HISTORY_SIZE;
300 }
301
isZStick(void)302 static char isZStick(void){
303 int i=0,j=0,temp=0;
304 for (i = 0; i < AIX_HISTORY_SIZE; i++){
305 if ((abs(aixHistort[i*3+2]) < STICK_LSB)&&(aixHistort[i*3+2] != 0)){
306 break;
307 }
308 }
309
310 for(j = 0; j < AIX_HISTORY_SIZE; j++){
311 temp |= aixHistort[j*3+2];
312 }
313
314 if(0 == temp)
315 return 1;
316
317 return i == AIX_HISTORY_SIZE;
318 }
319
squareRoot(int val)320 static int squareRoot(int val){
321 int r = 0;
322 int shift;
323
324 if (val < 0){
325 return 0;
326 }
327
328 for(shift=0;shift<32;shift+=2)
329 {
330 int x=0x40000000l >> shift;
331 if(x + r <= val)
332 {
333 val -= x + r;
334 r = (r >> 1) | x;
335 } else{
336 r = r >> 1;
337 }
338 }
339
340 return r;
341 }
342 #endif /* ! MIR3DA_STK_TEMP_SOLUTION */
343
344 #if FILTER_AVERAGE_ENHANCE
filter_average(short preAve,short sample,int paramN,int * refNum)345 static short filter_average(short preAve, short sample, int paramN, int* refNum)
346 {
347 #if FILTER_AVERAGE_EX
348 if( abs(sample-preAve) > PEAK_LVL && *refNum < 3 ){
349 MI_DATA("Hit, sample = %d, preAve = %d, refN =%d\n", sample, preAve, *refNum);
350 sample = preAve;
351 (*refNum) ++;
352 }else{
353 if (*refNum == 3){
354 preAve = sample;
355 }
356 *refNum = 0;
357 }
358 #endif
359
360 return preAve + (sample - preAve)/paramN;
361 }
362
filter_average_enhance(FilterAverageContext * fac,short sample)363 static int filter_average_enhance(FilterAverageContext* fac, short sample)
364 {
365 if (fac == 0){
366 MI_ERR("0 parameter fac");
367 return 0;
368 }
369
370 if (fac->filter_param_l == fac->filter_param_h){
371 fac->sample_l = fac->sample_h = filter_average(fac->sample_l, sample, fac->filter_param_l, &fac->refN_l);
372 }else{
373 fac->sample_l = filter_average(fac->sample_l, sample, fac->filter_param_l, &fac->refN_l);
374 fac->sample_h= filter_average(fac->sample_h, sample, fac->filter_param_h, &fac->refN_h);
375 if (abs(fac->sample_l- fac->sample_h) > fac->filter_threhold){
376 MI_DATA("adjust, fac->sample_l = %d, fac->sample_h = %d\n", fac->sample_l, fac->sample_h);
377 fac->sample_h = fac->sample_l;
378 }
379 }
380
381 return fac->sample_h;
382 }
383 #endif /* ! FILTER_AVERAGE_ENHANCE */
384
mir3da_register_read(MIR_HANDLE handle,short addr,unsigned char * data)385 int mir3da_register_read(MIR_HANDLE handle, short addr, unsigned char *data)
386 {
387 int res = 0;
388
389 res = mir3da_gsensor_drv.method->smi.read(handle, MIR3DA_REG_ADDR(addr), data);
390
391 return res;
392 }
393
mir3da_register_read_continuously(MIR_HANDLE handle,short addr,unsigned char count,unsigned char * data)394 int mir3da_register_read_continuously(MIR_HANDLE handle, short addr, unsigned char count, unsigned char *data)
395 {
396 int res = 0;
397
398 res = (count==mir3da_gsensor_drv.method->smi.read_block(handle, MIR3DA_REG_ADDR(addr), count, data)) ? 0 : 1;
399
400 return res;
401 }
402
mir3da_register_write(MIR_HANDLE handle,short addr,unsigned char data)403 int mir3da_register_write(MIR_HANDLE handle, short addr, unsigned char data)
404 {
405 int res = 0;
406
407 res = mir3da_gsensor_drv.method->smi.write(handle, MIR3DA_REG_ADDR(addr), data);
408
409 return res;
410 }
411
mir3da_register_mask_write(MIR_HANDLE handle,short addr,unsigned char mask,unsigned char data)412 int mir3da_register_mask_write(MIR_HANDLE handle, short addr, unsigned char mask, unsigned char data)
413 {
414 int res = 0;
415 unsigned char tmp_data;
416
417 res = mir3da_register_read(handle, addr, &tmp_data);
418 if(res) {
419 return res;
420 }
421
422 tmp_data &= ~mask;
423 tmp_data |= data & mask;
424 res = mir3da_register_write(handle, addr, tmp_data);
425
426 return res;
427 }
428
mir3da_read_raw_data(MIR_HANDLE handle,short * x,short * y,short * z)429 static int mir3da_read_raw_data(MIR_HANDLE handle, short *x, short *y, short *z)
430 {
431 unsigned char tmp_data[6] = {0};
432
433 if (mir3da_register_read_continuously(handle, mir3da_gsensor_drv.obj[gsensor_mod].data.data_sect[0].addr, 6, tmp_data) != 0) {
434 MI_ERR("i2c block read failed\n");
435 return -1;
436 }
437
438 *x = ((short)(tmp_data[1] << mir3da_gsensor_drv.obj[gsensor_mod].data.data_fmt.msbw | tmp_data[0]))>> (8-mir3da_gsensor_drv.obj[gsensor_mod].data.data_fmt.lsbw);
439 *y = ((short)(tmp_data[3] << mir3da_gsensor_drv.obj[gsensor_mod].data.data_fmt.msbw | tmp_data[2]))>> (8-mir3da_gsensor_drv.obj[gsensor_mod].data.data_fmt.lsbw);
440 *z = ((short)(tmp_data[5] << mir3da_gsensor_drv.obj[gsensor_mod].data.data_fmt.msbw | tmp_data[4]))>> (8-mir3da_gsensor_drv.obj[gsensor_mod].data.data_fmt.lsbw);
441
442 MI_DATA("mir3da_raw: x=%d, y=%d, z=%d", *x, *y, *z);
443
444 #if MIR3DA_SENS_TEMP_SOLUTION
445 if (bSensZoom == 1){
446 *z = (*z )*5/4;
447 MI_DATA("SensZoom take effect, Zoomed Z = %d", *z);
448 }
449 #endif
450
451 #if YZ_CROSS_TALK_ENABLE
452 if(yzcross)
453 *y=*y-(*z)*yzcross/100;
454 #endif
455 return 0;
456 }
457
458 static int remap[8][4] = {{0,0,0,0},
459 {0,1,0,1},
460 {1,1,0,0},
461 {1,0,0,1},
462 {1,0,1,0},
463 {0,0,1,1},
464 {0,1,1,0},
465 {1,1,1,1}};
466
mir3da_direction_remap(short * x,short * y,short * z,int direction)467 int mir3da_direction_remap(short *x,short *y, short *z, int direction)
468 {
469 short temp = 0;
470
471 *x = *x - ((*x) * remap[direction][0]*2);
472 *y = *y - ((*y) * remap[direction][1]*2);
473 *z = *z - ((*z) * remap[direction][2]*2);
474
475 if(remap[direction][3])
476 {
477 temp = *x;
478 *x = *y;
479 *y = temp;
480 }
481
482 if(remap[direction][2]) {
483 return -1;
484 }
485
486 return 1;
487 }
488
mir3da_read_step(MIR_HANDLE handle,unsigned short * count)489 int mir3da_read_step(MIR_HANDLE handle, unsigned short *count){
490 unsigned char step_temp[2];
491
492 mir3da_register_read_continuously(handle, NSA_REG_STEPS_MSB, 2, step_temp);
493
494 *count = ((step_temp[0]<<8) + step_temp[1])/2;
495 return 0;
496 }
497
mir3da_read_data(MIR_HANDLE handle,short * x,short * y,short * z)498 int mir3da_read_data(MIR_HANDLE handle, short *x, short *y, short *z)
499 {
500 int rst = 0;
501
502 #if MIR3DA_SUPPORT_MULTI_LAYOUT
503 short temp =0;
504 #endif
505
506 #if MIR3DA_OFFSET_TEMP_SOLUTION
507 if(is_cali){
508 *x = *y = *z = 0;
509 return 0;
510 }
511
512 manual_load_cali_file(handle);
513 #endif
514
515 rst = mir3da_read_raw_data(handle, x, y, z);
516 if (rst != 0){
517 MI_ERR("mir3da_read_raw_data failed, rst = %d", rst);
518 return rst;
519 }
520
521 #if MIR3DA_AUTO_CALIBRATE
522 #if MIR3DA_SUPPORT_FAST_AUTO_CALI
523 if((mir3da_gsensor_drv.method->support_fast_auto_cali() &&(bLoad !=FILE_EXIST))||(bLoad ==FILE_NO_EXIST))
524 #else
525 if(bLoad ==FILE_NO_EXIST)
526 #endif
527 {
528 mir3da_auto_calibrate(handle, *x, *y, *z);
529 }
530 #endif
531
532 #if MIR3DA_STK_TEMP_SOLUTION
533 addAixHistory(*x,*y,*z);
534
535 bxstk = isXStick();
536 bystk = isYStick();
537 bzstk = isZStick();
538
539 if((gsensor_chip_info.mems==MEMS_TV03 ||gsensor_chip_info.mems==MEMS_RTO3)
540 &&(gsensor_chip_info.reg_value != 0x4B)
541 &&(gsensor_chip_info.reg_value != 0x8C)
542 &&(gsensor_chip_info.reg_value != 0xCA))
543 {
544 if ((bxstk + bystk+ bzstk) >0){
545 if(resume_times<20){
546 resume_times++;
547 MI_DATA("IN USE STK & resume!!\n");
548 mir3da_chip_resume(handle);
549 }
550 }else
551 resume_times = 0;
552 }
553 else
554 {
555 if ((bxstk + bystk+ bzstk) < 2){
556 if(bxstk)
557 *x = squareRoot(1024*1024 - (*y)*(*y) - (*z)*(*z));
558 if(bystk)
559 *y = squareRoot(1024*1024 - (*x)*(*x) - (*z)*(*z));
560 if(bzstk)
561 *z = squareRoot(1024*1024 - (*x)*(*x) - (*y)*(*y));
562 }else{
563 // MI_ERR( "CHIP ERR !MORE STK!\n");
564 return 0;
565 }
566 }
567 #endif
568
569
570 #if FILTER_AVERAGE_ENHANCE
571 *x = filter_average_enhance(&core_ctx.tFac[0], *x);
572 *y = filter_average_enhance(&core_ctx.tFac[1], *y);
573 *z = filter_average_enhance(&core_ctx.tFac[2], *z);
574 MI_DATA("mir3da_filt: x=%d, y=%d, z=%d", *x, *y, *z);
575 #endif
576
577
578 #if MIR3DA_SUPPORT_MULTI_LAYOUT
579 if(gsensor_chip_info.package ==PACKAGE_2X2_12PIN ){
580 *x =*x;
581 *y =*z;
582 *z =*z;
583 }else if(gsensor_chip_info.package ==PACKAGE_3X3_10PIN){
584 temp = *x;
585 *x = *y;
586 *y = temp;
587 *z =*z;
588 }else if(gsensor_chip_info.package ==PACKAGE_3X3_16PIN){
589 temp = -1*(*x);
590 *x = -1*(*y);
591 *y = temp;
592 *z =*z;
593 }
594 #endif
595
596 if((gsensor_chip_info.reg_value == 0x4B)
597 ||(gsensor_chip_info.reg_value == 0x8C)
598 ||(gsensor_chip_info.reg_value == 0xCA)
599 ||(gsensor_chip_info.mems == MEMS_GT2))
600 {
601 *z = 0;
602 #if MIR3DA_STK_TEMP_SOLUTION
603 bzstk = 1;
604 #endif
605
606 }
607
608 return 0;
609 }
610
cycle_read_xyz(MIR_HANDLE handle,int * x,int * y,int * z,int ncycle)611 static int cycle_read_xyz(MIR_HANDLE handle, int* x, int* y, int* z, int ncycle)
612 {
613 unsigned int j = 0;
614 short raw_x,raw_y,raw_z;
615
616 *x = *y = *z = 0;
617
618 for (j = 0; j < ncycle; j++)
619 {
620 raw_x = raw_y = raw_z = 0;
621 mir3da_read_raw_data (handle, &raw_x, &raw_y, &raw_z);
622
623 (*x) += raw_x;
624 (*y) += raw_y;
625 (*z) += raw_z;
626
627 mir3da_gsensor_drv.method->msdelay(5);
628 }
629
630 (*x) /= ncycle;
631 (*y) /= ncycle;
632 (*z) /= ncycle;
633
634 return 0;
635 }
636
mir3da_read_offset(MIR_HANDLE handle,unsigned char * offset)637 int mir3da_read_offset(MIR_HANDLE handle, unsigned char* offset)
638 {
639 int i, res = 0;
640
641 for(i=0;i<MIR3DA_OFF_SECT_LEN;i++) {
642 if( mir3da_gsensor_drv.obj[gsensor_mod].offset_sect[i].addr < 0 ) {
643 break;
644 }
645
646 res = mir3da_register_read(handle, mir3da_gsensor_drv.obj[gsensor_mod].offset_sect[i].addr, &offset[i]);
647 if(res != 0) {
648 return res;
649 }
650 }
651
652 return res;
653 }
654
mir3da_write_offset(MIR_HANDLE handle,unsigned char * offset)655 int mir3da_write_offset(MIR_HANDLE handle, unsigned char* offset)
656 {
657 int i, res = 0;
658
659 for(i=0;i<MIR3DA_OFF_SECT_LEN;i++) {
660 if( mir3da_gsensor_drv.obj[gsensor_mod].offset_sect[i].addr < 0 ) {
661 break;
662 }
663
664 res = mir3da_register_write(handle, mir3da_gsensor_drv.obj[gsensor_mod].offset_sect[i].addr, offset[i]);
665 if(res != 0) {
666 return res;
667 }
668 }
669
670 return res;
671 }
672
673 #if MIR3DA_OFFSET_TEMP_SOLUTION
mir3da_write_offset_to_file(unsigned char * offset)674 static int mir3da_write_offset_to_file(unsigned char* offset)
675 {
676 int ret = 0;
677
678 if(0 == mir3da_gsensor_drv.method->data_save)
679 return 0;
680
681 ret = mir3da_gsensor_drv.method->data_save(offset);
682
683 MI_MSG("====sensor_sync_write, offset = 0x%x,0x%x,0x%x,0x%x,0x%x,0x%x,0x%x,0x%x,0x%x", offset[0],offset[1],offset[2],offset[3],offset[4],offset[5],offset[6],offset[7],offset[8]);
684
685 return ret;
686 }
687
mir3da_read_offset_from_file(unsigned char * offset)688 static int mir3da_read_offset_from_file(unsigned char* offset)
689 {
690 int ret = 0;
691 int i=0,sum=0;
692
693 if(0 == mir3da_gsensor_drv.method->data_get)
694 return -1;
695
696 ret = mir3da_gsensor_drv.method->data_get(offset);
697
698 for(i=0;i<MIR3DA_OFF_SECT_LEN;i++){
699 sum += offset[i];
700 }
701
702 if(sum==0)
703 return -1;
704
705 MI_MSG("====sensor_sync_read, offset = 0x%x,0x%x,0x%x,0x%x,0x%x,0x%x,0x%x,0x%x,0x%x", offset[0],offset[1],offset[2],offset[3],offset[4],offset[5],offset[6],offset[7],offset[8]);
706
707 return ret;
708 }
709
manual_load_cali_file(MIR_HANDLE handle)710 static void manual_load_cali_file(MIR_HANDLE handle)
711 {
712 unsigned char offset[MIR3DA_OFFSET_LEN] = {0};
713
714 if (bLoad ==FILE_CHECKING){
715
716 readOffsetCnt++;
717
718 if(readOffsetCnt%8 == 0){
719
720 readOffsetCnt =0;
721
722 MI_ERR("====444 manual_load_cali_file(), bLoad = %d, readOffsetCnt=%d.\n", bLoad, readOffsetCnt);
723
724 if(mir3da_gsensor_drv.method->data_check()){
725
726 readsubfileCnt++;
727
728 if(!mir3da_read_offset_from_file(offset)) {
729 MI_ERR("========= FILE EXIST & WRITE OFFSET!");
730 mir3da_write_offset(handle, offset);
731 bLoad = FILE_EXIST;
732 }else if(5 == readsubfileCnt){
733 MI_ERR("========= NO FILE EXIST!");
734 bLoad = FILE_NO_EXIST;
735 }
736 }else{
737 MI_ERR("========= FILE CHECKING....");
738 bLoad = FILE_CHECKING;
739 readsubfileCnt =0;
740 }
741 }
742 }
743 }
744
mir3da_cali_off_to_lsb(int off,int * coarse,int coarse_step,int * fine,int fine_step)745 static void mir3da_cali_off_to_lsb(int off, int *coarse, int coarse_step, int *fine, int fine_step)
746 {
747 *coarse = off/coarse_step;
748 *fine = 100*(off-(*coarse)*coarse_step)/fine_step;
749
750 MI_MSG("off = %d; delta_coarse = %d; delta_fine = %d", off, *coarse, *fine);
751 }
752
753 #if MIR3DA_AUTO_CALIBRATE
NSA_once_calibrate(MIR_HANDLE handle,int coarse_step[3],int fine_step[3],int xyz[3])754 static int NSA_once_calibrate(MIR_HANDLE handle, int coarse_step[3], int fine_step[3], int xyz[3])
755 {
756 int coarse[3] = {0};
757 int coarse_delta[3] = {0};
758 int fine[3] = {0};
759 int fine_delta[3] = {0};
760 int target[3] = {0};
761 int i;
762 unsigned char offset_data[9] = {0};
763
764 if(mir3da_read_offset(handle, offset_data)){
765 MI_ERR("Get old offset failed !");
766 return -1;
767 }
768 coarse[0] = offset_data[0] & 0x3f;
769 coarse[1] = offset_data[1] & 0x3f;
770 coarse[2] = offset_data[2] & 0x3f;
771 fine[0] = (((int)offset_data[0] << 2) & 0x300)|offset_data[3];
772 fine[1] = (((int)offset_data[1] << 2) & 0x300)|offset_data[4];
773 fine[2] = (((int)offset_data[2] << 2) & 0x300)|offset_data[5];
774
775 MI_MSG("Old coarse_x = %d; coarse_y = %d; coarse_z = %d; fine_x = %d; fine_y = %d; fine_z = %d;", coarse[0], coarse[1], coarse[2], fine[0], fine[1], fine[2]);
776
777 /* 0 means auto detect z direction assume z axis is verticle */
778 if ((abs(target[0]) + abs(target[1]) + abs(target[2])) == 0){
779 target[2] = (xyz[2] > 0) ? 1024 : (-1024);
780 }
781
782 for(i = 0;i < 3; i++){
783 coarse_step[i] *= coarse[i] >= 32 ? (-1) : 1;
784 mir3da_cali_off_to_lsb((xyz[i]-target[i]), &coarse_delta[i], coarse_step[i], &fine_delta[i], fine_step[i]);
785
786 coarse[i] += coarse_delta[i];
787 fine[i] += fine_delta[i];
788 offset_data[i] = coarse[i]|((fine[i]>>2)&0xc0);
789 offset_data[i+3] = fine[i]&0xFF;
790 }
791
792 if(mir3da_write_offset(handle, offset_data)){
793 MI_ERR("Update offset failed !");
794 return -1;
795 }
796 /* Discard unstable data after offset register changed */
797 cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5);
798 if(cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 10)){
799 return -1;
800 }
801 MI_MSG("---calibrate_Done, x = %d, y = %d, z = %d, coarse_x = %d, coarse_y = %d, coarse_z = %d, fine_x = %d, fine_y = %d, fine_z = %d", xyz[0], xyz[1], xyz[2], coarse[0], coarse[1], coarse[2], fine[0], fine[1], fine[2]);
802
803 return mir3da_write_offset_to_file(offset_data);
804 }
805 #endif /* !MIR3DA_AUTO_CALIBRATE */
806
NSA_calibrate(MIR_HANDLE handle,int coarse_step[3],int fine_step[3],int fine_max,int target[3])807 static int NSA_calibrate(MIR_HANDLE handle, int coarse_step[3], int fine_step[3], int fine_max, int target[3])
808 {
809 int i = 0, j = 0;
810 unsigned char ncycle = 20;
811 unsigned char nLoop = 20;
812 unsigned char offset_data[9] = {0};
813 unsigned char fine_ok_map = 0;
814
815 int xyz[3] = {0};
816 int coarse[3] = {0};
817 int coarse_delta[3] = {0};
818 int fine[3] = {0};
819 int fine_delta[3] = {0};
820
821 if( (abs(target[0]) + abs(target[1]) + abs(target[2])) != 0 && (abs(target[0]) + abs(target[1]) + abs(target[2])) != 1024 ) {
822 MI_ERR("Invalid argument !");
823 return -1;
824 }
825
826 /* 0 means auto detect z direction assume z axis is verticle */
827 if ((abs(target[0]) + abs(target[1]) + abs(target[2])) == 0){
828 if(cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5)){
829 MI_ERR("check z direction failed\n");
830 return -1;
831 }
832 target[2] = (xyz[2] > 0) ? 1024 : (-1024);
833 }
834
835 MI_MSG("---Start Calibrate, trim target %d, %d, %d---\n", target[0], target[1], target[2]);
836
837 // Stage1: Coarse tune once
838 MI_MSG("---Stage1, coarse tune---");
839 // change to 16G mode
840 if(mir3da_register_mask_write(handle, NSA_REG_G_RANGE, 0x03, 3)){
841 MI_ERR("i2c mask write failed !\n");
842 return -1;
843 }
844
845 /* reset coarse offset register */
846 mir3da_write_offset(handle, offset_data);
847 /* Discard unstable data after offset register changed */
848 cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5);
849
850 if( cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], ncycle) ){
851 goto EXIT_16G_MOD;
852 }
853
854 for(i = 0; i < 3; i++){
855 /* check rule */
856 xyz[i] *= 8;
857
858 coarse[i] = ((xyz[i]-target[i]) > 0) ? 0 : 32;
859
860 MI_MSG("xyz[%d] = %d, coarse[%d] = 0x%x", i, xyz[i], i, coarse[i]);
861
862 coarse_step[i] *= coarse[i] >= 32 ? (-1) : 1;
863 mir3da_cali_off_to_lsb((xyz[i]-target[i]), &coarse_delta[i], coarse_step[i], &fine_delta[i], fine_step[i]);
864
865 coarse[i] += coarse_delta[i];
866 fine[i] += fine_delta[i];
867 mir3da_register_mask_write(handle, NSA_REG_COARSE_OFFSET_TRIM_X+i, 0x3f, (unsigned char)coarse[i]);
868 }
869
870 /* Discard unstable data after offset register changed */
871 cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5);
872 if(cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5)){
873 return -1;
874 }
875 for(i = 0; i < 3; i++){
876 fine[i] += (xyz[i] > 0) ? 0 : fine_max;
877 mir3da_register_write(handle, NSA_REG_FINE_OFFSET_TRIM_X+i, (unsigned char)(fine[i]&0xff));
878 mir3da_register_mask_write(handle, NSA_REG_COARSE_OFFSET_TRIM_X+i, 0xc0, (unsigned char)(0xc0&(fine[i]>>2)));
879 }
880
881 EXIT_16G_MOD:
882 // change back to 2G mode
883 if(mir3da_register_mask_write(handle, NSA_REG_G_RANGE, 0x03, 0)){
884 MI_ERR("i2c mask write failed !\n");
885 return -1;
886 }
887 /* Discard unstable data after offset register changed */
888 cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5);
889 if(cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], ncycle)){
890 return -1;
891 }
892 MI_MSG("---Stage1, coarse tune done: x = %d, y = %d, z = %d, coarse_x = %d, coarse_y = %d, coarse_z = %d, fine_x = %d, fine_y = %d, fine_z = %d", xyz[0], xyz[1], xyz[2], coarse[0], coarse[1], coarse[2], fine[0], fine[1], fine[2]);
893
894 // Stage2: Fine tune
895 MI_MSG("---Stage2, Fine tune---");
896 for (i = 0; i < nLoop; i++){
897
898 if( 0x07==(fine_ok_map & 0x07) ){
899 break;
900 }
901 /* Discard unstable data after offset register changed */
902 cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5);
903 MI_MSG("---Stage2, Fine loop %d", i);
904 if(cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], ncycle)){
905 return -1;
906 }
907
908 for(j = 0; j < 3; j++){
909 MI_MSG("xyz[%d] = %d, caorse[%d] = 0x%x, fine[%d] = 0x%x", j, xyz[j], j, coarse[j], j, fine[j]);
910 if( abs(xyz[j]-target[j]) < MIR3DA_OFFSET_THRESHOLD ){
911 fine_ok_map |= (1<<j);
912 offset_data[j] = coarse[j]|((fine[j]>>2)&0xc0);
913 offset_data[j+3] = fine[j];
914 continue;
915 }
916 mir3da_cali_off_to_lsb((xyz[j]-target[j]), &coarse_delta[j], coarse_step[j], &fine_delta[j], fine_step[j]);
917
918 coarse[j] += coarse_delta[j];
919 fine[j] += fine_delta[j];
920 mir3da_register_write(handle, NSA_REG_FINE_OFFSET_TRIM_X+j, (unsigned char)(fine[j]&0xff));
921 mir3da_register_mask_write(handle, NSA_REG_COARSE_OFFSET_TRIM_X+j, 0xFF, (unsigned char)(0xc0&(fine[j]>>2))|coarse[j]);
922 }
923 }
924 MI_MSG("---Stage2, Fine tune done: x = %d, y = %d, z = %d, coarse_x = %d, coarse_y = %d, coarse_z = %d, fine_x = %d, fine_y = %d, fine_z = %d", xyz[0], xyz[1], xyz[2], coarse[0], coarse[1], coarse[2], fine[0], fine[1], fine[2]);
925
926 if( 0x07==(fine_ok_map & 0x07) ){
927 goto SUCCESS_EXIT;
928 }
929 #if MIR3DA_STK_TEMP_SOLUTION
930 if( 0x03==(fine_ok_map & 0x07) ){
931 goto SUCCESS_EXIT;
932 }
933 #endif
934
935 MI_MSG("---calibrate Failed !---");
936 return -1;
937
938 SUCCESS_EXIT:
939 MI_MSG("---calibrate OK !---");
940 return mir3da_write_offset_to_file(offset_data);
941 }
942
NSA_NTO_cali_step_calc(MIR_HANDLE handle,int coarse[3],int x100_fine[3],int x100_cust[3])943 static int NSA_NTO_cali_step_calc(MIR_HANDLE handle, int coarse[3], int x100_fine[3], int x100_cust[3])
944 {
945 int i;
946 unsigned int total_gain[3] = {0};
947 unsigned char coarse_gain = 0;
948 unsigned char fine_gain[3] = {0};
949 unsigned int const coarse_gain_map[] = {1000, 1125, 1250, 1375, 500, 625, 750, 875}; /* *1000 */
950 unsigned char const fine_dig_gain_map[] = {1, 2, 4, 8};
951
952 if(mir3da_register_read_continuously(handle, NSA_REG_SENSITIVITY_TRIM_X, 3, fine_gain) != 0){
953 MI_ERR("i2c block read failed\n");
954 return -1;
955 }
956
957 if(mir3da_register_read(handle, NSA_REG_SENS_COARSE_TRIM, &coarse_gain) != 0){
958 MI_ERR("i2c block read failed\n");
959 return -1;
960 }
961
962 for(i = 0;i < 3;i++) {
963 // *100*1000
964 total_gain[i] = ((1000 + (fine_gain[i]&0x1F)*1000/32)/15) * fine_dig_gain_map[((fine_gain[i]>>5)&0x03)] * coarse_gain_map[coarse_gain&0x07];
965 coarse[i] = (int)(total_gain[i] * 500 / 100000);
966 x100_fine[i] = (int)(total_gain[i] * 293 / 100000);
967 x100_cust[i] = (int)(total_gain[i] * 390 / 100000);
968 }
969 MI_MSG("coarse_step_x = %d, coarse_step_y = %d, coarse_step_z = %d\n", coarse[0], coarse[1], coarse[2]);
970 MI_MSG("fine_step_x = %d, fine_step_y = %d, fine_step_z = %d\n", x100_fine[0], x100_fine[1], x100_fine[2]);
971 MI_MSG("custom_step_x = %d, custom_step_y = %d, custom_step_z = %d\n", x100_cust[0], x100_cust[1], x100_cust[2]);
972
973 return 0;
974 }
975 #endif /* !MIR3DA_OFFSET_TEMP_SOLUTION */
976
NSA_NTO_calibrate(MIR_HANDLE handle,int z_dir)977 static int NSA_NTO_calibrate(MIR_HANDLE handle, int z_dir)
978 {
979 int result = 0;
980
981 #if MIR3DA_OFFSET_TEMP_SOLUTION
982 int coarse_step[3] = {0};
983 int fine_step[3] = {0};
984 int custom_step[3] = {0};
985 int target[3] = {0};
986
987 unsigned char swap_plarity_old = 0;
988
989 /* compute step */
990 if( NSA_NTO_cali_step_calc(handle, coarse_step, fine_step, custom_step) ) {
991 MI_ERR("Compute step failed !");
992 return -1;
993 }
994 target[2] = z_dir*1024;
995
996 // save swap/plarity old setting
997 if(mir3da_register_read(handle, NSA_REG_SWAP_POLARITY, &swap_plarity_old)){
998 MI_ERR("Get SWAP/PLARITY setting failed !");
999 return -1;
1000 }
1001
1002 if((gsensor_chip_info.asic == ASIC_2512B)||(gsensor_chip_info.asic == ASIC_2513A)||(gsensor_chip_info.asic == ASIC_2516)){
1003 coarse_step[2] = 2 *coarse_step[2];
1004 target[2] = ((swap_plarity_old & (1<<1)) == 0) ? (-target[2]) :target[2];
1005 if(mir3da_register_mask_write(handle, NSA_REG_SWAP_POLARITY, 0x0F, 0x0E)){
1006 MI_ERR("Set Plarity failed !");
1007 return -1;
1008 }
1009 }else if(gsensor_chip_info.asic == ASIC_2511){
1010 target[2] = ((swap_plarity_old & (1<<1)) != 0) ? (-target[2]) :target[2];
1011 if(mir3da_register_mask_write(handle, NSA_REG_SWAP_POLARITY, 0x0F, 0x00)){
1012 MI_ERR("Set Plarity failed !");
1013 return -1;
1014 }
1015 }
1016
1017 result=NSA_calibrate(handle, coarse_step, fine_step, 0x3ff, target);
1018
1019 // Restore swap/plarity setting
1020 if(mir3da_register_mask_write(handle, NSA_REG_SWAP_POLARITY, 0x0F, swap_plarity_old&0x0F)){
1021 MI_ERR("Restore SWAP/PLARITY setting failed !");
1022 return -1;
1023 }
1024 #endif /* !MIR3DA_OFFSET_TEMP_SOLUTION */
1025 return result;
1026 }
1027
NSA_NTO_auto_calibrate(MIR_HANDLE handle,int xyz[3])1028 static int NSA_NTO_auto_calibrate(MIR_HANDLE handle, int xyz[3])
1029 {
1030 int result = 0;
1031
1032 #if MIR3DA_AUTO_CALIBRATE
1033 int coarse_step[3];
1034 int fine_step[3];
1035 int custom_step[3] = {0};
1036 unsigned char swap_plarity_old = 0;
1037 int temp=0;
1038
1039 /* compute step */
1040 if( NSA_NTO_cali_step_calc(handle, coarse_step, fine_step, custom_step) ) {
1041 MI_ERR("Compute step failed !");
1042 return -1;
1043 }
1044
1045 // save swap/plarity old setting
1046 if(mir3da_register_read(handle, NSA_REG_SWAP_POLARITY, &swap_plarity_old)){
1047 MI_ERR("Get SWAP/PLARITY setting failed !");
1048 return -1;
1049 }
1050
1051 if(gsensor_chip_info.asic == ASIC_2512B){
1052 coarse_step[2] = 2 *coarse_step[2];
1053
1054 if((swap_plarity_old & (1<<0))){
1055 temp = xyz[0];
1056 xyz[0] = ((swap_plarity_old & (1<<2)) == 0) ? (-xyz[1]) :xyz[1];
1057 xyz[1] = ((swap_plarity_old & (1<<3)) == 0) ? (-temp) :temp;
1058 }else{
1059 xyz[0] = ((swap_plarity_old & (1<<3)) == 0) ? (-xyz[0]) :xyz[0];
1060 xyz[1] = ((swap_plarity_old & (1<<2)) == 0) ? (-xyz[1]) :xyz[1];
1061 }
1062 xyz[2] = ((swap_plarity_old & (1<<1)) == 0) ? (-xyz[2]) :xyz[2];
1063 }else if(gsensor_chip_info.asic == ASIC_2511){
1064 if((swap_plarity_old & (1<<0))){
1065 temp = xyz[0];
1066 xyz[0] = ((swap_plarity_old & (1<<2)) != 0) ? (-xyz[1]) :xyz[1];
1067 xyz[1] = ((swap_plarity_old & (1<<3)) != 0) ? (-temp) :temp;
1068 }else{
1069 xyz[0] = ((swap_plarity_old & (1<<3)) != 0) ? (-xyz[0]) :xyz[0];
1070 xyz[1] = ((swap_plarity_old & (1<<2)) != 0) ? (-xyz[1]) :xyz[1];
1071 }
1072
1073 xyz[2] = ((swap_plarity_old & (1<<1)) != 0) ? (-xyz[2]) :xyz[2];
1074 }
1075
1076 result = NSA_once_calibrate(handle, coarse_step, fine_step, xyz);
1077 #endif /* !MIR3DA_AUTO_CALIBRATE */
1078 return result;
1079 }
1080
mir3da_calibrate(MIR_HANDLE handle,int z_dir)1081 int mir3da_calibrate(MIR_HANDLE handle, int z_dir)
1082 {
1083 int res = 0;
1084
1085 #if MIR3DA_OFFSET_TEMP_SOLUTION
1086
1087 int xyz[3]={0};
1088
1089 if( is_cali )
1090 return -1;
1091 is_cali = 1;
1092
1093 /* restore original direction if last calibration was done in a wrong direction */
1094 mir3da_write_offset(handle, original_offset);
1095
1096 cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 20);
1097
1098 res = mir3da_gsensor_drv.obj[gsensor_mod].calibrate(handle, z_dir);
1099 if (res != 0){
1100 MI_ERR("Calibrate failed !");
1101 mir3da_write_offset(handle, original_offset);
1102 }else
1103 bLoad = FILE_EXIST;
1104
1105 is_cali = 0;
1106 #endif /* !MIR3DA_OFFSET_TEMP_SOLUTION */
1107 return res;
1108 }
1109
1110 #if MIR3DA_AUTO_CALIBRATE
1111 #define STABLE_CHECK_SAMPLE_NUM 10
1112 #define STABLE_CHECK_THRESHOLD 50000
1113 #define AUTO_CALI_THRESHOLD_XY 200
1114 #define AUTO_CALI_THRESHOLD_Z 200
1115
1116 static unsigned char stable_sample_cnt = 0;
1117 static int stable_sample_pow_sum[STABLE_CHECK_SAMPLE_NUM] = {0};
1118 static int stable_sample_sum[3] = {0};
1119
mir3da_auto_cali_condition_confirm(int x,int y,int z,int ave_xyz[3])1120 static int mir3da_auto_cali_condition_confirm(int x, int y, int z, int ave_xyz[3])
1121 {
1122 int max = 0, min = 0;
1123 int i;
1124 int x_ok=0,y_ok=0,z_ok=0;
1125
1126 stable_sample_pow_sum[stable_sample_cnt] = x*x + y*y + z*z;
1127 stable_sample_sum[0] += x;
1128 stable_sample_sum[1] += y;
1129 stable_sample_sum[2] += z;
1130 stable_sample_cnt++;
1131
1132 MI_MSG("---stable_sample_cnt = %d", stable_sample_cnt);
1133
1134 if( stable_sample_cnt < STABLE_CHECK_SAMPLE_NUM )
1135 return -1;
1136 stable_sample_cnt = 0;
1137
1138 max = stable_sample_pow_sum[0];
1139 min = stable_sample_pow_sum[0];
1140 stable_sample_pow_sum[0] = 0;
1141 for(i = 1; i < STABLE_CHECK_SAMPLE_NUM; i++){
1142 if( stable_sample_pow_sum[i] > max )
1143 max = stable_sample_pow_sum[i];
1144 if( stable_sample_pow_sum[i] < min )
1145 min = stable_sample_pow_sum[i];
1146 stable_sample_pow_sum[i] = 0;
1147 }
1148 MI_MSG("---max = %d; min = %d", max, min);
1149
1150 ave_xyz[0] = stable_sample_sum[0]/STABLE_CHECK_SAMPLE_NUM;
1151 stable_sample_sum[0] = 0;
1152 ave_xyz[1] = stable_sample_sum[1]/STABLE_CHECK_SAMPLE_NUM;
1153 stable_sample_sum[1] = 0;
1154 ave_xyz[2] = stable_sample_sum[2]/STABLE_CHECK_SAMPLE_NUM;
1155 stable_sample_sum[2] = 0;
1156
1157 MI_MSG("ave_x = %d, ave_y = %d, ave_z = %d", ave_xyz[0], ave_xyz[1], ave_xyz[2]);
1158 x_ok = (abs(ave_xyz[0]) < AUTO_CALI_THRESHOLD_XY) ? 1:0;
1159 y_ok = (abs(ave_xyz[1]) < AUTO_CALI_THRESHOLD_XY) ? 1:0;
1160 z_ok = (abs(abs(ave_xyz[2])-1024) < AUTO_CALI_THRESHOLD_Z) ? 1:0;
1161
1162 if( (abs(max-min) > STABLE_CHECK_THRESHOLD) ||((x_ok + y_ok + z_ok) < 2) ) {
1163 return -1;
1164 }
1165
1166 return 0;
1167 }
1168
mir3da_auto_calibrate(MIR_HANDLE handle,int x,int y,int z)1169 static int mir3da_auto_calibrate(MIR_HANDLE handle, int x, int y, int z)
1170 {
1171 int res = 0;
1172 int xyz[3] = {0};
1173
1174 if((gsensor_chip_info.mems== MEMS_RTO3)
1175 ||(gsensor_chip_info.reg_value == 0x4B)
1176 ||(gsensor_chip_info.reg_value == 0x8C)
1177 ||(gsensor_chip_info.reg_value == 0xCA)
1178 ||(gsensor_chip_info.mems == MEMS_GT2))
1179 return -1;
1180
1181 if( is_cali )
1182 return -1;
1183 is_cali = 1;
1184
1185 #if MIR3DA_SUPPORT_FAST_AUTO_CALI
1186 if(mir3da_gsensor_drv.method->support_fast_auto_cali()){
1187 cycle_read_xyz(handle,&xyz[0],&xyz[1],&xyz[2],5);
1188 }
1189 else{
1190 if( mir3da_auto_cali_condition_confirm(x, y, z, xyz) ){
1191 res = -1;
1192 goto EXIT;
1193 }
1194 }
1195 #else
1196 if( mir3da_auto_cali_condition_confirm(x, y, z, xyz) ){
1197 res = -1;
1198 goto EXIT;
1199 }
1200 #endif
1201
1202 mir3da_write_offset(handle, original_offset);
1203
1204 res = mir3da_gsensor_drv.obj[gsensor_mod].auto_calibrate(handle, xyz);
1205 if (res != 0){
1206 MI_ERR("Calibrate failed !");
1207 mir3da_write_offset(handle, original_offset);
1208 }else
1209 bLoad = FILE_EXIST;
1210
1211 EXIT:
1212 is_cali = 0;
1213
1214 return res;
1215 }
1216 #endif /* !MIR3DA_AUTO_CALIBRATE */
1217
NSA_interrupt_ops(MIR_HANDLE handle,mir_int_ops_t * ops)1218 static int NSA_interrupt_ops(MIR_HANDLE handle, mir_int_ops_t *ops)
1219 {
1220 switch(ops->type)
1221 {
1222 case INTERRUPT_OP_INIT:
1223
1224 /* latch */
1225 mir3da_register_mask_write(handle, NSA_REG_INT_LATCH, 0x0f, ops->data.init.latch);
1226 /* active level & output mode */
1227 mir3da_register_mask_write(handle, NSA_REG_INT_PIN_CONFIG, 0x0f, ops->data.init.level|(ops->data.init.pin_mod<<1)|(ops->data.init.level<<2)|(ops->data.init.pin_mod<<3));
1228
1229 break;
1230
1231 case INTERRUPT_OP_ENABLE:
1232 switch( ops->data.int_src )
1233 {
1234 case INTERRUPT_ACTIVITY:
1235 /* Enable active interrupt */
1236 mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_SETTINGS1, 0x07, 0x07);
1237 break;
1238 case INTERRUPT_CLICK:
1239 /* Enable single and double tap detect */
1240 mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_SETTINGS1, 0x30, 0x30);
1241 break;
1242 }
1243 break;
1244
1245 case INTERRUPT_OP_CONFIG:
1246
1247 switch( ops->data.cfg.int_src )
1248 {
1249 case INTERRUPT_ACTIVITY:
1250
1251 mir3da_register_write(handle, NSA_REG_ACTIVE_THRESHOLD, ops->data.cfg.int_cfg.act.threshold);
1252 mir3da_register_mask_write(handle, NSA_REG_ACTIVE_DURATION, 0x03, ops->data.cfg.int_cfg.act.duration);
1253
1254 /* Int mapping */
1255 if(ops->data.cfg.pin == INTERRUPT_PIN1) {
1256 mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_MAPPING1, (1<<2), (1<<2));
1257 }
1258 else if(ops->data.cfg.pin == INTERRUPT_PIN2) {
1259 mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_MAPPING3, (1<<2), (1<<2));
1260 }
1261 break;
1262
1263 case INTERRUPT_CLICK:
1264
1265 mir3da_register_mask_write(handle, NSA_REG_TAP_THRESHOLD, 0x1f, ops->data.cfg.int_cfg.clk.threshold);
1266 mir3da_register_mask_write(handle, NSA_REG_TAP_DURATION, (0x03<<5)|(0x07), (ops->data.cfg.int_cfg.clk.quiet_time<<7)|(ops->data.cfg.int_cfg.clk.click_time<<6)|(ops->data.cfg.int_cfg.clk.window));
1267
1268 if(ops->data.cfg.pin == INTERRUPT_PIN1) {
1269 mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_MAPPING1, 0x30, 0x30);
1270 }
1271 else if(ops->data.cfg.pin == INTERRUPT_PIN2) {
1272 mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_MAPPING3, 0x30, 0x30);
1273 }
1274 break;
1275 }
1276 break;
1277
1278 case INTERRUPT_OP_DISABLE:
1279 switch( ops->data.int_src )
1280 {
1281 case INTERRUPT_ACTIVITY:
1282 /* Enable active interrupt */
1283 mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_SETTINGS1, 0x07, 0x00);
1284 break;
1285
1286 case INTERRUPT_CLICK:
1287 /* Enable single and double tap detect */
1288 mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_SETTINGS1, 0x30, 0x00);
1289 break;
1290 }
1291 break;
1292
1293 default:
1294 MI_ERR("Unsupport operation !");
1295 }
1296 return 0;
1297 }
1298
mir3da_interrupt_ops(MIR_HANDLE handle,mir_int_ops_t * ops)1299 int mir3da_interrupt_ops(MIR_HANDLE handle, mir_int_ops_t *ops)
1300 {
1301 int res = 0;
1302
1303 res = mir3da_gsensor_drv.obj[gsensor_mod].int_ops(handle, ops);
1304 return res;
1305 }
1306
1307 #if FILTER_AVERAGE_ENHANCE
mir3da_get_filter_param(struct mir3da_filter_param_s * param)1308 int mir3da_get_filter_param(struct mir3da_filter_param_s* param){
1309 if (param == 0){
1310 MI_ERR("Invalid param!");
1311 return -1;
1312 }
1313
1314 param->filter_param_h = core_ctx.tFac[0].filter_param_h;
1315 param->filter_param_l = core_ctx.tFac[0].filter_param_l;
1316 param->filter_threhold = core_ctx.tFac[0].filter_threhold;
1317
1318 MI_MSG("FILTER param is get: filter_param_h = %d, filter_param_l = %d, filter_threhold = %d", param->filter_param_h, param->filter_param_l, param->filter_threhold);
1319
1320 return 0;
1321 }
1322
mir3da_set_filter_param(struct mir3da_filter_param_s * param)1323 int mir3da_set_filter_param(struct mir3da_filter_param_s* param){
1324
1325 if (param == 0){
1326 MI_ERR("Invalid param!");
1327 return -1;
1328 }
1329
1330 MI_MSG("FILTER param is set: filter_param_h = %d, filter_param_l = %d, filter_threhold = %d", param->filter_param_h, param->filter_param_l, param->filter_threhold);
1331
1332 core_ctx.tFac[1].filter_param_l = core_ctx.tFac[2].filter_param_l = core_ctx.tFac[0].filter_param_l = param->filter_param_l;
1333 core_ctx.tFac[1].filter_param_h =core_ctx.tFac[2].filter_param_h = core_ctx.tFac[0].filter_param_h = param->filter_param_h;
1334 core_ctx.tFac[1].filter_threhold = core_ctx.tFac[2].filter_threhold =core_ctx.tFac[0].filter_threhold = param->filter_threhold;
1335
1336 return 0;
1337 }
1338 #endif //#if FILTER_AVERAGE_ENHANCE
1339
mir3da_get_enable(MIR_HANDLE handle,char * enable)1340 int mir3da_get_enable(MIR_HANDLE handle, char *enable)
1341 {
1342 int res = 0;
1343 unsigned char reg_data=0;
1344
1345 res = mir3da_register_read(handle, mir3da_gsensor_drv.obj[gsensor_mod].power.addr, ®_data);
1346 if(res != 0) {
1347 return res;
1348 }
1349
1350 *enable = ( reg_data & mir3da_gsensor_drv.obj[gsensor_mod].power.mask ) ? 0 : 1;
1351
1352 return res;
1353 }
1354
mir3da_set_enable(MIR_HANDLE handle,char enable)1355 int mir3da_set_enable(MIR_HANDLE handle, char enable)
1356 {
1357 int res = 0;
1358 unsigned char reg_data = 0;
1359
1360 if(!enable) {
1361 reg_data = mir3da_gsensor_drv.obj[gsensor_mod].power.value;
1362 }
1363
1364 res = mir3da_register_mask_write(handle, mir3da_gsensor_drv.obj[gsensor_mod].power.addr, mir3da_gsensor_drv.obj[gsensor_mod].power.mask, reg_data);
1365
1366 return res;
1367 }
1368
NSA_get_reg_data(MIR_HANDLE handle,char * buf)1369 static int NSA_get_reg_data(MIR_HANDLE handle, char *buf)
1370 {
1371 int count = 0;
1372 int i;
1373 unsigned char val;
1374
1375 count += mir3da_gsensor_drv.method->mysprintf(buf+count, "---------start---------");
1376 for (i = 0; i <= 0xd2; i++){
1377 if(i%16 == 0)
1378 count += mir3da_gsensor_drv.method->mysprintf(buf+count, "\n%02x\t", i);
1379 mir3da_register_read(handle, i, &val);
1380 count += mir3da_gsensor_drv.method->mysprintf(buf+count, "%02X ", val);
1381 }
1382
1383 count += mir3da_gsensor_drv.method->mysprintf(buf+count, "\n--------end---------\n");
1384 return count;
1385 }
1386
mir3da_get_reg_data(MIR_HANDLE handle,char * buf)1387 int mir3da_get_reg_data(MIR_HANDLE handle, char *buf)
1388 {
1389 return mir3da_gsensor_drv.obj[gsensor_mod].get_reg_data(handle, buf);
1390 }
1391
mir3da_set_odr(MIR_HANDLE handle,int delay)1392 int mir3da_set_odr(MIR_HANDLE handle, int delay)
1393 {
1394 int res = 0;
1395 int odr = 0;
1396
1397 if(delay <= 5)
1398 {
1399 odr = MIR3DA_ODR_200HZ;
1400 }
1401 else if(delay <= 10)
1402 {
1403 odr = MIR3DA_ODR_100HZ;
1404 }
1405 else
1406 {
1407 odr = MIR3DA_ODR_50HZ;
1408 }
1409
1410 res = mir3da_register_mask_write(handle, mir3da_gsensor_drv.obj[gsensor_mod].odr_sect[odr].addr,
1411 mir3da_gsensor_drv.obj[gsensor_mod].odr_sect[odr].mask,mir3da_gsensor_drv.obj[gsensor_mod].odr_sect[odr].value);
1412 if(res != 0) {
1413 return res;
1414 }
1415
1416 return res;
1417 }
1418
mir3da_soft_reset(MIR_HANDLE handle)1419 static int mir3da_soft_reset(MIR_HANDLE handle)
1420 {
1421 int res = 0;
1422 unsigned char reg_data;
1423
1424 reg_data = mir3da_gsensor_drv.obj[gsensor_mod].soft_reset.value;
1425 res = mir3da_register_mask_write(handle, mir3da_gsensor_drv.obj[gsensor_mod].soft_reset.addr, mir3da_gsensor_drv.obj[gsensor_mod].soft_reset.mask, reg_data);
1426 mir3da_gsensor_drv.method->msdelay(5);
1427
1428 return res;
1429 }
1430
mir3da_module_detect(PLAT_HANDLE handle)1431 static int mir3da_module_detect(PLAT_HANDLE handle)
1432 {
1433 int i, res = 0;
1434 unsigned char cid, mid;
1435 int is_find = -1;
1436
1437 /* Probe gsensor module */
1438 for(i=0;i<sizeof(mir3da_gsensor)/sizeof(mir3da_gsensor[0]);i++) {
1439 res = mir3da_register_read(handle, mir3da_gsensor[i].chip_id.addr, &cid);
1440 if(res != 0) {
1441 return res;
1442 }
1443
1444 cid &= mir3da_gsensor[i].chip_id.mask;
1445 if(mir3da_gsensor[i].chip_id.value == cid) {
1446 res = mir3da_register_read(handle, mir3da_gsensor[i].mod_id.addr, &mid);
1447 if(res != 0) {
1448 return res;
1449 }
1450
1451 mid &= mir3da_gsensor[i].mod_id.mask;
1452 if( mir3da_gsensor[i].mod_id.value == mid ){
1453 MI_MSG("Found Gsensor MIR3DA !");
1454 gsensor_mod = i;
1455 is_find =0;
1456 break;
1457 }
1458 }
1459 }
1460
1461 return is_find;
1462 }
1463
mir3da_parse_chip_info(PLAT_HANDLE handle)1464 static int mir3da_parse_chip_info(PLAT_HANDLE handle){
1465 unsigned char i=0,tmp=0;
1466 unsigned char reg_value = -1,reg_value1 = -1,reg_value2 = -1;
1467 char res=-1;
1468
1469 if(-1 == gsensor_mod)
1470 return res;
1471
1472 res = mir3da_register_read(handle, NSA_REG_CHIP_INFO, ®_value);
1473 if(res != 0) {
1474 return res;
1475 }
1476
1477 gsensor_chip_info.reg_value = reg_value;
1478
1479 if(0 == (reg_value>>6)){
1480 return -1;
1481 }
1482
1483 if(!(reg_value&0xc0)){
1484 gsensor_chip_info.asic = ASIC_2511;
1485 gsensor_chip_info.mems= MEMS_T9;
1486 gsensor_chip_info.package= PACKAGE_NONE;
1487
1488 for(i=0;i<sizeof(mir3da_chip_info_list)/sizeof(mir3da_chip_info_list[0]);i++){
1489 if(reg_value == mir3da_chip_info_list[i].reg_value){
1490 gsensor_chip_info.package = mir3da_chip_info_list[i].package;
1491 gsensor_chip_info.asic= mir3da_chip_info_list[i].asic;
1492 gsensor_chip_info.mems= mir3da_chip_info_list[i].mems;
1493 break;
1494 }
1495 }
1496 }
1497 else{
1498 gsensor_chip_info.asic = ASIC_2512B;
1499 gsensor_chip_info.mems= MEMS_T9;
1500 gsensor_chip_info.package= PACKAGE_NONE;
1501
1502 gsensor_chip_info.package = (package_type)((reg_value&0xc0)>>6);
1503
1504 if((reg_value&0x38)>>3 == 0x01)
1505 gsensor_chip_info.asic =ASIC_2512B;
1506 else if((reg_value&0x38)>>3 == 0x02)
1507 gsensor_chip_info.asic =ASIC_2513A;
1508 else if((reg_value&0x38)>>3 == 0x03)
1509 gsensor_chip_info.asic =ASIC_2516;
1510
1511 res = mir3da_register_read(handle, NSA_REG_CHIP_INFO_SECOND, ®_value1);
1512 if(res != 0) {
1513 return res;
1514 }
1515
1516 if(gsensor_chip_info.asic == ASIC_2512B){
1517 res = mir3da_register_read(handle, NSA_REG_MEMS_OPTION, ®_value);
1518 if(res != 0) {
1519 return res;
1520 }
1521 tmp= ((reg_value&0x01)<<2) |((reg_value1&0xc0)>>6);
1522 }
1523 else
1524 {
1525 tmp= (reg_value1&0xe0)>>5;
1526 }
1527
1528 res = mir3da_register_read(handle, NSA_REG_MEMS_OPTION, ®_value2);
1529 if(res != 0) {
1530 return res;
1531 }
1532
1533 if(tmp == 0x00){
1534 if(reg_value2&0x80)
1535 gsensor_chip_info.mems =MEMS_TV03;
1536 else
1537 gsensor_chip_info.mems =MEMS_T9;
1538 }else if(tmp == 0x01){
1539 gsensor_chip_info.mems =MEMS_RTO3;
1540 }
1541 else if(tmp == 0x03){
1542 gsensor_chip_info.mems =MEMS_GT2;
1543 if((gsensor_chip_info.reg_value!=0x5A)&&(gsensor_chip_info.asic==ASIC_2516))
1544 gsensor_chip_info.mems =MEMS_GT3;
1545 }
1546 else if(tmp == 0x04){
1547 gsensor_chip_info.mems =MEMS_GT3;
1548 }
1549
1550 #if YZ_CROSS_TALK_ENABLE
1551 if(reg_value1&0x10)
1552 yzcross = -(reg_value1&0x0f);
1553 else
1554 yzcross = (reg_value1&0x0f);
1555 #endif
1556 }
1557
1558 return 0;
1559 }
1560
1561
mir3da_install_general_ops(struct general_op_s * ops)1562 int mir3da_install_general_ops(struct general_op_s *ops)
1563 {
1564 if(0 == ops){
1565 return -1;
1566 }
1567
1568 mir3da_gsensor_drv.method = ops;
1569 return 0;
1570 }
1571
mir3da_core_init(PLAT_HANDLE handle)1572 MIR_HANDLE mir3da_core_init(PLAT_HANDLE handle)
1573 {
1574 int res = 0;
1575
1576 #if FILTER_AVERAGE_ENHANCE
1577 int i =0;
1578 #endif
1579
1580 mir3da_gsensor_drv.obj = mir3da_gsensor;
1581
1582 if(gsensor_mod < 0){
1583 res = mir3da_module_detect(handle);
1584 if(res) {
1585 MI_ERR("Can't find Mir3da gsensor!!");
1586 return 0;
1587 }
1588
1589 /* No miramems gsensor instance found */
1590 if(gsensor_mod < 0) {
1591 return 0;
1592 }
1593 }
1594
1595 MI_MSG("Probe gsensor module: %s", mir3da_gsensor[gsensor_mod].asic);
1596
1597 #if FILTER_AVERAGE_ENHANCE
1598 /* configure default filter param */
1599 for (i = 0; i < 3;i++){
1600 core_ctx.tFac[i].filter_param_l = 2;
1601 core_ctx.tFac[i].filter_param_h = 8;
1602 core_ctx.tFac[i].filter_threhold = 60;
1603
1604 core_ctx.tFac[i].refN_l = 0;
1605 core_ctx.tFac[i].refN_h = 0;
1606 }
1607 #endif
1608
1609 res = mir3da_chip_resume(handle);
1610 if(res) {
1611 MI_ERR("chip resume fail!!\n");
1612 return 0;
1613 }
1614
1615 return handle;
1616 }
1617
mir3da_chip_resume(MIR_HANDLE handle)1618 int mir3da_chip_resume(MIR_HANDLE handle)
1619 {
1620 int res = 0;
1621 unsigned char reg_data;
1622 unsigned char i = 0;
1623
1624 res = mir3da_soft_reset(handle);
1625 if(res) {
1626 MI_ERR("Do softreset failed !");
1627 return res;
1628 }
1629
1630 for(i=0;i<MIR3DA_INIT_SECT_LEN;i++) {
1631 if( mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].addr < 0 ) {
1632 break;
1633 }
1634
1635 reg_data = mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].value;
1636 res = mir3da_register_mask_write(handle, mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].addr, mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].mask, reg_data);
1637 if(res != 0) {
1638 return res;
1639 }
1640 }
1641
1642 mir3da_gsensor_drv.method->msdelay(10);
1643 if(gsensor_type<0){
1644 gsensor_type=mir3da_parse_chip_info(handle);
1645
1646 if(gsensor_type<0){
1647 MI_ERR("Can't parse Mir3da gsensor chipinfo!!");
1648 return -1;
1649 }
1650 }
1651
1652 if(gsensor_chip_info.asic==ASIC_2513A){
1653
1654 res = mir3da_register_read(handle, NSA_REG_CHIP_INFO, ®_data);
1655 if((reg_data == 0x55)||(reg_data == 0x50)){
1656
1657 mir3da_register_mask_write(handle, 0x40, 0xff, 0x96);
1658 mir3da_register_read(handle, 0x41, ®_data);
1659 if(reg_data != 0xBB){
1660 MI_ERR("error chip");
1661 return -1;
1662 }
1663
1664 mir3da_register_mask_write(handle, NSA_REG_POWERMODE_BW, 0x36, 0x30);
1665 mir3da_register_mask_write(handle, NSA_REG_INT_PIN_CONFIG, 0xff, 0x00);
1666
1667 mir3da_register_read(handle, NAS_REG_OSC_TRIM, ®_data);
1668 if(reg_data == 0x00)
1669 mir3da_register_mask_write(handle, NAS_REG_OSC_TRIM, 0xff, 0x50);
1670
1671 is_da217 = 1;
1672 }else{
1673 MI_ERR("parse asic error");
1674 return -1;
1675 }
1676 }
1677
1678 if((gsensor_chip_info.asic==ASIC_2512B)||(gsensor_chip_info.asic == ASIC_2513A)){
1679
1680 reg_data = mir3da_gsensor_drv.method->get_address(handle);
1681
1682
1683 if(reg_data ==0x26 ||reg_data ==0x4c){
1684
1685 mir3da_register_mask_write(handle,NSA_REG_SENS_COMP,0xc0,0x00);
1686 }
1687 }
1688
1689 #if MIR3DA_OFFSET_TEMP_SOLUTION
1690 res = mir3da_read_offset(handle, original_offset);
1691 if (res != 0){
1692 MI_ERR("Read offset failed !");
1693 return res;
1694 }
1695
1696 bLoad = FILE_CHECKING;
1697 readOffsetCnt = 0;
1698 readsubfileCnt =0;
1699 manual_load_cali_file(handle);
1700 #endif
1701
1702 #if 0
1703 if(is_da217 == 1){
1704 res = mir3da_irq_init(handle);
1705 if(res){
1706 MI_ERR("step count init fail!!\n");
1707 return 0;
1708 }
1709 }
1710 #endif
1711 return res;
1712 }
1713
mir3da_get_primary_offset(MIR_HANDLE handle,int * x,int * y,int * z)1714 int mir3da_get_primary_offset(MIR_HANDLE handle,int *x,int *y,int *z){
1715 int res = 0;
1716 unsigned char reg_data;
1717 unsigned char i = 0;
1718 unsigned char offset[9]={0};
1719
1720 res = mir3da_read_offset(handle, offset);
1721 if (res != 0){
1722 MI_ERR("Read offset failed !");
1723 return -1;
1724 }
1725
1726 res = mir3da_soft_reset(handle);
1727 if(res) {
1728 MI_ERR("Do softreset failed !");
1729 return -1;
1730 }
1731
1732 for(i=0;i<MIR3DA_INIT_SECT_LEN;i++) {
1733 if( mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].addr < 0 ) {
1734 break;
1735 }
1736
1737 reg_data = mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].value;
1738 res = mir3da_register_mask_write(handle, mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].addr, mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].mask, reg_data);
1739 if(res != 0) {
1740 MI_ERR("Write register[0x%x] error!",mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].addr);
1741 goto EXIT;
1742 }
1743 }
1744
1745 mir3da_gsensor_drv.method->msdelay(100);
1746
1747 res = cycle_read_xyz(handle, x, y, z, 20);
1748 if (res){
1749 MI_ERR("i2c block read failed\n");
1750 goto EXIT;
1751 }
1752
1753 mir3da_write_offset(handle, offset);
1754
1755 if((gsensor_chip_info.reg_value == 0x4B)
1756 ||(gsensor_chip_info.reg_value == 0x8C)
1757 ||(gsensor_chip_info.reg_value == 0xCA)
1758 ||(gsensor_chip_info.mems == MEMS_GT2))
1759 {
1760 *z = 0;
1761 }
1762
1763 return 0;
1764
1765 EXIT:
1766 mir3da_write_offset(handle, offset);
1767 return -1;
1768 }
1769
mir3da_irq_init(MIR_HANDLE handle)1770 int mir3da_irq_init(MIR_HANDLE handle){
1771 int res = 0;
1772 // irq config
1773 res |= mir3da_register_mask_write(handle, NSA_REG_INT_LATCH, 0x0F, 0x00); //latch 0s
1774
1775 // step config
1776 res |= mir3da_register_mask_write(handle, NSA_REG_STEP_CONFIG1, 0xff, 0x01);
1777 res |= mir3da_register_mask_write(handle, NSA_REG_STEP_CONFIG2, 0xff, 0x62);
1778 res |= mir3da_register_mask_write(handle, NSA_REG_STEP_CONFIG3, 0xff, 0x46);
1779 res |= mir3da_register_mask_write(handle, NSA_REG_STEP_CONFIG4, 0xff, 0x32);
1780 res |= mir3da_register_mask_write(handle, NSA_REG_STEP_FILTER, 0xff, 0x22); //enable bit
1781
1782 //step count
1783 res |= mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_MAPPING1, 0x02, 0x02);
1784 res |= mir3da_register_mask_write(handle, NAS_REG_INT_SET0, 0x01, 0x00); //irq bit
1785
1786 //significont motion
1787 res |= mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_MAPPING1, 0x80, 0x80);
1788 res |= mir3da_register_mask_write(handle, NAS_REG_INT_SET0, 0x02, 0x00); //irq bit
1789 res |= mir3da_register_mask_write(handle, NSA_REG_SM_THRESHOLD, 0x0A, 0x0A); //step number
1790
1791 //tilt
1792 res |= mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_MAPPING1, 0x08, 0x08);
1793 res |= mir3da_register_mask_write(handle, NAS_REG_INT_SET0, 0x10, 0x00); //irq bit
1794
1795 //active
1796 res |= mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_MAPPING1, 0x04, 0x04);
1797 res |= mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_SETTINGS1, 0xC7, 0x80);
1798 res |= mir3da_register_mask_write(handle, NSA_REG_ACTIVE_DURATION, 0xff, 0x01);
1799 res |= mir3da_register_mask_write(handle, NSA_REG_ACTIVE_THRESHOLD, 0xff, 0x14);
1800
1801 if(res)
1802 MI_ERR("irq init error");
1803
1804 MI_MSG("irq init ok")
1805 return res;
1806 }
1807
mir3da_step_count_init(MIR_HANDLE handle)1808 int mir3da_step_count_init(MIR_HANDLE handle){
1809 int res = 0;
1810
1811 // step config
1812 res |= mir3da_register_mask_write(handle, NSA_REG_STEP_CONFIG1, 0xff, 0x01);
1813 res |= mir3da_register_mask_write(handle, NSA_REG_STEP_CONFIG2, 0xff, 0x62);
1814 res |= mir3da_register_mask_write(handle, NSA_REG_STEP_CONFIG3, 0xff, 0x46);
1815 res |= mir3da_register_mask_write(handle, NSA_REG_STEP_CONFIG4, 0xff, 0x32);
1816 res |= mir3da_register_mask_write(handle, NSA_REG_STEP_FILTER, 0xff, 0x22);
1817
1818 return res;
1819 }
1820
mir3da_get_step_enable(MIR_HANDLE handle,char * enable)1821 int mir3da_get_step_enable(MIR_HANDLE handle, char *enable)
1822 {
1823 int res = 0;
1824 unsigned char reg_data = 0;
1825
1826 res = mir3da_register_read(handle, NSA_REG_STEP_FILTER, ®_data); //check irq
1827 if(res != 0) {
1828 return res;
1829 }
1830
1831 *enable = ( reg_data & 0x80 ) ? 1 : 0;
1832
1833 return res;
1834 }
1835
mir3da_set_step_enable(MIR_HANDLE handle,char enable)1836 int mir3da_set_step_enable(MIR_HANDLE handle, char enable)
1837 {
1838 int res = 0;
1839
1840 if(enable){
1841 res |= mir3da_register_mask_write(handle, NSA_REG_STEP_FILTER, 0x80, 0x80); //step count enable
1842 //res |= mir3da_register_mask_write(handle, NAS_REG_INT_SET0, 0x01, 0x01); //step irq bit
1843 }else{
1844 res |= mir3da_register_mask_write(handle, NSA_REG_STEP_FILTER, 0x80, 0x00);
1845 //res |= mir3da_register_mask_write(handle, NAS_REG_INT_SET0, 0x01, 0x00);
1846 }
1847
1848 return res;
1849 }
1850
mir3da_get_sm_enable(MIR_HANDLE handle,char * enable)1851 int mir3da_get_sm_enable(MIR_HANDLE handle, char *enable)
1852 {
1853 int res = 0;
1854 unsigned char reg_data = 0;
1855
1856 res = mir3da_register_read(handle, NSA_REG_STEP_FILTER, ®_data);
1857 if(res != 0) {
1858 return res;
1859 }
1860
1861 *enable = ( reg_data & 0x80 ) ? 1 : 0;
1862
1863 return res;
1864 }
1865
mir3da_set_sm_enable(MIR_HANDLE handle,char enable)1866 int mir3da_set_sm_enable(MIR_HANDLE handle, char enable)
1867 {
1868 int res = 0;
1869
1870 if(enable){
1871 res |= mir3da_register_mask_write(handle, NSA_REG_STEP_FILTER, 0x80, 0x80);
1872 res |= mir3da_register_mask_write(handle, NAS_REG_INT_SET0, 0x02, 0x02);
1873 }else{
1874 res |= mir3da_register_mask_write(handle, NSA_REG_STEP_FILTER, 0x80, 0x00);
1875 res |= mir3da_register_mask_write(handle, NAS_REG_INT_SET0, 0x02, 0x00);
1876 }
1877
1878 return res;
1879 }
1880
mir3da_get_tilt_enable(MIR_HANDLE handle,char * enable)1881 int mir3da_get_tilt_enable(MIR_HANDLE handle, char *enable)
1882 {
1883 int res = 0;
1884 unsigned char reg_data = 0;
1885
1886 res = mir3da_register_read(handle, NAS_REG_INT_SET0, ®_data);
1887 if(res != 0) {
1888 return res;
1889 }
1890
1891 *enable = ( reg_data & 0x10 ) ? 1 : 0;
1892
1893 return res;
1894 }
1895
mir3da_set_tilt_enable(MIR_HANDLE handle,char enable)1896 int mir3da_set_tilt_enable(MIR_HANDLE handle, char enable)
1897 {
1898 int res = 0;
1899
1900 if(enable){
1901 res |= mir3da_register_mask_write(handle, NAS_REG_INT_SET0, 0x10, 0x10);
1902 }else{
1903 res |= mir3da_register_mask_write(handle, NAS_REG_INT_SET0, 0x10, 0x00);
1904 }
1905
1906 return res;
1907 }
1908