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 L3GD20H_I2C_ADDR1 (0x6A)
16 #define L3GD20H_I2C_ADDR2 (0x6B)
17 #define L3GD20H_I2C_ADDR_TRANS(n) ((n)<<1)
18 #define L3GD20H_I2C_ADDR L3GD20H_I2C_ADDR_TRANS(L3GD20H_I2C_ADDR1)
19
20 #define L3GD20H_GYRO_WHO_AM_I_REG 0x0F
21 #define L3GD20H_GYRO_CTRL1 0x20
22 #define L3GD20H_GYRO_CTRL2 0x21
23 #define L3GD20H_GYRO_CTRL3 0x22
24 #define L3GD20H_GYRO_CTRL4 0x23
25 #define L3GD20H_GYRO_CTRL5 0x24
26 #define L3GD20H_GYRO_OUT_TEMP 0x26
27 #define L3GD20H_GYRO_STATUS 0x27
28 #define L3GD20H_GYRO_OUTX_L 0x28
29 #define L3GD20H_GYRO_OUTX_H 0x29
30 #define L3GD20H_GYRO_OUTY_L 0x2A
31 #define L3GD20H_GYRO_OUTY_H 0x2B
32 #define L3GD20H_GYRO_OUTZ_L 0x2C
33 #define L3GD20H_GYRO_OUTZ_H 0x2D
34 #define L3GD20H_GYRO_LOW_ODR 0x39
35
36 #define L3GD20H_CHIP_ID_VALUE (0xD7)
37
38 #define L3GD20H_ENABLE_SOFT_RESET_VALUE (0x04)
39
40 #define L3GD20H_BDU_ENABLE (0x80)
41
42 #define L3GD20H_GYRO_ODR_POWER_DOWN (0x00)
43 #define L3GD20H_GYRO_ODR_12_5_HZ (0x0F)
44 #define L3GD20H_GYRO_ODR_25_HZ (0x4F)
45 #define L3GD20H_GYRO_ODR_50_HZ (0x8F)
46 #define L3GD20H_GYRO_ODR_100_HZ (0x0F)
47 #define L3GD20H_GYRO_ODR_200_HZ (0x4F)
48 #define L3GD20H_GYRO_ODR_400_HZ (0x8F)
49 #define L3GD20H_GYRO_ODR_800_HZ (0xCF)
50
51 #define L3GD20H_LOW_ODR_ENABLE (0x01)
52
53 #define L3GD20H_GYRO_POWER_ON (0x08)
54 #define L3GD20H_GYRO_SLEEP (0x08)
55 #define L3GD20H_GYRO_SLEEP_MASK (0x0F)
56
57 #define L3GD20H_GYRO_RANGE_245 (0x0)
58 #define L3GD20H_GYRO_RANGE_500 (0x1)
59 #define L3GD20H_GYRO_RANGE_2000 (0x2)
60 #define L3GD20H_GYRO_RANGE_MSK (0x30)
61 #define L3GD20H_GYRO_RANGE_POS (4)
62
63 #define L3GD20H_GYRO_SENSITIVITY_245DPS 87500
64 #define L3GD20H_GYRO_SENSITIVITY_500DPS 175000
65 #define L3GD20H_GYRO_SENSITIVITY_2000DPS 700000
66
67 #define L3GD20H_GYRO_MUL 1000
68
69 #define L3GD20H_SHIFT_EIGHT_BITS (8)
70
71 #define L3GD20H_GYRO_DEFAULT_ODR_100HZ (100)
72
73 #define L3GD20H_GET_BITSLICE(regvar, bitname)\
74 ((regvar & bitname##_MSK) >> bitname##_POS)
75
76 #define L3GD20H_SET_BITSLICE(regvar, bitname, val)\
77 ((regvar & ~bitname##_MSK) | ((val<<bitname##_POS)&bitname##_MSK))
78
79 static int32_t l3gd20h_gyro_factor[GYRO_RANGE_MAX] = {0, L3GD20H_GYRO_SENSITIVITY_245DPS, L3GD20H_GYRO_SENSITIVITY_500DPS, 0,
80 L3GD20H_GYRO_SENSITIVITY_2000DPS };
81
82 static int32_t cur_gyro_factor = 0;
83
84 i2c_dev_t l3gd20h_ctx = {
85 .port = 3,
86 .config.address_width = 8,
87 .config.freq = 400000,
88 .config.dev_addr = L3GD20H_I2C_ADDR,
89 };
90
drv_gyro_st_l3gd20h_soft_reset(i2c_dev_t * drv)91 UNUSED static int drv_gyro_st_l3gd20h_soft_reset(i2c_dev_t* drv)
92 {
93 int ret = 0;
94 uint8_t value = 0x00;
95
96 ret = sensor_i2c_read(drv, L3GD20H_GYRO_LOW_ODR, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
97 if(unlikely(ret)){
98 return ret;
99 }
100
101 value |= L3GD20H_ENABLE_SOFT_RESET_VALUE;
102
103 ret = sensor_i2c_write(drv, L3GD20H_GYRO_LOW_ODR, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
104 if(unlikely(ret)){
105 return -1;
106 }
107 return 0;
108 }
109
drv_gyro_st_l3gd20h_validate_id(i2c_dev_t * drv,uint8_t id_value)110 static int drv_gyro_st_l3gd20h_validate_id(i2c_dev_t* drv, uint8_t id_value)
111 {
112 uint8_t value = 0x00;
113 int ret = 0;
114
115 if(drv == NULL){
116 return -1;
117 }
118
119 ret = sensor_i2c_read(drv, L3GD20H_GYRO_WHO_AM_I_REG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
120 if(unlikely(ret)){
121 return ret;
122 }
123
124 if (id_value != value){
125 return -1;
126 }
127
128 return 0;
129 }
130
drv_gyro_st_l3gd20h_set_bdu(i2c_dev_t * drv)131 static int drv_gyro_st_l3gd20h_set_bdu(i2c_dev_t* drv)
132 {
133 int ret = 0;
134 uint8_t value = 0x00;
135
136 ret = sensor_i2c_read(drv, L3GD20H_GYRO_CTRL4, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
137 if(unlikely(ret)){
138 return ret;
139 }
140
141 if (value & L3GD20H_BDU_ENABLE)
142 return 0;
143
144 value |= L3GD20H_BDU_ENABLE;
145
146 ret = sensor_i2c_write(drv, L3GD20H_GYRO_CTRL4, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
147 if(unlikely(ret)){
148 return -1;
149 }
150 return 0;
151 }
152
drv_gyro_st_l3gd20h_set_power_mode(i2c_dev_t * drv,dev_power_mode_e mode)153 static int drv_gyro_st_l3gd20h_set_power_mode(i2c_dev_t* drv, dev_power_mode_e mode)
154 {
155 uint8_t value = 0x00;
156 int ret = 0;
157
158 ret = sensor_i2c_read(drv, L3GD20H_GYRO_CTRL1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
159 if(unlikely(ret)){
160 return ret;
161 }
162
163 switch(mode){
164 case DEV_POWER_ON:{
165
166 value |= L3GD20H_GYRO_POWER_ON;
167 ret = sensor_i2c_write(drv, L3GD20H_GYRO_CTRL1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
168 if(unlikely(ret)){
169 return ret;
170 }
171 }break;
172
173 case DEV_POWER_OFF:{
174 value &= ~L3GD20H_GYRO_POWER_ON;
175 ret = sensor_i2c_write(drv, L3GD20H_GYRO_CTRL1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
176 if(unlikely(ret)){
177 return ret;
178 }
179 }break;
180
181 case DEV_SLEEP:{
182 value &= ~L3GD20H_GYRO_SLEEP_MASK;
183 value |= L3GD20H_GYRO_SLEEP;
184 ret = sensor_i2c_write(drv, L3GD20H_GYRO_CTRL1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
185 if(unlikely(ret)){
186 return ret;
187 }
188
189 }break;
190
191 default:break;
192 }
193 return 0;
194 }
195
drv_gyro_st_l3gd20h_hz2odr(uint32_t hz)196 static uint8_t drv_gyro_st_l3gd20h_hz2odr(uint32_t hz)
197 {
198 if(hz > 400)
199 return L3GD20H_GYRO_ODR_800_HZ;
200 else if(hz > 200)
201 return L3GD20H_GYRO_ODR_400_HZ;
202 else if(hz > 100)
203 return L3GD20H_GYRO_ODR_200_HZ;
204 else if(hz > 50)
205 return L3GD20H_GYRO_ODR_100_HZ;
206 else if(hz > 25)
207 return L3GD20H_GYRO_ODR_50_HZ;
208 else if(hz > 13)
209 return L3GD20H_GYRO_ODR_25_HZ;
210 else
211 return L3GD20H_GYRO_ODR_12_5_HZ;
212
213 }
214
215
drv_gyro_st_l3gd20h_set_odr(i2c_dev_t * drv,uint32_t hz)216 static int drv_gyro_st_l3gd20h_set_odr(i2c_dev_t* drv, uint32_t hz)
217 {
218 int ret = 0;
219 uint8_t value = 0x00;
220 uint8_t odr = drv_gyro_st_l3gd20h_hz2odr(hz);
221
222 if(hz < 100){
223 ret = sensor_i2c_read(drv, L3GD20H_GYRO_LOW_ODR, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
224 if(unlikely(ret)){
225 return ret;
226 }
227
228 value |= L3GD20H_LOW_ODR_ENABLE;
229
230 ret = sensor_i2c_write(drv, L3GD20H_GYRO_LOW_ODR, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
231 if(unlikely(ret)){
232 return ret;
233 }
234 }else{
235 ret = sensor_i2c_read(drv, L3GD20H_GYRO_LOW_ODR, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
236 if(unlikely(ret)){
237 return ret;
238 }
239
240 value &= ~L3GD20H_LOW_ODR_ENABLE;
241
242 ret = sensor_i2c_write(drv, L3GD20H_GYRO_LOW_ODR, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
243 if(unlikely(ret)){
244 return ret;
245 }
246 }
247
248 ret = sensor_i2c_write(drv, L3GD20H_GYRO_CTRL1, &odr, I2C_DATA_LEN, I2C_OP_RETRIES);
249 if(unlikely(ret)){
250 return ret;
251 }
252 return 0;
253 }
254
drv_gyro_st_l3gd20h_set_range(i2c_dev_t * drv,uint32_t range)255 static int drv_gyro_st_l3gd20h_set_range(i2c_dev_t* drv, uint32_t range)
256 {
257 int ret = 0;
258 uint8_t value = 0x00;
259 uint8_t tmp = 0;
260
261 ret = sensor_i2c_read(drv, L3GD20H_GYRO_CTRL4, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
262 if(unlikely(ret)){
263 return ret;
264 }
265
266 switch (range){
267 case GYRO_RANGE_250DPS:{
268 tmp = L3GD20H_GYRO_RANGE_245;
269 }break;
270
271 case GYRO_RANGE_500DPS:{
272 tmp = L3GD20H_GYRO_RANGE_500;
273 }break;
274 case GYRO_RANGE_2000DPS:{
275 tmp = L3GD20H_GYRO_RANGE_2000;
276 }break;
277
278 default:break;
279 }
280
281 value = L3GD20H_SET_BITSLICE(value,L3GD20H_GYRO_RANGE,tmp);
282 ret = sensor_i2c_write(drv, L3GD20H_GYRO_CTRL4, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
283 if(unlikely(ret)){
284 return ret;
285 }
286
287 if((range >= GYRO_RANGE_250DPS)&&(range <= GYRO_RANGE_2000DPS)){
288 cur_gyro_factor = l3gd20h_gyro_factor[range];
289 }
290
291 return 0;
292 }
293
294
drv_gyro_st_l3gd20h_irq_handle(void)295 static void drv_gyro_st_l3gd20h_irq_handle(void)
296 {
297 /* no handle so far */
298 }
299
drv_gyro_st_l3gd20h_open(void)300 static int drv_gyro_st_l3gd20h_open(void)
301 {
302 int ret = 0;
303 ret = drv_gyro_st_l3gd20h_set_power_mode(&l3gd20h_ctx, DEV_POWER_ON);
304 if(unlikely(ret)){
305 return -1;
306 }
307
308 ret = drv_gyro_st_l3gd20h_set_bdu(&l3gd20h_ctx);
309 if(unlikely(ret)){
310 return -1;
311 }
312
313 ret = drv_gyro_st_l3gd20h_set_range(&l3gd20h_ctx, GYRO_RANGE_2000DPS);
314 if(unlikely(ret)){
315 return -1;
316 }
317
318 ret = drv_gyro_st_l3gd20h_set_odr(&l3gd20h_ctx, L3GD20H_GYRO_DEFAULT_ODR_100HZ);
319 if(unlikely(ret)){
320 return -1;
321 }
322
323 return 0;
324
325 }
326
drv_gyro_st_l3gd20h_close(void)327 static int drv_gyro_st_l3gd20h_close(void)
328 {
329 int ret = 0;
330 ret = drv_gyro_st_l3gd20h_set_power_mode(&l3gd20h_ctx, DEV_POWER_OFF);
331 if(unlikely(ret)){
332 return -1;
333 }
334 return 0;
335 }
336
drv_gyro_st_l3gd20h_read(void * buf,size_t len)337 static int drv_gyro_st_l3gd20h_read(void *buf, size_t len)
338 {
339 int ret = 0;
340 size_t size;
341 uint8_t reg[6];
342
343 gyro_data_t *gyro = (gyro_data_t *)buf;
344 if(buf == NULL){
345 return -1;
346 }
347
348 size = sizeof(gyro_data_t);
349 if(len < size){
350 return -1;
351 }
352
353 ret = sensor_i2c_read(&l3gd20h_ctx, (L3GD20H_GYRO_OUTX_L | 0x80), reg, 6, I2C_OP_RETRIES);
354 if(unlikely(ret)){
355 return -1;
356 }
357 gyro->data[DATA_AXIS_X] = (int16_t)((((int32_t)((int8_t)reg[1]))<< L3GD20H_SHIFT_EIGHT_BITS)|(reg[0]));
358 gyro->data[DATA_AXIS_Y] = (int16_t)((((int32_t)((int8_t)reg[3]))<< L3GD20H_SHIFT_EIGHT_BITS)|(reg[2]));
359 gyro->data[DATA_AXIS_Z] = (int16_t)((((int32_t)((int8_t)reg[5]))<< L3GD20H_SHIFT_EIGHT_BITS)|(reg[4]));
360
361 if(cur_gyro_factor != 0){
362 gyro->data[DATA_AXIS_X] = (gyro->data[DATA_AXIS_X] * cur_gyro_factor);
363 gyro->data[DATA_AXIS_Y] = (gyro->data[DATA_AXIS_Y] * cur_gyro_factor);
364 gyro->data[DATA_AXIS_Z] = (gyro->data[DATA_AXIS_Z] * cur_gyro_factor);
365 }
366 gyro->timestamp = aos_now_ms();
367
368 return (int)size;
369 }
370
drv_gyro_st_l3gd20h_ioctl(int cmd,unsigned long arg)371 static int drv_gyro_st_l3gd20h_ioctl(int cmd, unsigned long arg)
372 {
373 int ret = 0;
374
375 switch(cmd){
376 case SENSOR_IOCTL_ODR_SET:{
377 ret = drv_gyro_st_l3gd20h_set_odr(&l3gd20h_ctx, arg);
378 if(unlikely(ret)){
379 return -1;
380 }
381 }break;
382 case SENSOR_IOCTL_RANGE_SET:{
383 ret = drv_gyro_st_l3gd20h_set_range(&l3gd20h_ctx, arg);
384 if(unlikely(ret)){
385 return -1;
386 }
387 }break;
388 case SENSOR_IOCTL_SET_POWER:{
389 ret = drv_gyro_st_l3gd20h_set_power_mode(&l3gd20h_ctx, arg);
390 if(unlikely(ret)){
391 return -1;
392 }
393 }break;
394 case SENSOR_IOCTL_GET_INFO:{
395 /* fill the dev info here */
396 dev_sensor_info_t *info = (dev_sensor_info_t *)arg;
397 info->model = "L3GD20H";
398 info->range_max = 2000;
399 info->range_min = 245;
400 info->unit = udps;
401 }break;
402
403 default:break;
404 }
405
406 return 0;
407 }
408
drv_gyro_st_l3gd20h_init(void)409 int drv_gyro_st_l3gd20h_init(void){
410 int ret = 0;
411 sensor_obj_t sensor;
412 memset(&sensor, 0, sizeof(sensor));
413
414 /* fill the sensor obj parameters here */
415 sensor.io_port = I2C_PORT;
416 sensor.tag = TAG_DEV_GYRO;
417 sensor.path = dev_gyro_path;
418 sensor.open = drv_gyro_st_l3gd20h_open;
419 sensor.close = drv_gyro_st_l3gd20h_close;
420 sensor.read = drv_gyro_st_l3gd20h_read;
421 sensor.write = NULL;
422 sensor.ioctl = drv_gyro_st_l3gd20h_ioctl;
423 sensor.irq_handle = drv_gyro_st_l3gd20h_irq_handle;
424
425 ret = sensor_create_obj(&sensor);
426 if(unlikely(ret)){
427 return -1;
428 }
429
430 ret = drv_gyro_st_l3gd20h_validate_id(&l3gd20h_ctx, L3GD20H_CHIP_ID_VALUE);
431 if(unlikely(ret)){
432 return -1;
433 }
434
435 ret = drv_gyro_st_l3gd20h_set_range(&l3gd20h_ctx, GYRO_RANGE_2000DPS);
436 if(unlikely(ret)){
437 return -1;
438 }
439
440 ret = drv_gyro_st_l3gd20h_set_odr(&l3gd20h_ctx, L3GD20H_GYRO_ODR_100_HZ);
441 if(unlikely(ret)){
442 return -1;
443 }
444
445 ret = drv_gyro_st_l3gd20h_set_power_mode(&l3gd20h_ctx, DEV_POWER_OFF);
446 if(unlikely(ret)){
447 return -1;
448 }
449
450 /* update the phy sensor info to sensor hal */
451 LOG("%s %s successfully \n", SENSOR_STR, __func__);
452 return 0;
453 }
454
455
456 SENSOR_DRV_ADD(drv_gyro_st_l3gd20h_init);
457
458