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 SH200Q_DEBUG*/
16
17 #ifdef SH200Q_DEBUG
18 #define SH200Q_LOG LOG
19 #else
20 #define SH200Q_LOG
21 #endif
22
23 #define SH200Q_I2C_ADDR1 (0x6C)
24 #define SH200Q_I2C_ADDR2 (0x6D)
25 #define SH200Q_I2C_ADDR_TRANS(n) ((n)<<1)
26 #define SH200Q_I2C_ADDR SH200Q_I2C_ADDR_TRANS(SH200Q_I2C_ADDR2)
27 #define SH200Q_ACC_GYRO_WHO_AM_I_REG 0x30
28 #define SH200Q_CHIP_ID_VALUE (0x18)
29
30 #define SH200Q_ACC_CONFIG 0x0E
31 #define SH200Q_GYRO_CONFIG 0x0F
32 #define SH200Q_GYRO_DLPF 0x11
33 #define SH200Q_FIFO_CONFIG 0x12
34 #define SH200Q_ACC_RANGE 0x16
35 #define SH200Q_GYRO_RANGE 0x2B
36 #define SH200Q_OUTPUT_ACC_X 0x00
37 #define SH200Q_OUTPUT_GYRO_X 0x06
38 #define SH200Q_OUTPUT_TEMP 0x0C
39 #define SH200Q_REG_SET1 0xBA
40 #define SH200Q_REG_SET2 0xCA
41 #define SH200Q_ADC_RESET 0xC2
42 #define SH200Q_SOFT_RESET 0x7F
43 #define SH200Q_RESET 0x75
44
45 #define SH200Q_ACC_RANGE_2G (0x0)
46 #define SH200Q_ACC_RANGE_4G (0x0)
47 #define SH200Q_ACC_RANGE_8G (0x1)
48 #define SH200Q_ACC_RANGE_16G (0x2)
49 #define SH200Q_ACC_RANGE_MSK (0X3)
50 #define SH200Q_ACC_RANGE_POS (0)
51
52 #define SH200Q_ACC_SENSITIVITY_2G (61)
53 #define SH200Q_ACC_SENSITIVITY_4G (122)
54 #define SH200Q_ACC_SENSITIVITY_8G (244)
55 #define SH200Q_ACC_SENSITIVITY_16G (488)
56
57
58 #define SH200Q_ACC_ODR_16_HZ (0X03)
59 #define SH200Q_ACC_ODR_32_HZ (0x02)
60 #define SH200Q_ACC_ODR_64_HZ (0x01)
61 #define SH200Q_ACC_ODR_128_LP_HZ (0x00)
62 #define SH200Q_ACC_ODR_128_HZ (0x03)
63 #define SH200Q_ACC_ODR_256_HZ (0x02)
64 #define SH200Q_ACC_ODR_512_HZ (0x01)
65 #define SH200Q_ACC_ODR_1024_HZ (0x00)
66 #define SH200Q_ACC_ODR_MSK (0X18)
67 #define SH200Q_ACC_ODR_POS (3)
68
69
70 #define SH200Q_GYRO_ODR_31_HZ (0x03)
71 #define SH200Q_GYRO_ODR_250_HZ (0x02)
72 #define SH200Q_GYRO_ODR_500_HZ (0x01)
73 #define SH200Q_GYRO_ODR_1000_HZ (0x00)
74 #define SH200Q_GYRO_ODR_8K_HZ (0x04)
75 #define SH200Q_GYRO_ODR_16K_HZ (0x05)
76 #define SH200Q_GYRO_ODR_32K_HZ (0x06)
77
78 #define SH200Q_GYRO_ODR_MSK (0X0E)
79 #define SH200Q_GYRO_ODR_POS (1)
80
81
82 //#define SH200Q_GYRO_RANGE_125 (0x4)
83 #define SH200Q_GYRO_RANGE_245 (0x3)
84 #define SH200Q_GYRO_RANGE_500 (0x2)
85 #define SH200Q_GYRO_RANGE_1000 (0x1)
86 #define SH200Q_GYRO_RANGE_2000 (0x0)
87 #define SH200Q_GYRO_RANGE_MSK (0X7)
88 #define SH200Q_GYRO_RANGE_POS (0)
89
90
91 #define SH200Q_GYRO_SENSITIVITY_245DPS (7633)
92 #define SH200Q_GYRO_SENSITIVITY_500DPS (15267)
93 #define SH200Q_GYRO_SENSITIVITY_1000DPS (30487)
94 #define SH200Q_GYRO_SENSITIVITY_2000DPS (60975)
95
96
97 #define SH200Q_SHIFT_EIGHT_BITS (8)
98 #define SH200Q_16_BIT_SHIFT (0xFF)
99 #define SH200Q_ACC_MUL (1000)
100 #define SH200Q_GYRO_MUL (1)
101
102 #define SH200Q_ACC_DEFAULT_ODR_100HZ (100)
103 #define SH200Q_GYRO_DEFAULT_ODR_100HZ (100)
104
105 #define SH200Q_GET_BITSLICE(regvar, bitname)\
106 ((regvar & bitname##_MSK) >> bitname##_POS)
107
108 #define SH200Q_SET_BITSLICE(regvar, bitname, val)\
109 ((regvar & ~bitname##_MSK) | ((val<<bitname##_POS)&bitname##_MSK))
110
111 static int32_t sh200q_acc_factor[ACC_RANGE_MAX] = { SH200Q_ACC_SENSITIVITY_2G, SH200Q_ACC_SENSITIVITY_4G,
112 SH200Q_ACC_SENSITIVITY_8G, SH200Q_ACC_SENSITIVITY_16G };
113 static int32_t sh200q_gyro_factor[GYRO_RANGE_MAX] = {0, SH200Q_GYRO_SENSITIVITY_245DPS, SH200Q_GYRO_SENSITIVITY_500DPS,
114 SH200Q_GYRO_SENSITIVITY_1000DPS, SH200Q_GYRO_SENSITIVITY_2000DPS };
115 static int32_t cur_acc_factor = 0;
116 static int32_t cur_gyro_factor = 0;
117 static int32_t g_sh200qflag = 0;
118
119 static int32_t g_sleep = 0;
120 static int32_t g_powerdown = 0;
121 static int32_t g_low_odr = 0;
122
123
124
125
126
127 i2c_dev_t sh200q_ctx = {
128 .port = 4,
129 .config.address_width = 8,
130 .config.freq = 400000,
131 .config.dev_addr = SH200Q_I2C_ADDR,
132 };
133
drv_acc_senodia_sh200q_dump_reg(void)134 UNUSED static void drv_acc_senodia_sh200q_dump_reg(void)
135 {
136
137 uint8_t rw_buffer[32] = {0};
138 uint8_t reg_map[] = {
139 0xc2,
140 0x0E,
141 0x0F,
142 0x11,
143 0x12,
144 0x16,
145 0x2B,
146 0xba,
147 };
148
149 uint8_t i = 0;
150 uint16_t n = sizeof(reg_map)/sizeof(reg_map[0]);
151 int ret = 0;
152
153 for(i=0; i<n;i++) {
154 ret = sensor_i2c_read(&sh200q_ctx, reg_map[i], &rw_buffer[i],I2C_REG_LEN, I2C_OP_RETRIES);
155 if(unlikely(ret)){
156 return;
157 }
158
159 }
160
161 }
162
163
drv_acc_gyro_senodia_sh200q_hw_init(i2c_dev_t * drv)164 static int drv_acc_gyro_senodia_sh200q_hw_init(i2c_dev_t* drv)
165 {
166
167 uint8_t value = 0x00;
168 int ret = 0;
169
170
171 /*set acc odr 256hz*/
172 value = 0x91;
173 ret = sensor_i2c_write(drv, SH200Q_ACC_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
174 if(unlikely(ret)){
175 return -1;
176 }
177
178 /*set gyro odr 500hz*/
179 value = 0x13;
180 ret = sensor_i2c_write(drv, SH200Q_GYRO_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
181 if(unlikely(ret)){
182 return -1;
183 }
184
185
186 /*set gyro dlpf 50hz*/
187 value = 0x03;
188 ret = sensor_i2c_write(drv, SH200Q_GYRO_DLPF, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
189 if(unlikely(ret)){
190 return -1;
191 }
192
193
194 /*set no buffer mode*/
195 value = 0x00;
196 ret = sensor_i2c_write(drv, SH200Q_FIFO_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
197 if(unlikely(ret)){
198 return -1;
199 }
200
201
202 /*set acc range +-8G*/
203 value = 0x01;
204 ret = sensor_i2c_write(drv, SH200Q_ACC_RANGE, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
205 if(unlikely(ret)){
206 return -1;
207 }
208
209
210 /*set gyro range +-2000/s*/
211 value = 0x00;
212 ret = sensor_i2c_write(drv, SH200Q_GYRO_RANGE, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
213 if(unlikely(ret)){
214 return -1;
215 }
216
217
218 value = 0xC0;
219 ret = sensor_i2c_write(drv, SH200Q_REG_SET1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
220 if(unlikely(ret)){
221 return -1;
222 }
223
224 value = 0x10;
225 ret = sensor_i2c_write(drv, SH200Q_REG_SET2, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
226 if(unlikely(ret)){
227 return -1;
228 }
229
230 aos_msleep(1);
231
232
233 value = 0x00;
234 ret = sensor_i2c_write(drv, SH200Q_REG_SET2, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
235 if(unlikely(ret)){
236 return -1;
237 }
238
239
240 aos_msleep(10);
241
242
243 return 0;
244 }
245
246
247
248
drv_acc_gyro_senodia_sh200q_soft_reset(i2c_dev_t * drv)249 static int drv_acc_gyro_senodia_sh200q_soft_reset(i2c_dev_t* drv)
250 {
251 int ret = 0;
252 uint8_t value = 0x00;
253
254
255 /*soft reset*/
256 value = 0x00;
257 sensor_i2c_write(drv, SH200Q_SOFT_RESET, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
258
259 aos_msleep(1);
260
261 /*ADC Reset*/
262 value = 0x0E;
263 ret = sensor_i2c_write(drv, SH200Q_ADC_RESET, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
264 if(unlikely(ret)){
265 return -1;
266 }
267
268 aos_msleep(1);
269
270
271 value = 0x0A;
272 ret = sensor_i2c_write(drv, SH200Q_ADC_RESET, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
273 if(unlikely(ret)){
274 return -1;
275 }
276
277 return 0;
278 }
279
drv_acc_gyro_senodia_sh200q_validate_id(i2c_dev_t * drv,uint8_t id_value)280 static int drv_acc_gyro_senodia_sh200q_validate_id(i2c_dev_t* drv, uint8_t id_value)
281 {
282 uint8_t value = 0x00;
283 int ret = 0, i = 3;
284 (void)ret;
285 if(drv == NULL){
286 return -1;
287 }
288
289 do{
290 ret = sensor_i2c_read(drv, SH200Q_ACC_GYRO_WHO_AM_I_REG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
291 }while((value != 0x18) && (i-- > 0));
292
293 if (id_value != value){
294 return -1;
295 }
296
297 return 0;
298 }
299
drv_acc_senodia_sh200q_set_power_mode(i2c_dev_t * drv,dev_power_mode_e mode)300 static int drv_acc_senodia_sh200q_set_power_mode(i2c_dev_t* drv, dev_power_mode_e mode)
301 {
302 uint8_t value = 0x00;
303 int ret = 0;
304
305
306 ret = sensor_i2c_read(drv, SH200Q_ACC_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
307 if(unlikely(ret)){
308 return ret;
309 }
310
311 switch(mode){
312 case DEV_POWER_ON:{
313 /*1. bit2 ==>0; bit0 ==>1 normal mode && filter enable*/
314 value &= (~(1<<2));
315 value |= (1<<0);
316
317 /*2. set odr*/
318 value = SH200Q_SET_BITSLICE(value,SH200Q_ACC_ODR, SH200Q_ACC_ODR_128_HZ);
319 ret = sensor_i2c_write(drv, SH200Q_ACC_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
320 if(unlikely(ret)){
321 return ret;
322 }
323 g_sleep = 0;
324 g_powerdown = 0;
325 }break;
326
327 case DEV_POWER_OFF:{
328 /*1. filter disable*/
329 value &= (~(1<<0));
330 ret = sensor_i2c_write(drv, SH200Q_ACC_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
331 if(unlikely(ret)){
332 return ret;
333 }
334
335 /*2. low power mode*/
336 value |= (1<<2);
337 ret = sensor_i2c_write(drv, SH200Q_ACC_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
338 if(unlikely(ret)){
339 return ret;
340 }
341 g_powerdown = 1;
342 }break;
343
344 case DEV_SLEEP:{
345 /*1. filter disable */
346 value &= (~(1<<0));
347 ret = sensor_i2c_write(drv, SH200Q_ACC_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
348 if(unlikely(ret)){
349 return ret;
350 }
351
352 /*2. lower power mode*/
353 value |= (1<<2);
354 ret = sensor_i2c_write(drv, SH200Q_ACC_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
355 if(unlikely(ret)){
356 return ret;
357 }
358
359 /*3. bit2==>1; bit0 ==> 1 set odr && filter enable*/
360 value |= (1<<0);
361 value = SH200Q_SET_BITSLICE(value,SH200Q_ACC_ODR, SH200Q_ACC_ODR_16_HZ);
362 ret = sensor_i2c_write(drv, SH200Q_ACC_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
363 if(unlikely(ret)){
364 return ret;
365 }
366 g_sleep = 1;
367 }break;
368
369 default:break;
370 }
371
372
373
374
375 return 0;
376 }
377
drv_acc_senodia_sh200q_hz2odr(uint32_t hz)378 static uint8_t drv_acc_senodia_sh200q_hz2odr(uint32_t hz)
379 {
380 if(hz > 512)
381 return SH200Q_ACC_ODR_1024_HZ;
382 else if(hz > 256)
383 return SH200Q_ACC_ODR_512_HZ;
384 else if(hz > 128)
385 return SH200Q_ACC_ODR_256_HZ;
386 else if(hz > 64){
387 if(g_sleep == 0){
388 g_low_odr = 0;
389 return SH200Q_ACC_ODR_128_HZ;
390 }
391 else{
392 g_low_odr = 1;
393 return SH200Q_ACC_ODR_128_LP_HZ;
394 }
395 }
396 else if(hz > 32)
397 return SH200Q_ACC_ODR_64_HZ;
398 else if(hz > 16)
399 return SH200Q_ACC_ODR_32_HZ;
400 else if(hz > 8)
401 return SH200Q_ACC_ODR_16_HZ;
402 else
403 return SH200Q_ACC_ODR_16_HZ;
404
405
406 if(hz > 128)
407 g_low_odr = 0;
408 else if(hz < 64)
409 g_low_odr = 1;
410
411 }
412
413
drv_acc_senodia_sh200q_set_odr(i2c_dev_t * drv,uint32_t hz)414 static int drv_acc_senodia_sh200q_set_odr(i2c_dev_t* drv, uint32_t hz)
415 {
416 int ret = 0;
417 uint8_t value = 0x00;
418 uint8_t odr = drv_acc_senodia_sh200q_hz2odr(hz);
419
420
421 ret = sensor_i2c_read(drv, SH200Q_ACC_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
422 if(unlikely(ret)){
423 return ret;
424 }
425
426
427 if(1 == g_low_odr){
428 value &= (~(1<<0));
429 ret = sensor_i2c_write(drv, SH200Q_ACC_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
430 if(unlikely(ret)){
431 return ret;
432 }
433
434 value |= (1<<2);
435 ret = sensor_i2c_write(drv, SH200Q_ACC_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
436 if(unlikely(ret)){
437 return ret;
438 }
439
440 value |= (1<<0);
441 ret = sensor_i2c_write(drv, SH200Q_ACC_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
442 if(unlikely(ret)){
443 return ret;
444 }
445
446 }
447
448
449 value = SH200Q_SET_BITSLICE(value,SH200Q_ACC_ODR,odr);
450
451 ret = sensor_i2c_write(drv, SH200Q_ACC_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
452 if(unlikely(ret)){
453 return ret;
454 }
455
456
457 return 0;
458 }
459
drv_acc_senodia_sh200q_set_range(i2c_dev_t * drv,uint32_t range)460 static int drv_acc_senodia_sh200q_set_range(i2c_dev_t* drv, uint32_t range)
461 {
462 int ret = 0;
463 uint8_t value = 0x00;
464 uint8_t tmp = 0;
465
466 ret = sensor_i2c_read(drv, SH200Q_ACC_RANGE, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
467 if(unlikely(ret)){
468 return ret;
469 }
470
471 switch (range){
472
473 case ACC_RANGE_4G:{
474 tmp = SH200Q_ACC_RANGE_4G;
475 }break;
476
477 case ACC_RANGE_8G:{
478 tmp = SH200Q_ACC_RANGE_8G;
479 }break;
480
481 case ACC_RANGE_16G:{
482 tmp = SH200Q_ACC_RANGE_16G;
483 }break;
484
485 default:break;
486 }
487
488 value = SH200Q_SET_BITSLICE(value,SH200Q_ACC_RANGE,tmp);
489 ret = sensor_i2c_write(drv, SH200Q_ACC_RANGE, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
490 if(unlikely(ret)){
491 return ret;
492 }
493
494 if((range >= ACC_RANGE_2G)&&(range <= ACC_RANGE_16G)){
495 cur_acc_factor = sh200q_acc_factor[range];
496 }
497
498 return 0;
499 }
500
501
drv_acc_senodia_sh200q_irq_handle(void)502 static void drv_acc_senodia_sh200q_irq_handle(void)
503 {
504 /* no handle so far */
505 }
506
drv_acc_senodia_sh200q_open(void)507 static int drv_acc_senodia_sh200q_open(void)
508 {
509 int ret = 0;
510
511 drv_acc_gyro_senodia_sh200q_hw_init(&sh200q_ctx);
512
513 ret = drv_acc_senodia_sh200q_set_range(&sh200q_ctx, ACC_RANGE_8G);
514 if(unlikely(ret)){
515 return -1;
516 }
517
518 ret = drv_acc_senodia_sh200q_set_power_mode(&sh200q_ctx, DEV_POWER_ON);
519 if(unlikely(ret)){
520 return -1;
521 }
522
523
524 ret = drv_acc_senodia_sh200q_set_odr(&sh200q_ctx, SH200Q_ACC_DEFAULT_ODR_100HZ);
525 if(unlikely(ret)){
526 return -1;
527 }
528
529
530 return 0;
531
532 }
533
drv_acc_senodia_sh200q_close(void)534 static int drv_acc_senodia_sh200q_close(void)
535 {
536 int ret = 0;
537 ret = drv_acc_senodia_sh200q_set_power_mode(&sh200q_ctx, DEV_POWER_OFF);
538 if(unlikely(ret)){
539 return -1;
540 }
541 return 0;
542 }
543
544
545
546
drv_acc_senodia_sh200q_read(void * buf,size_t len)547 static int drv_acc_senodia_sh200q_read(void *buf, size_t len)
548 {
549 int ret = 0;
550 size_t size;
551 uint8_t reg[6];
552 accel_data_t *accel = (accel_data_t *)buf;
553 if(buf == NULL){
554 return -1;
555 }
556
557 size = sizeof(accel_data_t);
558 if(len < size){
559 return -1;
560 }
561
562 ret = sensor_i2c_read(&sh200q_ctx, SH200Q_OUTPUT_ACC_X, ®[0], I2C_REG_LEN, I2C_OP_RETRIES);
563 ret |= sensor_i2c_read(&sh200q_ctx, SH200Q_OUTPUT_ACC_X + 1, ®[1], I2C_REG_LEN, I2C_OP_RETRIES);
564 ret |= sensor_i2c_read(&sh200q_ctx, SH200Q_OUTPUT_ACC_X + 2, ®[2], I2C_REG_LEN, I2C_OP_RETRIES);
565 ret |= sensor_i2c_read(&sh200q_ctx, SH200Q_OUTPUT_ACC_X + 3, ®[3], I2C_REG_LEN, I2C_OP_RETRIES);
566 ret |= sensor_i2c_read(&sh200q_ctx, SH200Q_OUTPUT_ACC_X + 4, ®[4], I2C_REG_LEN, I2C_OP_RETRIES);
567 ret |= sensor_i2c_read(&sh200q_ctx, SH200Q_OUTPUT_ACC_X + 5, ®[5], I2C_REG_LEN, I2C_OP_RETRIES);
568 if(unlikely(ret)){
569 return -1;
570 }
571 accel->data[DATA_AXIS_X] = (int16_t)((((int16_t)((int8_t)reg[1]))<< SH200Q_SHIFT_EIGHT_BITS)|(reg[0]));
572 accel->data[DATA_AXIS_Y] = (int16_t)((((int16_t)((int8_t)reg[3]))<< SH200Q_SHIFT_EIGHT_BITS)|(reg[2]));
573 accel->data[DATA_AXIS_Z] = (int16_t)((((int16_t)((int8_t)reg[5]))<< SH200Q_SHIFT_EIGHT_BITS)|(reg[4]));
574
575 if(cur_acc_factor != 0){
576 accel->data[DATA_AXIS_X] = (accel->data[DATA_AXIS_X] * cur_acc_factor)/SH200Q_ACC_MUL;
577 accel->data[DATA_AXIS_Y] = (accel->data[DATA_AXIS_Y] * cur_acc_factor)/SH200Q_ACC_MUL;
578 accel->data[DATA_AXIS_Z] = (accel->data[DATA_AXIS_Z] * cur_acc_factor)/SH200Q_ACC_MUL;
579 }
580
581 /*SH200Q_LOG("acc x = %d, y = %d, z = %d , cur_acc_factor = %d\n", accel->data[DATA_AXIS_X], accel->data[DATA_AXIS_Y], accel->data[DATA_AXIS_Z], cur_acc_factor);*/
582 accel->timestamp = aos_now_ms();
583
584
585 return (int)size;
586 }
587
drv_acc_senodia_sh200q_ioctl(int cmd,unsigned long arg)588 static int drv_acc_senodia_sh200q_ioctl(int cmd, unsigned long arg)
589 {
590 int ret = 0;
591
592 switch(cmd){
593 case SENSOR_IOCTL_ODR_SET:{
594 ret = drv_acc_senodia_sh200q_set_odr(&sh200q_ctx, arg);
595 if(unlikely(ret)){
596 return -1;
597 }
598 }break;
599 case SENSOR_IOCTL_RANGE_SET:{
600 ret = drv_acc_senodia_sh200q_set_range(&sh200q_ctx, arg);
601 if(unlikely(ret)){
602 return -1;
603 }
604 }break;
605 case SENSOR_IOCTL_SET_POWER:{
606 ret = drv_acc_senodia_sh200q_set_power_mode(&sh200q_ctx, arg);
607 if(unlikely(ret)){
608 return -1;
609 }
610 }break;
611 case SENSOR_IOCTL_GET_INFO:{
612 /* fill the dev info here */
613 dev_sensor_info_t *info = (dev_sensor_info_t*)arg;
614 info->model = "SH200Q";
615 info->range_max = 16;
616 info->range_min = 4;
617 info->unit = mg;
618 }break;
619
620 default:break;
621 }
622
623 return 0;
624 }
625
626
drv_acc_senodia_sh200q_init(void)627 int drv_acc_senodia_sh200q_init(void){
628 int ret = 0;
629 sensor_obj_t sensor;
630 memset(&sensor, 0, sizeof(sensor));
631
632 /* fill the sensor obj parameters here */
633 sensor.io_port = I2C_PORT;
634 sensor.tag = TAG_DEV_ACC;
635 sensor.path = dev_acc_path;
636 sensor.open = drv_acc_senodia_sh200q_open;
637 sensor.close = drv_acc_senodia_sh200q_close;
638 sensor.read = drv_acc_senodia_sh200q_read;
639 sensor.write = NULL;
640 sensor.ioctl = drv_acc_senodia_sh200q_ioctl;
641 sensor.irq_handle = drv_acc_senodia_sh200q_irq_handle;
642
643 ret = sensor_create_obj(&sensor);
644 if(unlikely(ret)){
645 return -1;
646 }
647 ret = drv_acc_gyro_senodia_sh200q_validate_id(&sh200q_ctx, SH200Q_CHIP_ID_VALUE);
648 if(unlikely(ret)){
649 return -1;
650 }
651
652 if(0 == g_sh200qflag){
653 ret = drv_acc_gyro_senodia_sh200q_soft_reset(&sh200q_ctx);
654 if(unlikely(ret)){
655 return -1;
656 }
657
658 ret = drv_acc_senodia_sh200q_set_power_mode(&sh200q_ctx, DEV_POWER_OFF);
659 if(unlikely(ret)){
660 return -1;
661 }
662
663 #if 0
664 ret = drv_acc_gyro_senodia_sh200q_hw_init(&sh200q_ctx);
665 if(unlikely(ret)){
666 return -1;
667 }
668 #endif
669 g_sh200qflag = 1;
670 }
671 else{
672 }
673
674
675 return 0;
676 }
677
678
drv_gyro_senodia_sh200q_set_power_mode(i2c_dev_t * drv,dev_power_mode_e mode)679 static int drv_gyro_senodia_sh200q_set_power_mode(i2c_dev_t* drv, dev_power_mode_e mode)
680 {
681 uint8_t value = 0x00;
682 int ret = 0;
683
684 ret = sensor_i2c_read(drv, SH200Q_GYRO_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
685 if(unlikely(ret)){
686 return ret;
687 }
688
689 switch(mode){
690 case DEV_POWER_ON:{
691
692 value = SH200Q_SET_BITSLICE(value,SH200Q_GYRO_ODR,SH200Q_GYRO_ODR_31_HZ);
693 ret = sensor_i2c_write(drv, SH200Q_GYRO_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
694 if(unlikely(ret)){
695 return ret;
696 }
697 }break;
698
699 case DEV_POWER_OFF:{
700 value &= (~(1<<0));
701 ret = sensor_i2c_write(drv, SH200Q_GYRO_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
702 if(unlikely(ret)){
703 return ret;
704 }
705 }break;
706
707 case DEV_SLEEP:{
708 value = SH200Q_SET_BITSLICE(value,SH200Q_GYRO_ODR,SH200Q_GYRO_ODR_31_HZ);
709 ret = sensor_i2c_write(drv, SH200Q_GYRO_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
710 if(unlikely(ret)){
711 return ret;
712 }
713
714 }break;
715
716 default:break;
717 }
718 return 0;
719 }
720
drv_gyro_senodia_sh200q_hz2odr(uint32_t hz)721 static uint8_t drv_gyro_senodia_sh200q_hz2odr(uint32_t hz)
722 {
723
724 if(hz > 1660)
725 return SH200Q_GYRO_ODR_32K_HZ;
726 else if(hz > 833)
727 return SH200Q_GYRO_ODR_16K_HZ;
728 else if(hz > 416)
729 return SH200Q_GYRO_ODR_8K_HZ;
730 else if(hz > 208)
731 return SH200Q_GYRO_ODR_1000_HZ;
732 else if(hz > 104)
733 return SH200Q_GYRO_ODR_500_HZ;
734 else if(hz > 52)
735 return SH200Q_GYRO_ODR_250_HZ;
736 else if(hz > 26)
737 return SH200Q_GYRO_ODR_31_HZ;
738 else
739 return SH200Q_GYRO_ODR_31_HZ;
740
741 }
742
743
drv_gyro_senodia_sh200q_set_odr(i2c_dev_t * drv,uint32_t hz)744 static int drv_gyro_senodia_sh200q_set_odr(i2c_dev_t* drv, uint32_t hz)
745 {
746 int ret = 0;
747 uint8_t value = 0x00;
748 uint8_t odr = drv_gyro_senodia_sh200q_hz2odr(hz);
749
750 ret = sensor_i2c_read(drv, SH200Q_GYRO_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
751 if(unlikely(ret)){
752 return ret;
753 }
754
755 value = SH200Q_SET_BITSLICE(value,SH200Q_GYRO_ODR,odr);
756
757 ret = sensor_i2c_write(drv, SH200Q_GYRO_CONFIG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
758 if(unlikely(ret)){
759 return ret;
760 }
761 return 0;
762 }
763
drv_gyro_senodia_sh200q_set_range(i2c_dev_t * drv,uint32_t range)764 static int drv_gyro_senodia_sh200q_set_range(i2c_dev_t* drv, uint32_t range)
765 {
766 int ret = 0;
767 uint8_t value = 0x00;
768 uint8_t tmp = 0;
769
770 ret = sensor_i2c_read(drv, SH200Q_GYRO_RANGE, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
771 if(unlikely(ret)){
772 return ret;
773 }
774
775 switch (range){
776 case GYRO_RANGE_250DPS:{
777 tmp = SH200Q_GYRO_RANGE_245;
778 }break;
779
780 case GYRO_RANGE_500DPS:{
781 tmp = SH200Q_GYRO_RANGE_500;
782 }break;
783
784 case GYRO_RANGE_1000DPS:{
785 tmp = SH200Q_GYRO_RANGE_1000;
786 }break;
787
788 case GYRO_RANGE_2000DPS:{
789 tmp = SH200Q_GYRO_RANGE_2000;
790 }break;
791
792 default:break;
793 }
794
795 value = SH200Q_SET_BITSLICE(value,SH200Q_GYRO_RANGE,tmp);
796 ret = sensor_i2c_write(drv, SH200Q_GYRO_RANGE, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
797 if(unlikely(ret)){
798 return ret;
799 }
800
801 if((range >= GYRO_RANGE_250DPS)&&(range <= GYRO_RANGE_2000DPS)){
802 cur_gyro_factor = sh200q_gyro_factor[range];
803 }
804
805 return 0;
806 }
807
808
drv_gyro_senodia_sh200q_irq_handle(void)809 static void drv_gyro_senodia_sh200q_irq_handle(void)
810 {
811 /* no handle so far */
812 }
813
drv_gyro_senodia_sh200q_open(void)814 static int drv_gyro_senodia_sh200q_open(void)
815 {
816 int ret = 0;
817
818 drv_acc_gyro_senodia_sh200q_hw_init(&sh200q_ctx);
819
820
821 ret = drv_gyro_senodia_sh200q_set_power_mode(&sh200q_ctx, DEV_POWER_ON);
822 if(unlikely(ret)){
823 return -1;
824 }
825
826 ret = drv_gyro_senodia_sh200q_set_range(&sh200q_ctx, GYRO_RANGE_2000DPS);
827 if(unlikely(ret)){
828 return -1;
829 }
830
831 #ifdef FASTMODE_TEST
832 ret = drv_gyro_senodia_sh200q_set_odr(&sh200q_ctx, 200);
833 if(unlikely(ret)){
834 return -1;
835 }
836
837 #else
838 ret = drv_gyro_senodia_sh200q_set_odr(&sh200q_ctx, SH200Q_GYRO_DEFAULT_ODR_100HZ);
839 if(unlikely(ret)){
840 return -1;
841 }
842 #endif
843
844 return 0;
845
846 }
847
drv_gyro_senodia_sh200q_close(void)848 static int drv_gyro_senodia_sh200q_close(void)
849 {
850 int ret = 0;
851 ret = drv_gyro_senodia_sh200q_set_power_mode(&sh200q_ctx, DEV_POWER_OFF);
852 if(unlikely(ret)){
853 return -1;
854 }
855 return 0;
856 }
857
drv_gyro_senodia_sh200q_read(void * buf,size_t len)858 static int drv_gyro_senodia_sh200q_read(void *buf, size_t len)
859 {
860 int ret = 0;
861 size_t size;
862 uint8_t reg[6];
863 gyro_data_t *gyro = (gyro_data_t *)buf;
864 if(buf == NULL){
865 return -1;
866 }
867
868 size = sizeof(gyro_data_t);
869 if(len < size){
870 return -1;
871 }
872
873 ret = sensor_i2c_read(&sh200q_ctx, SH200Q_OUTPUT_GYRO_X, ®[0], I2C_REG_LEN, I2C_OP_RETRIES);
874 ret |= sensor_i2c_read(&sh200q_ctx, SH200Q_OUTPUT_GYRO_X + 1, ®[1], I2C_REG_LEN, I2C_OP_RETRIES);
875 ret |= sensor_i2c_read(&sh200q_ctx, SH200Q_OUTPUT_GYRO_X + 2, ®[2], I2C_REG_LEN, I2C_OP_RETRIES);
876 ret |= sensor_i2c_read(&sh200q_ctx, SH200Q_OUTPUT_GYRO_X + 3, ®[3], I2C_REG_LEN, I2C_OP_RETRIES);
877 ret |= sensor_i2c_read(&sh200q_ctx, SH200Q_OUTPUT_GYRO_X + 4, ®[4], I2C_REG_LEN, I2C_OP_RETRIES);
878 ret |= sensor_i2c_read(&sh200q_ctx, SH200Q_OUTPUT_GYRO_X + 5, ®[5], I2C_REG_LEN, I2C_OP_RETRIES);
879 if(unlikely(ret)){
880 return -1;
881 }
882 gyro->data[DATA_AXIS_X] = (int16_t)((((int32_t)((int8_t)reg[1]))<< SH200Q_SHIFT_EIGHT_BITS)|(reg[0]));
883 gyro->data[DATA_AXIS_Y] = (int16_t)((((int32_t)((int8_t)reg[3]))<< SH200Q_SHIFT_EIGHT_BITS)|(reg[2]));
884 gyro->data[DATA_AXIS_Z] = (int16_t)((((int32_t)((int8_t)reg[5]))<< SH200Q_SHIFT_EIGHT_BITS)|(reg[4]));
885
886 if(cur_gyro_factor != 0){
887 gyro->data[DATA_AXIS_X] = (gyro->data[DATA_AXIS_X] * cur_gyro_factor)/SH200Q_GYRO_MUL;
888 gyro->data[DATA_AXIS_Y] = (gyro->data[DATA_AXIS_Y] * cur_gyro_factor)/SH200Q_GYRO_MUL;
889 gyro->data[DATA_AXIS_Z] = (gyro->data[DATA_AXIS_Z] * cur_gyro_factor)/SH200Q_GYRO_MUL;
890 }
891 gyro->timestamp = aos_now_ms();
892
893 /*SH200Q_LOG("gyro x = %d, y = %d, z = %d , cur_gyro_factor = %d\n", gyro->data[DATA_AXIS_X], gyro->data[DATA_AXIS_Y], gyro->data[DATA_AXIS_Z], cur_gyro_factor);*/
894
895 return (int)size;
896 }
897
drv_acc_gyro_senodia_sh200q_self_test(i2c_dev_t * drv,int32_t * diff)898 UNUSED static int drv_acc_gyro_senodia_sh200q_self_test(i2c_dev_t* drv, int32_t* diff)
899 {
900
901 uint8_t gainRegister[32] = {147, 148, 149, 153, 154, 155, 159, 160, 161, 187, 198, 199, 200, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 219, 220, 221, 222, 223, 224};
902
903 uint8_t gainData[26] = {0, 4, 0, 0, 4, 0, 0, 4, 0, 0x10,0xC0, 0x40, 0x40, 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
904
905 uint8_t gainData1[3] = { 0xA0, 0x20, 0x20};
906
907 uint8_t i, reg, data;
908
909 int16_t c_gyro_self[3] = {0};
910
911
912 int sumGyroData[3] = {0};
913 int16_t avgGyroData[3] = {0};
914 int16_t GyroData[3] = {0};
915 int16_t avgGyroData1[3] = {0};
916
917 uint8_t self_gain_data[32] = {0};
918 uint8_t temp[6]= {0};
919
920 int ret = 0;
921
922
923 if(NULL == diff)
924 return -1;
925
926
927 for( i = 0 ; i < 32 ; i++ ){
928 reg = gainRegister[i];
929
930 ret = sensor_i2c_read(drv, reg, &(self_gain_data[i]), I2C_DATA_LEN, I2C_OP_RETRIES);
931 if(unlikely(ret)){
932 return -1;
933 }
934 aos_msleep(1);
935 }
936
937 c_gyro_self[0] = self_gain_data[26] + self_gain_data[27] * 256;
938 c_gyro_self[1] = self_gain_data[28] + self_gain_data[29] * 256;
939 c_gyro_self[2] = self_gain_data[30] + self_gain_data[31] * 256;
940
941 for( i = 0 ; i < 26 ; i++ ){
942 reg = gainRegister[i];
943 data = gainData[i];
944 ret = sensor_i2c_write(drv, reg, &data, I2C_DATA_LEN, I2C_OP_RETRIES);
945 if(unlikely(ret)){
946 return ret;
947 }
948 aos_msleep(1);
949 }
950
951
952 for(i=0;i<3;i++){
953 sumGyroData[i] = 0;
954 avgGyroData[i] = 0;
955 }
956
957 aos_msleep(500);
958
959
960 for(i=30; i>0; i--){
961
962 aos_msleep(1);
963
964 ret = sensor_i2c_read(drv, SH200Q_OUTPUT_GYRO_X, &temp[0], 6, I2C_OP_RETRIES);
965 if(unlikely(ret)){
966 return -1;
967 }
968
969 GyroData[0]=((int16_t)((temp[1]<<8)|temp[0]));//
970 GyroData[1]=((int16_t)((temp[3]<<8)|temp[2]));//
971 GyroData[2]=((int16_t)((temp[5]<<8)|temp[4]));//
972
973 sumGyroData[0] += GyroData[0];
974 sumGyroData[1] += GyroData[1];
975 sumGyroData[2] += GyroData[2];
976 }
977
978
979 for(i = 0; i < 3; i++){
980 avgGyroData[i] = sumGyroData[i] / 30;
981 }
982
983
984 for( i = 0 ; i < 3 ; i++ ){
985 reg = gainRegister[i + 10];
986 data = gainData1[i];
987 ret = sensor_i2c_write(drv, reg, &data, I2C_DATA_LEN, I2C_OP_RETRIES);
988 if(unlikely(ret)){
989 return ret;
990 }
991 aos_msleep(1);
992 }
993
994
995 aos_msleep(500);
996
997
998 for(i=0;i<3;i++){
999 sumGyroData[i] = 0;
1000 avgGyroData1[i] = 0;
1001 }
1002
1003
1004 for(i=30; i>0; i--){
1005
1006 aos_msleep(1);
1007
1008 ret = sensor_i2c_read(drv, SH200Q_OUTPUT_GYRO_X, &temp[0], 6, I2C_OP_RETRIES);
1009 if(unlikely(ret)){
1010 return -1;
1011 }
1012
1013 GyroData[0]=((int16_t)((temp[1]<<8)|temp[0]));//
1014 GyroData[1]=((int16_t)((temp[3]<<8)|temp[2]));//
1015 GyroData[2]=((int16_t)((temp[5]<<8)|temp[4]));//
1016
1017 sumGyroData[0] += GyroData[0];
1018 sumGyroData[1] += GyroData[1];
1019 sumGyroData[2] += GyroData[2];
1020 }
1021
1022
1023 for(i = 0; i < 3; i++){
1024 avgGyroData1[i] = sumGyroData[i] / 30;
1025 }
1026
1027
1028 for(i = 0; i < 3; i++){
1029 diff[i] = avgGyroData[i] - avgGyroData1[i];
1030 if((diff[i] > (c_gyro_self[i] + c_gyro_self[i] / 3)) ||(diff[i] < (c_gyro_self[i] - c_gyro_self[i] / 3))){
1031 ret = -1;
1032 break;
1033 }
1034 }
1035
1036
1037 return ret;
1038 }
1039
1040
drv_gyro_senodia_sh200q_ioctl(int cmd,unsigned long arg)1041 static int drv_gyro_senodia_sh200q_ioctl(int cmd, unsigned long arg)
1042 {
1043 int ret = 0;
1044
1045 switch(cmd){
1046 case SENSOR_IOCTL_ODR_SET:{
1047 ret = drv_gyro_senodia_sh200q_set_odr(&sh200q_ctx, arg);
1048 if(unlikely(ret)){
1049 return -1;
1050 }
1051 }break;
1052 case SENSOR_IOCTL_RANGE_SET:{
1053 ret = drv_gyro_senodia_sh200q_set_range(&sh200q_ctx, arg);
1054 if(unlikely(ret)){
1055 return -1;
1056 }
1057 }break;
1058 case SENSOR_IOCTL_SET_POWER:{
1059 ret = drv_gyro_senodia_sh200q_set_power_mode(&sh200q_ctx, arg);
1060 if(unlikely(ret)){
1061 return -1;
1062 }
1063 }break;
1064 case SENSOR_IOCTL_GET_INFO:{
1065 /* fill the dev info here */
1066 dev_sensor_info_t *info = (dev_sensor_info_t *)arg;
1067 info->model = "SH200Q";
1068 info->range_max = 2000;
1069 info->range_min = 125;
1070 info->unit = udps;
1071 }break;
1072 #if 0
1073 case SENSOR_IOCTL_SELF_TEST:{
1074 ret = drv_acc_gyro_senodia_sh200q_self_test(&sh200q_ctx, info->data);
1075 SH200Q_LOG("%d %d %d\n",info->data[0],info->data[1],info->data[2]);
1076 return ret;
1077
1078 }break;
1079 #endif
1080 default:break;
1081 }
1082
1083 return 0;
1084 }
1085
drv_gyro_senodia_sh200q_init(void)1086 int drv_gyro_senodia_sh200q_init(void){
1087 int ret = 0;
1088 sensor_obj_t sensor;
1089 memset(&sensor, 0, sizeof(sensor));
1090
1091 /* fill the sensor obj parameters here */
1092 sensor.io_port = I2C_PORT;
1093 sensor.tag = TAG_DEV_GYRO;
1094 sensor.path = dev_gyro_path;
1095 sensor.open = drv_gyro_senodia_sh200q_open;
1096 sensor.close = drv_gyro_senodia_sh200q_close;
1097 sensor.read = drv_gyro_senodia_sh200q_read;
1098 sensor.write = NULL;
1099 sensor.ioctl = drv_gyro_senodia_sh200q_ioctl;
1100 sensor.irq_handle = drv_gyro_senodia_sh200q_irq_handle;
1101
1102 ret = sensor_create_obj(&sensor);
1103 if(unlikely(ret)){
1104 return -1;
1105 }
1106
1107 ret = drv_acc_gyro_senodia_sh200q_validate_id(&sh200q_ctx, SH200Q_CHIP_ID_VALUE);
1108 if(unlikely(ret)){
1109 return -1;
1110 }
1111
1112 if(0 == g_sh200qflag){
1113
1114 ret = drv_acc_gyro_senodia_sh200q_soft_reset(&sh200q_ctx);
1115 if(unlikely(ret)){
1116 return -1;
1117 }
1118
1119 ret = drv_acc_gyro_senodia_sh200q_hw_init(&sh200q_ctx);
1120 if(unlikely(ret)){
1121 return -1;
1122 }
1123
1124 g_sh200qflag = 1;
1125 }
1126 else{
1127 }
1128
1129 /* update the phy sensor info to sensor hal */
1130 return 0;
1131 }
1132
1133
1134 SENSOR_DRV_ADD(drv_acc_senodia_sh200q_init);
1135 SENSOR_DRV_ADD(drv_gyro_senodia_sh200q_init);
1136
1137