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 A3G4250D_I2C_ADDR1 (0x68)
16 #define A3G4250D_I2C_ADDR2 (0x69)
17 #define A3G4250D_I2C_ADDR_TRANS(n) ((n)<<1)
18 #define A3G4250D_I2C_ADDR A3G4250D_I2C_ADDR_TRANS(A3G4250D_I2C_ADDR1)
19
20 #define A3G4250D_GYRO_WHO_AM_I_REG 0x0F
21 #define A3G4250D_GYRO_CTRL1 0x20
22 #define A3G4250D_GYRO_CTRL2 0x21
23 #define A3G4250D_GYRO_CTRL3 0x22
24 #define A3G4250D_GYRO_CTRL4 0x23
25 #define A3G4250D_GYRO_CTRL5 0x24
26 #define A3G4250D_GYRO_OUT_TEMP 0x26
27 #define A3G4250D_GYRO_STATUS 0x27
28 #define A3G4250D_GYRO_OUTX_L 0x28
29 #define A3G4250D_GYRO_OUTX_H 0x29
30 #define A3G4250D_GYRO_OUTY_L 0x2A
31 #define A3G4250D_GYRO_OUTY_H 0x2B
32 #define A3G4250D_GYRO_OUTZ_L 0x2C
33 #define A3G4250D_GYRO_OUTZ_H 0x2D
34
35 #define A3G4250D_CHIP_ID_VALUE (0xD3)
36
37 #define A3G4250D_GYRO_ODR_POWER_DOWN (0x00)
38 #define A3G4250D_GYRO_ODR_100_HZ (0x0F)
39 #define A3G4250D_GYRO_ODR_200_HZ (0x4F)
40 #define A3G4250D_GYRO_ODR_400_HZ (0x8F)
41 #define A3G4250D_GYRO_ODR_800_HZ (0xCF)
42
43 #define A3G4250D_GYRO_POWER_ON (0x0F)
44 #define A3G4250D_GYRO_SLEEP (0x08)
45 #define A3G4250D_GYRO_SLEEP_MASK (0x0F)
46
47 #define A3G4250D_GYRO_RANGE_250 (0x0)
48
49 #define A3G4250D_GYRO_SENSITIVITY_250DPS 8750
50
51 #define A3G4250D_GYRO_MUL 1000
52
53 #define A3G4250D_SHIFT_EIGHT_BITS (8)
54
55 #define A3G4250D_GYRO_DEFAULT_ODR_100HZ (100)
56
57 #define A3G4250D_GET_BITSLICE(regvar, bitname)\
58 ((regvar & bitname##_MSK) >> bitname##_POS)
59
60 #define A3G4250D_SET_BITSLICE(regvar, bitname, val)\
61 ((regvar & ~bitname##_MSK) | ((val<<bitname##_POS)&bitname##_MSK))
62
63 static int32_t a3g4250d_gyro_factor[GYRO_RANGE_MAX] = {0, A3G4250D_GYRO_SENSITIVITY_250DPS, 0, 0,
64 0 };
65
66 static int32_t cur_gyro_factor = 0;
67
68 i2c_dev_t a3g4250d_ctx = {
69 .port = 3,
70 .config.address_width = 8,
71 .config.freq = 400000,
72 .config.dev_addr = A3G4250D_I2C_ADDR,
73 };
74
drv_gyro_st_a3g4250d_soft_reset(i2c_dev_t * drv)75 UNUSED static int drv_gyro_st_a3g4250d_soft_reset(i2c_dev_t* drv)
76 {
77 return 0;
78 }
79
drv_gyro_st_a3g4250d_validate_id(i2c_dev_t * drv,uint8_t id_value)80 static int drv_gyro_st_a3g4250d_validate_id(i2c_dev_t* drv, uint8_t id_value)
81 {
82 uint8_t value = 0x00;
83 int ret = 0;
84
85 if(drv == NULL){
86 return -1;
87 }
88
89 ret = sensor_i2c_read(drv, A3G4250D_GYRO_WHO_AM_I_REG, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
90 if(unlikely(ret)){
91 return ret;
92 }
93
94 if (id_value != value){
95 return -1;
96 }
97
98 return 0;
99 }
100
drv_gyro_st_a3g4250d_set_bdu(i2c_dev_t * drv)101 static int drv_gyro_st_a3g4250d_set_bdu(i2c_dev_t* drv)
102 {
103 return 0;
104 }
105
drv_gyro_st_a3g4250d_set_power_mode(i2c_dev_t * drv,dev_power_mode_e mode)106 static int drv_gyro_st_a3g4250d_set_power_mode(i2c_dev_t* drv, dev_power_mode_e mode)
107 {
108 uint8_t value = 0x00;
109 int ret = 0;
110
111 ret = sensor_i2c_read(drv, A3G4250D_GYRO_CTRL1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
112 if(unlikely(ret)){
113 return ret;
114 }
115
116 switch(mode){
117 case DEV_POWER_ON:{
118
119 value |= A3G4250D_GYRO_POWER_ON;
120 ret = sensor_i2c_write(drv, A3G4250D_GYRO_CTRL1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
121 if(unlikely(ret)){
122 return ret;
123 }
124 }break;
125
126 case DEV_POWER_OFF:{
127 value &= ~A3G4250D_GYRO_POWER_ON;
128 ret = sensor_i2c_write(drv, A3G4250D_GYRO_CTRL1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
129 if(unlikely(ret)){
130 return ret;
131 }
132 }break;
133
134 case DEV_SLEEP:{
135 value &= ~A3G4250D_GYRO_SLEEP_MASK;
136 value |= A3G4250D_GYRO_SLEEP;
137 ret = sensor_i2c_write(drv, A3G4250D_GYRO_CTRL1, &value, I2C_DATA_LEN, I2C_OP_RETRIES);
138 if(unlikely(ret)){
139 return ret;
140 }
141
142 }break;
143
144 default:break;
145 }
146 return 0;
147 }
148
drv_gyro_st_a3g4250d_hz2odr(uint32_t hz)149 static uint8_t drv_gyro_st_a3g4250d_hz2odr(uint32_t hz)
150 {
151 if(hz > 400)
152 return A3G4250D_GYRO_ODR_800_HZ;
153 else if(hz > 200)
154 return A3G4250D_GYRO_ODR_400_HZ;
155 else if(hz > 100)
156 return A3G4250D_GYRO_ODR_200_HZ;
157 else
158 return A3G4250D_GYRO_ODR_100_HZ;
159
160 }
161
162
drv_gyro_st_a3g4250d_set_odr(i2c_dev_t * drv,uint32_t hz)163 static int drv_gyro_st_a3g4250d_set_odr(i2c_dev_t* drv, uint32_t hz)
164 {
165 int ret = 0;
166 uint8_t odr = drv_gyro_st_a3g4250d_hz2odr(hz);
167
168 ret = sensor_i2c_write(drv, A3G4250D_GYRO_CTRL1, &odr, I2C_DATA_LEN, I2C_OP_RETRIES);
169 if(unlikely(ret)){
170 return ret;
171 }
172 return 0;
173 }
174
drv_gyro_st_a3g4250d_set_range(i2c_dev_t * drv,uint32_t range)175 static int drv_gyro_st_a3g4250d_set_range(i2c_dev_t* drv, uint32_t range)
176 {
177
178 if(range == GYRO_RANGE_250DPS){
179 cur_gyro_factor = a3g4250d_gyro_factor[range];
180 }
181
182 return 0;
183 }
184
185
drv_gyro_st_a3g4250d_irq_handle(void)186 static void drv_gyro_st_a3g4250d_irq_handle(void)
187 {
188 /* no handle so far */
189 }
190
drv_gyro_st_a3g4250d_open(void)191 static int drv_gyro_st_a3g4250d_open(void)
192 {
193 int ret = 0;
194 ret = drv_gyro_st_a3g4250d_set_power_mode(&a3g4250d_ctx, DEV_POWER_ON);
195 if(unlikely(ret)){
196 return -1;
197 }
198
199 ret = drv_gyro_st_a3g4250d_set_bdu(&a3g4250d_ctx);
200 if(unlikely(ret)){
201 return -1;
202 }
203
204 ret = drv_gyro_st_a3g4250d_set_range(&a3g4250d_ctx, GYRO_RANGE_250DPS);
205 if(unlikely(ret)){
206 return -1;
207 }
208
209 ret = drv_gyro_st_a3g4250d_set_odr(&a3g4250d_ctx, A3G4250D_GYRO_DEFAULT_ODR_100HZ);
210 if(unlikely(ret)){
211 return -1;
212 }
213
214 return 0;
215
216 }
217
drv_gyro_st_a3g4250d_close(void)218 static int drv_gyro_st_a3g4250d_close(void)
219 {
220 int ret = 0;
221 ret = drv_gyro_st_a3g4250d_set_power_mode(&a3g4250d_ctx, DEV_POWER_OFF);
222 if(unlikely(ret)){
223 return -1;
224 }
225 return 0;
226 }
227
drv_gyro_st_a3g4250d_read(void * buf,size_t len)228 static int drv_gyro_st_a3g4250d_read(void *buf, size_t len)
229 {
230 int ret = 0;
231 size_t size;
232 uint8_t reg[6];
233 gyro_data_t *gyro = (gyro_data_t *)buf;
234 if(buf == NULL){
235 return -1;
236 }
237
238 size = sizeof(gyro_data_t);
239 if(len < size){
240 return -1;
241 }
242
243 ret = sensor_i2c_read(&a3g4250d_ctx, (A3G4250D_GYRO_OUTX_L | 0x80), reg, 6, I2C_OP_RETRIES);
244 if(unlikely(ret)){
245 return -1;
246 }
247 gyro->data[DATA_AXIS_X] = (int16_t)((((int32_t)((int8_t)reg[1]))<< A3G4250D_SHIFT_EIGHT_BITS)|(reg[0]));
248 gyro->data[DATA_AXIS_Y] = (int16_t)((((int32_t)((int8_t)reg[3]))<< A3G4250D_SHIFT_EIGHT_BITS)|(reg[2]));
249 gyro->data[DATA_AXIS_Z] = (int16_t)((((int32_t)((int8_t)reg[5]))<< A3G4250D_SHIFT_EIGHT_BITS)|(reg[4]));
250 // LOG("A3G4250D X: %x; Y: %x; Z: %x\n",gyro->data[DATA_AXIS_X], gyro->data[DATA_AXIS_Y], gyro->data[DATA_AXIS_Z]);
251 if(cur_gyro_factor != 0){
252 gyro->data[DATA_AXIS_X] = (gyro->data[DATA_AXIS_X] * cur_gyro_factor);
253 gyro->data[DATA_AXIS_Y] = (gyro->data[DATA_AXIS_Y] * cur_gyro_factor);
254 gyro->data[DATA_AXIS_Z] = (gyro->data[DATA_AXIS_Z] * cur_gyro_factor);
255 }
256 gyro->timestamp = aos_now_ms();
257
258 return (int)size;
259 }
260
drv_gyro_st_a3g4250d_ioctl(int cmd,unsigned long arg)261 static int drv_gyro_st_a3g4250d_ioctl(int cmd, unsigned long arg)
262 {
263 int ret = 0;
264
265 switch(cmd){
266 case SENSOR_IOCTL_ODR_SET:{
267 ret = drv_gyro_st_a3g4250d_set_odr(&a3g4250d_ctx, arg);
268 if(unlikely(ret)){
269 return -1;
270 }
271 }break;
272 case SENSOR_IOCTL_RANGE_SET:{
273 ret = drv_gyro_st_a3g4250d_set_range(&a3g4250d_ctx, arg);
274 if(unlikely(ret)){
275 return -1;
276 }
277 }break;
278 case SENSOR_IOCTL_SET_POWER:{
279 ret = drv_gyro_st_a3g4250d_set_power_mode(&a3g4250d_ctx, arg);
280 if(unlikely(ret)){
281 return -1;
282 }
283 }break;
284 case SENSOR_IOCTL_GET_INFO:{
285 /* fill the dev info here */
286 dev_sensor_info_t *info = (dev_sensor_info_t *)arg;
287 info->model = "A3G4250D";
288 info->range_max = 250;
289 info->range_min = 250;
290 info->unit = udps;
291 }break;
292
293 default:break;
294 }
295
296 return 0;
297 }
298
drv_gyro_st_a3g4250d_init(void)299 int drv_gyro_st_a3g4250d_init(void){
300 int ret = 0;
301 sensor_obj_t sensor;
302 memset(&sensor, 0, sizeof(sensor));
303
304 /* fill the sensor obj parameters here */
305 sensor.io_port = I2C_PORT;
306 sensor.tag = TAG_DEV_GYRO;
307 sensor.path = dev_gyro_path;
308 sensor.open = drv_gyro_st_a3g4250d_open;
309 sensor.close = drv_gyro_st_a3g4250d_close;
310 sensor.read = drv_gyro_st_a3g4250d_read;
311 sensor.write = NULL;
312 sensor.ioctl = drv_gyro_st_a3g4250d_ioctl;
313 sensor.irq_handle = drv_gyro_st_a3g4250d_irq_handle;
314
315 ret = sensor_create_obj(&sensor);
316 if(unlikely(ret)){
317 return -1;
318 }
319
320 ret = drv_gyro_st_a3g4250d_validate_id(&a3g4250d_ctx, A3G4250D_CHIP_ID_VALUE);
321 if(unlikely(ret)){
322 return -1;
323 }
324
325 ret = drv_gyro_st_a3g4250d_set_range(&a3g4250d_ctx, GYRO_RANGE_250DPS);
326 if(unlikely(ret)){
327 return -1;
328 }
329
330 ret = drv_gyro_st_a3g4250d_set_odr(&a3g4250d_ctx, A3G4250D_GYRO_ODR_100_HZ);
331 if(unlikely(ret)){
332 return -1;
333 }
334
335 ret = drv_gyro_st_a3g4250d_set_power_mode(&a3g4250d_ctx, DEV_POWER_OFF);
336 if(unlikely(ret)){
337 return -1;
338 }
339
340 /* update the phy sensor info to sensor hal */
341 LOG("%s %s successfully \n", SENSOR_STR, __func__);
342 return 0;
343 }
344
345
346 SENSOR_DRV_ADD(drv_gyro_st_a3g4250d_init);
347
348
349
350