Guten Morgen, ich habe zurzeit das Problem das der l3g4200d Gyro extrem
abdriftet bei den messergebnissen. Ich verwende ihn derzeit in meinem
Quoadcopter. Ziel ist es mit dem GYRO den Kopter stabiel zu halten,
jedoch macht es genau das gegenteil.
1 | void setupL3G4200D()
|
2 | {
|
3 | setRegister(0x20, 0b00001111);
|
4 | setRegister(0x21, 0b00000000);
|
5 | setRegister(0x22, 0b00001000);
|
6 | setRegister(0x23, 0b00110000);
|
7 | setRegister(0x24, 0b00000000);
|
8 |
|
9 | }
|
10 |
|
11 | double x_end,y_end, z_end;
|
12 | double fX, fY, fZ;
|
13 | void getgyro() //Messung + Ausgabe
|
14 | {
|
15 | twiStart();
|
16 | twiWriteByte(0xD2);
|
17 | twiWriteByte(0x29);
|
18 | //twiWriteByte(0x28);
|
19 | twiStop();
|
20 | twiStart();
|
21 | twiWriteByte(0xD3);
|
22 | x[0] = twiReadByteAck();
|
23 | x[1] = twiReadByteNoAck();
|
24 | x_end = ((x[1] << 8) | x[0]);
|
25 | fX = 0.0175*0.02 * x_end;
|
26 |
|
27 | twiStart();
|
28 | twiWriteByte(0xD2);
|
29 | twiWriteByte(0x2B);
|
30 | //twiWriteByte(0x2A);
|
31 | twiStop();
|
32 | twiStart();
|
33 | twiWriteByte(0xD3);
|
34 | y[0] = twiReadByteAck();
|
35 | y[1] = twiReadByteNoAck();
|
36 | y_end = ((y[1] << 8) | y[0]);
|
37 | fY = 0.0175*0.02 * y_end;
|
38 |
|
39 |
|
40 | twiStart();
|
41 | twiWriteByte(0xD2);
|
42 | twiWriteByte(0x2D);
|
43 | //twiWriteByte(0x2A);
|
44 | twiStop();
|
45 | twiStart();
|
46 | twiWriteByte(0xD3);
|
47 | z[0] = twiReadByteAck();
|
48 | z[1] = twiReadByteNoAck();
|
49 | z_end = ((z[1] << 8) | z[0]);
|
50 | fZ = 0.0175*0.02 * z_end;
|
51 |
|
52 | }
|
53 |
|
54 | int gyro_drift_x = 0;
|
55 | int gyro_drift_y = 0;
|
56 | int gyro_drift_z = 0;
|
57 |
|
58 | void g_drift(int anzahl)
|
59 | {
|
60 | int i = 0;
|
61 | for(i = 0; i<(anzahl+1); i++)
|
62 | {
|
63 | getgyro();
|
64 | gyro_drift_x += fX;
|
65 | gyro_drift_y += fY;
|
66 | gyro_drift_z += fZ;
|
67 |
|
68 | }
|
69 | gyro_drift_x = gyro_drift_x/anzahl;
|
70 | gyro_drift_y = gyro_drift_y/anzahl;
|
71 | gyro_drift_z = gyro_drift_z/anzahl;
|
72 | }
|
73 |
|
74 | float angle_x = 0, angle_y = 0, angle_z = 0;
|
75 |
|
76 | angle_x += fX-gyro_drift_x;
|
77 | angle_y += fY-gyro_drift_y;
|
78 | angle_z += fZ-gyro_drift_z;
|
Das sind die Programmteile wo der GYRO enthalten ist. Hat jemand von
euch vieleicht verbesserungsvorschläge?
Bei g_drift verwende ich als anzahl 250.
mfg Marc
