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 I3G4250D_I2C_ADDR1 (0x68)
16 #define I3G4250D_I2C_ADDR2 (0x69)
17 #define I3G4250D_I2C_ADDR_TRANS(n) ((n)<<1)
18 #define I3G4250D_I2C_ADDR I3G4250D_I2C_ADDR_TRANS(I3G4250D_I2C_ADDR1)
19
20 #define I3G4250D_GYRO_WHO_AM_I_REG 0x0F
21 #define I3G4250D_GYRO_CTRL1 0x20
22 #define I3G4250D_GYRO_CTRL2 0x21
23 #define I3G4250D_GYRO_CTRL3 0x22
24 #define I3G4250D_GYRO_CTRL4 0x23
25 #define I3G4250D_GYRO_CTRL5 0x24
26 #define I3G4250D_GYRO_OUT_TEMP 0x26
27 #define I3G4250D_GYRO_STATUS 0x27
28 #define I3G4250D_GYRO_OUTX_L 0x28
29 #define I3G4250D_GYRO_OUTX_H 0x29
30 #define I3G4250D_GYRO_OUTY_L 0x2A
31 #define I3G4250D_GYRO_OUTY_H 0x2B
32 #define I3G4250D_GYRO_OUTZ_L 0x2C
33 #define I3G4250D_GYRO_OUTZ_H 0x2D
34
35 #define I3G4250D_CHIP_ID_VALUE (0xD3)
36
37 #define I3G4250D_GYRO_ODR_POWER_DOWN (0x00)
38 #define I3G4250D_GYRO_ODR_100_HZ (0x0F)
39 #define I3G4250D_GYRO_ODR_200_HZ (0x4F)
40 #define I3G4250D_GYRO_ODR_400_HZ (0x8F)
41 #define I3G4250D_GYRO_ODR_800_HZ (0xCF)
42
43 #define I3G4250D_GYRO_POWER_ON (0x0F)
44 #define I3G4250D_GYRO_SLEEP (0x08)
45 #define I3G4250D_GYRO_SLEEP_MASK (0x0F)
46
47 #define I3G4250D_GYRO_RANGE_245 (0x0)
48 #define I3G4250D_GYRO_RANGE_500 (0x1)
49 #define I3G4250D_GYRO_RANGE_2000 (0x2)
50 #define I3G4250D_GYRO_RANGE_MSK (0x30)
51 #define I3G4250D_GYRO_RANGE_POS (4)
52
53 #define I3G4250D_GYRO_SENSITIVITY_245DPS 8750
54 #define I3G4250D_GYRO_SENSITIVITY_500DPS 17500
55 #define I3G4250D_GYRO_SENSITIVITY_2000DPS 70000
56
57 #define I3G4250D_GYRO_MUL 1000
58
59 #define I3G4250D_SHIFT_EIGHT_BITS (8)
60
61 #define I3G4250D_GYRO_DEFAULT_ODR_100HZ (100)
62
63 #define I3G4250D_GET_BITSLICE(regvar, bitname)\
64 ((regvar & bitname##_MSK) >> bitname##_POS)
65
66 #define I3G4250D_SET_BITSLICE(regvar, bitname, val)\
67 ((regvar & ~bitname##_MSK) | ((val<<bitname##_POS)&bitname##_MSK))
68
69 static int32_t i3g4250d_gyro_factor[GYRO_RANGE_MAX] = {0, I3G4250D_GYRO_SENSITIVITY_245DPS, I3G4250D_GYRO_SENSITIVITY_500DPS, 0,
70 I3G4250D_GYRO_SENSITIVITY_2000DPS };
71
72 static int32_t cur_gyro_factor = 0;
73
74 i2c_dev_t i3g4250d_ctx = {
75 .port = 3,
76 .config.address_width = 8,
77 .config.freq = 400000,
78 .config.dev_addr = I3G4250D_I2C_ADDR,
79 };
80
drv_gyro_st_i3g4250d_soft_reset(i2c_dev_t * drv)81 UNUSED static int drv_gyro_st_i3g4250d_soft_reset(i2c_dev_t* drv)
82 {
83 return 0;
84 }
85
drv_gyro_st_i3g4250d_validate_id(i2c_dev_t * drv,uint8_t id_value)86 static int drv_gyro_st_i3g4250d_validate_id(i2c_dev_t* drv, uint8_t id_value)
87 {
88 uint8_t value = 0x00;
89 int ret = 0;
90
91 if(drv == NULL){
92 return -1;
93 }
94
95 ret = sensor_i2c_read(drv, I3G4250D_GYRO_WHO_AM_I_REG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
96 if(unlikely(ret)){
97 return ret;
98 }
99
100 if (id_value != value){
101 return -1;
102 }
103
104 return 0;
105 }
106
drv_gyro_st_i3g4250d_set_bdu(i2c_dev_t * drv)107 static int drv_gyro_st_i3g4250d_set_bdu(i2c_dev_t* drv)
108 {
109 return 0;
110 }
111
drv_gyro_st_i3g4250d_set_power_mode(i2c_dev_t * drv,dev_power_mode_e mode)112 static int drv_gyro_st_i3g4250d_set_power_mode(i2c_dev_t* drv, dev_power_mode_e mode)
113 {
114 uint8_t value = 0x00;
115 int ret = 0;
116
117 ret = sensor_i2c_read(drv, I3G4250D_GYRO_CTRL1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
118 if(unlikely(ret)){
119 return ret;
120 }
121
122 switch(mode){
123 case DEV_POWER_ON:{
124
125 value |= I3G4250D_GYRO_POWER_ON;
126 ret = sensor_i2c_write(drv, I3G4250D_GYRO_CTRL1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
127 if(unlikely(ret)){
128 return ret;
129 }
130 }break;
131
132 case DEV_POWER_OFF:{
133 value &= ~I3G4250D_GYRO_POWER_ON;
134 ret = sensor_i2c_write(drv, I3G4250D_GYRO_CTRL1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
135 if(unlikely(ret)){
136 return ret;
137 }
138 }break;
139
140 case DEV_SLEEP:{
141 value &= ~I3G4250D_GYRO_SLEEP_MASK;
142 value |= I3G4250D_GYRO_SLEEP;
143 ret = sensor_i2c_write(drv, I3G4250D_GYRO_CTRL1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
144 if(unlikely(ret)){
145 return ret;
146 }
147
148 }break;
149
150 default:break;
151 }
152 return 0;
153 }
154
drv_gyro_st_i3g4250d_hz2odr(uint32_t hz)155 static uint8_t drv_gyro_st_i3g4250d_hz2odr(uint32_t hz)
156 {
157 if(hz > 400)
158 return I3G4250D_GYRO_ODR_800_HZ;
159 else if(hz > 200)
160 return I3G4250D_GYRO_ODR_400_HZ;
161 else if(hz > 100)
162 return I3G4250D_GYRO_ODR_200_HZ;
163 else
164 return I3G4250D_GYRO_ODR_100_HZ;
165
166 }
167
168
drv_gyro_st_i3g4250d_set_odr(i2c_dev_t * drv,uint32_t hz)169 static int drv_gyro_st_i3g4250d_set_odr(i2c_dev_t* drv, uint32_t hz)
170 {
171 int ret = 0;
172 uint8_t odr = drv_gyro_st_i3g4250d_hz2odr(hz);
173
174 ret = sensor_i2c_write(drv, I3G4250D_GYRO_CTRL1, &odr, I2C_DATA_LEN, I2C_OP_RETRIES);
175 if(unlikely(ret)){
176 return ret;
177 }
178 return 0;
179 }
180
drv_gyro_st_i3g4250d_set_range(i2c_dev_t * drv,uint32_t range)181 static int drv_gyro_st_i3g4250d_set_range(i2c_dev_t* drv, uint32_t range)
182 {
183 int ret = 0;
184 uint8_t value = 0x00;
185 uint8_t tmp = 0;
186
187 ret = sensor_i2c_read(drv, I3G4250D_GYRO_CTRL4, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
188 if(unlikely(ret)){
189 return ret;
190 }
191
192 switch (range){
193 case GYRO_RANGE_250DPS:{
194 tmp = I3G4250D_GYRO_RANGE_245;
195 }break;
196
197 case GYRO_RANGE_500DPS:{
198 tmp = I3G4250D_GYRO_RANGE_500;
199 }break;
200 case GYRO_RANGE_2000DPS:{
201 tmp = I3G4250D_GYRO_RANGE_2000;
202 }break;
203
204 default:break;
205 }
206
207 value = I3G4250D_SET_BITSLICE(value,I3G4250D_GYRO_RANGE,tmp);
208 ret = sensor_i2c_write(drv, I3G4250D_GYRO_CTRL4, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
209 if(unlikely(ret)){
210 return ret;
211 }
212
213 if((range >= GYRO_RANGE_250DPS)&&(range <= GYRO_RANGE_2000DPS)){
214 cur_gyro_factor = i3g4250d_gyro_factor[range];
215 }
216
217 return 0;
218 }
219
220
drv_gyro_st_i3g4250d_irq_handle(void)221 static void drv_gyro_st_i3g4250d_irq_handle(void)
222 {
223 /* no handle so far */
224 }
225
drv_gyro_st_i3g4250d_open(void)226 static int drv_gyro_st_i3g4250d_open(void)
227 {
228 int ret = 0;
229 ret = drv_gyro_st_i3g4250d_set_power_mode(&i3g4250d_ctx, DEV_POWER_ON);
230 if(unlikely(ret)){
231 return -1;
232 }
233
234 ret = drv_gyro_st_i3g4250d_set_bdu(&i3g4250d_ctx);
235 if(unlikely(ret)){
236 return -1;
237 }
238
239 ret = drv_gyro_st_i3g4250d_set_range(&i3g4250d_ctx, GYRO_RANGE_2000DPS);
240 if(unlikely(ret)){
241 return -1;
242 }
243
244 ret = drv_gyro_st_i3g4250d_set_odr(&i3g4250d_ctx, I3G4250D_GYRO_DEFAULT_ODR_100HZ);
245 if(unlikely(ret)){
246 return -1;
247 }
248
249 return 0;
250
251 }
252
drv_gyro_st_i3g4250d_close(void)253 static int drv_gyro_st_i3g4250d_close(void)
254 {
255 int ret = 0;
256 ret = drv_gyro_st_i3g4250d_set_power_mode(&i3g4250d_ctx, DEV_POWER_OFF);
257 if(unlikely(ret)){
258 return -1;
259 }
260 return 0;
261 }
262
drv_gyro_st_i3g4250d_read(void * buf,size_t len)263 static int drv_gyro_st_i3g4250d_read(void *buf, size_t len)
264 {
265 int ret = 0;
266 size_t size;
267 uint8_t reg[6];
268 gyro_data_t *gyro = (gyro_data_t *)buf;
269 if(buf == NULL){
270 return -1;
271 }
272
273 size = sizeof(gyro_data_t);
274 if(len < size){
275 return -1;
276 }
277
278 ret = sensor_i2c_read(&i3g4250d_ctx, (I3G4250D_GYRO_OUTX_L | 0x80), reg, 6, I2C_OP_RETRIES);
279 if(unlikely(ret)){
280 return -1;
281 }
282 gyro->data[DATA_AXIS_X] = (int16_t)((((int32_t)((int8_t)reg[1]))<< I3G4250D_SHIFT_EIGHT_BITS)|(reg[0]));
283 gyro->data[DATA_AXIS_Y] = (int16_t)((((int32_t)((int8_t)reg[3]))<< I3G4250D_SHIFT_EIGHT_BITS)|(reg[2]));
284 gyro->data[DATA_AXIS_Z] = (int16_t)((((int32_t)((int8_t)reg[5]))<< I3G4250D_SHIFT_EIGHT_BITS)|(reg[4]));
285
286 if(cur_gyro_factor != 0){
287 gyro->data[DATA_AXIS_X] = (gyro->data[DATA_AXIS_X] * cur_gyro_factor);
288 gyro->data[DATA_AXIS_Y] = (gyro->data[DATA_AXIS_Y] * cur_gyro_factor);
289 gyro->data[DATA_AXIS_Z] = (gyro->data[DATA_AXIS_Z] * cur_gyro_factor);
290 }
291 gyro->timestamp = aos_now_ms();
292
293 return (int)size;
294 }
295
drv_gyro_st_i3g4250d_ioctl(int cmd,unsigned long arg)296 static int drv_gyro_st_i3g4250d_ioctl(int cmd, unsigned long arg)
297 {
298 int ret = 0;
299
300 switch(cmd){
301 case SENSOR_IOCTL_ODR_SET:{
302 ret = drv_gyro_st_i3g4250d_set_odr(&i3g4250d_ctx, arg);
303 if(unlikely(ret)){
304 return -1;
305 }
306 }break;
307 case SENSOR_IOCTL_RANGE_SET:{
308 ret = drv_gyro_st_i3g4250d_set_range(&i3g4250d_ctx, arg);
309 if(unlikely(ret)){
310 return -1;
311 }
312 }break;
313 case SENSOR_IOCTL_SET_POWER:{
314 ret = drv_gyro_st_i3g4250d_set_power_mode(&i3g4250d_ctx, arg);
315 if(unlikely(ret)){
316 return -1;
317 }
318 }break;
319 case SENSOR_IOCTL_GET_INFO:{
320 /* fill the dev info here */
321 dev_sensor_info_t *info = (dev_sensor_info_t *)arg;
322 info->model = "I3G4250D";
323 info->range_max = 2000;
324 info->range_min = 245;
325 info->unit = udps;
326 }break;
327
328 default:break;
329 }
330
331 return 0;
332 }
333
drv_gyro_st_i3g4250d_init(void)334 int drv_gyro_st_i3g4250d_init(void){
335 int ret = 0;
336 sensor_obj_t sensor;
337 memset(&sensor, 0, sizeof(sensor));
338
339 /* fill the sensor obj parameters here */
340 sensor.io_port = I2C_PORT;
341 sensor.tag = TAG_DEV_GYRO;
342 sensor.path = dev_gyro_path;
343 sensor.open = drv_gyro_st_i3g4250d_open;
344 sensor.close = drv_gyro_st_i3g4250d_close;
345 sensor.read = drv_gyro_st_i3g4250d_read;
346 sensor.write = NULL;
347 sensor.ioctl = drv_gyro_st_i3g4250d_ioctl;
348 sensor.irq_handle = drv_gyro_st_i3g4250d_irq_handle;
349
350 ret = sensor_create_obj(&sensor);
351 if(unlikely(ret)){
352 return -1;
353 }
354
355 ret = drv_gyro_st_i3g4250d_validate_id(&i3g4250d_ctx, I3G4250D_CHIP_ID_VALUE);
356 if(unlikely(ret)){
357 return -1;
358 }
359
360 ret = drv_gyro_st_i3g4250d_set_range(&i3g4250d_ctx, GYRO_RANGE_2000DPS);
361 if(unlikely(ret)){
362 return -1;
363 }
364
365 ret = drv_gyro_st_i3g4250d_set_odr(&i3g4250d_ctx, I3G4250D_GYRO_ODR_100_HZ);
366 if(unlikely(ret)){
367 return -1;
368 }
369
370 ret = drv_gyro_st_i3g4250d_set_power_mode(&i3g4250d_ctx, DEV_POWER_OFF);
371 if(unlikely(ret)){
372 return -1;
373 }
374
375 /* update the phy sensor info to sensor hal */
376 LOG("%s %s successfully \n", SENSOR_STR, __func__);
377 return 0;
378 }
379
380
381 SENSOR_DRV_ADD(drv_gyro_st_i3g4250d_init);
382
383
384