根据malc《【翻译】加速度计和陀螺仪指南 》写的mpu6050程序·

cyq456

看了这贴http://www.geek-workshop.com/thread-1695-1-1.html都有几天了，有所领悟，所以根据http://starlino.com/imu_kalman_arduino.html这贴写了一个mpu6050可以用的程序。
————————————————分割线—————————————————————————————————
其中RwEst[0] 为处理后x轴的加速度， RwEst[1] 为处理后y轴的加速度 ， RwEst[z] 为处理后z轴的加速度。
处理后的加速度可以直接用于求角度。
想求角度可以参考这贴http://www.geek-workshop.com/for ... 328&page=1#pid27876[/url]  
代码：

1. #include "Wire.h"
   
2. #include "I2Cdev.h"
   
3. #include "MPU6050.h"
   
4. MPU6050 accelgyro;
   
5. #define PI 3.14159265358979f
   
6. #define INPUT_COUNT 5
   
7. #define Gz_offset 0.30
   
8. #define Gy_offset 1.09
   
9. #define Gx_offset -1.94
   
10. int16_t ax, ay, az;
    
11. int16_t gx, gy, gz;
    
12. int an[INPUT_COUNT];
    
13. char firstSample;
    
14. float wGyro;
    
15. float RwEst[3];
    
16. unsigned long lastMicros;
    
17. unsigned long interval;
    
18. float RwAcc[3];
    
19. float RwGyro[3];
    
20. float Awz[2];
    
21. bool blinkState = false;
    
22. void setup()
    
23. {
    
24.   Serial.begin(38400);
    
25.   Wire.begin();
    
26.   accelgyro.initialize();
    
27.   wGyro=10;
    
28.   firstSample=1;
    
29. }
    
30. void loop()
    
31. {
    
32.   getEstimatedInclination();
    
33.   Serial.print(interval);
    
34.   Serial.print(",");   
    
35.   Serial.print(RwAcc[0]);  //Inclination X axis (as measured by accelerometer)
    
36.   Serial.print(",");
    
37.   Serial.print(RwEst[0]);  //Inclination X axis (estimated / filtered)
    
38.   Serial.print(",");   
    
39.   Serial.print(RwAcc[1]);  //Inclination Y axis (as measured by accelerometer)
    
40.   Serial.print(",");
    
41.   Serial.print(RwEst[1]);  //Inclination Y axis (estimated / filtered)
    
42.   Serial.print(",");   
    
43.   Serial.print(RwAcc[2]);  //Inclination Z axis (as measured by accelerometer)
    
44.   Serial.print(",");
    
45.   Serial.print(RwEst[2]);  //Inclination Z axis (estimated / filtered)  
    
46.   Serial.println("");
    
47.   blinkState = !blinkState;
    
48. }
    
49. 
    
50. void getEstimatedInclination()
    
51. {
    
52.   static int i,w;
    
53.   static float tmpf,tmpf2;
    
54.   static unsigned long newMicros;
    
55.   static char signRzGyro;
    
56.   newMicros=millis();
    
57.   Get_an();
    
58.   interval=newMicros-lastMicros;   
    
59.   lastMicros = newMicros;
    
60.   for(w=0;w<=2;w++) RwAcc[w]=getInput(w);
    
61.   normalize3DVector(RwAcc);
    
62.   if (firstSample)
    
63.   {
    
64.     for(w=0;w<=2;w++) RwEst[w]=RwAcc[w];    //initialize with accelerometer readings
    
65.   }
    
66.   else
    
67.   {
    
68.     if(abs(RwEst[2])<0.1)
    
69.     {
    
70.       for(w=0;w<=2;w++) RwGyro[w]=RwEst[w];
    
71.     }
    
72.     else
    
73.     {
    
74.       for(w=0;w<=1;w++)
    
75.       {
    
76.         tmpf=getInput(3+w);
    
77.         tmpf *= interval / 1,000.0f;
    
78.         Awz[w] = atan2(RwEst[w],RwEst[2]) * 180 /PI;
    
79.         Awz[w] += tmpf;
    
80.       }
    
81.       signRzGyro=(cos(Awz[0]*PI/180)>=0)?1:-1;
    
82.       for(w=0;w<=1;w++)
    
83.       {
    
84.         RwGyro[w] = sin(Awz[w]*PI/180);
    
85.         RwGyro[w]/=sqrt(1+squared(cos(Awz[w]*PI/180))*squared(tan(Awz[1-w]*PI/180)) );
    
86.       }
    
87.        RwGyro[2]=signRzGyro*sqrt(1-squared(RwGyro[0])-squared(RwGyro[1]));
    
88.     }
    
89.     for(w=0;w<=2;w++) RwEst[w] = (RwAcc[w] + wGyro* RwGyro[w])/(1 + wGyro);
    
90.     normalize3DVector(RwEst);
    
91.   }
    
92.   firstSample = 0;
    
93. }
    
94. 
    
95. 
    
96. float squared(float x)
    
97. {
    
98.   return x*x;
    
99. }
    
100. 
     
101. 
     
102. float getInput(char i)
     
103. {
     
104.   static float tmpf;
     
105.   if(i<=2) tmpf=an[i]/16384.0;
     
106.   else if(i==3) tmpf=an[i]/131.0-Gx_offset;
     
107.   else if(i==4) tmpf=an[i]/131.0-Gy_offset;
     
108.   else tmpf=an[i]/131.0-Gz_offset;
     
109.   return tmpf;
     
110. }
     
111. 
     
112. 
     
113. void normalize3DVector(float* vector)
     
114. {
     
115.   static float R;  
     
116.   R = sqrt(vector[0]*vector[0]+vector[1]*vector[1]+vector[2]*vector[2]);
     
117.   vector[0] /= R;
     
118.   vector[1] /= R;  
     
119.   vector[2] /= R;  
     
120. }
     
121. 
     
122. 
     
123. void Get_an()
     
124. {
     
125.   accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
     
126.   an[0]=ax;
     
127.   an[1]=ay;
     
128.   an[2]=az;
     
129.   an[3]=gx;
     
130.   an[4]=gy;
     
131.   an[5]=gz;
     
132. }

复制代码

学慧放弃

调试效果怎样？？？

cyq456

学慧放弃 发表于 2013-8-25 09:59
调试效果怎样？？？

比没处理的数据稳定多了

学慧放弃

cyq456 发表于 2013-8-25 23:12
比没处理的数据稳定多了

滤波效果怎样？

学慧放弃

cyq456 发表于 2013-8-25 23:12
比没处理的数据稳定多了

滤波效果怎样？

菜鸟求指教

请问这个可以用在51单片机上吗？

rn9409

想問#define Gz_offset 0.30
#define Gy_offset 1.09
#define Gx_offset -1.94
GX GY GZ這三個值是如何得出來的