1 | uint8_t gyro_init(){
|
2 | uint16_t i;
|
3 | int16_t x,y,z;
|
4 |
|
5 | sendBuffer[2] = CTRL_REG4;
|
6 | sendBuffer[3] = 0xA0; //2000 dps
|
7 |
|
8 |
|
9 | TWI_MasterWriteRead(&twiMaster,L3G4200D_ADDR,&sendBuffer[2],2,0);
|
10 | while (twiMaster.status != TWIM_STATUS_READY) {
|
11 | /* Wait until transaction is complete. */
|
12 | }
|
13 |
|
14 | _delay_ms(10);
|
15 |
|
16 | sendBuffer[0] = CTRL_REG5;
|
17 | //sendBuffer[1] = 0x10;
|
18 | sendBuffer[1] = 0x02;
|
19 | TWI_MasterWriteRead(&twiMaster,L3G4200D_ADDR,&sendBuffer[0],2,0);
|
20 | while (twiMaster.status != TWIM_STATUS_READY) {
|
21 | /* Wait until transaction is complete. */
|
22 | }
|
23 |
|
24 | sendBuffer[0] = CTRL_REG2;
|
25 | sendBuffer[1] = 0x34;
|
26 | TWI_MasterWriteRead(&twiMaster,L3G4200D_ADDR,&sendBuffer[0],2,0);
|
27 | while (twiMaster.status != TWIM_STATUS_READY) {
|
28 | /* Wait until transaction is complete. */
|
29 | }
|
30 |
|
31 | sendBuffer[0] = CTRL_REG1;
|
32 | sendBuffer[1] = 0x8F; //enable
|
33 | TWI_MasterWriteRead(&twiMaster, L3G4200D_ADDR, &sendBuffer[0],2,0);
|
34 | while (twiMaster.status != TWIM_STATUS_READY) {
|
35 | /* Wait until transaction is complete. */
|
36 | }
|
37 |
|
38 | _delay_ms(20);
|
39 |
|
40 | gyro_calcoffset();
|
41 |
|
42 | return 0;
|
43 |
|
44 | }
|
45 |
|
46 | uint8_t gyro_calcoffset(){
|
47 | int16_t x,y,z,i;
|
48 |
|
49 | gyro_drift_x = 0;
|
50 | gyro_drift_y = 0;
|
51 | gyro_drift_z = 0;
|
52 | for(i = 0; i<200; i++){
|
53 | gyro_readRAW(&x, &y, &z);
|
54 | gyro_drift_x += x;
|
55 | gyro_drift_y += y;
|
56 | gyro_drift_z += z;
|
57 |
|
58 | }
|
59 | gyro_drift_x /= 200;
|
60 | gyro_drift_y /= 200;
|
61 | gyro_drift_z /= 200;
|
62 | }
|
63 |
|
64 | int8_t gyro_readRAW(int16_t *x,int16_t *y, int16_t *z){
|
65 |
|
66 | sendBuffer[0] = 0x80 | OUT_X_L;
|
67 | sendBuffer[1] = 0x80 | OUT_Y_L;
|
68 | sendBuffer[2] = 0x80 | OUT_Z_L;
|
69 | TWI_MasterWriteRead(&twiMaster, L3G4200D_ADDR, &sendBuffer[0],1,2);
|
70 | while (twiMaster.status != TWIM_STATUS_READY);
|
71 | *x = twiMaster.readData[1]<<8 | twiMaster.readData[0];
|
72 | TWI_MasterWriteRead(&twiMaster, L3G4200D_ADDR, &sendBuffer[1],1,2);
|
73 | while (twiMaster.status != TWIM_STATUS_READY);
|
74 | *y = twiMaster.readData[1]<<8 | twiMaster.readData[0];
|
75 | TWI_MasterWriteRead(&twiMaster, L3G4200D_ADDR, &sendBuffer[2],1,2);
|
76 | while (twiMaster.status != TWIM_STATUS_READY);
|
77 | *z = twiMaster.readData[1]<<8 | twiMaster.readData[0];
|
78 |
|
79 | }
|
80 |
|
81 | uint8_t gyro_readComp(int16_t *x,int16_t *y, int16_t *z){
|
82 |
|
83 | gyro_readRAW(x,y,z);
|
84 |
|
85 | *x -= gyro_drift_x;
|
86 | *y -= gyro_drift_y;
|
87 | *z -= gyro_drift_z;
|
88 |
|
89 | return 0;
|
90 | }
|
