用Arduino nuo+MPU6050做头追

吉克功放

最近和朋友在玩航模，想玩FPV。就想着搞一个“头追”。上X宝一搜，一般的都要一百多。再上上各种diy论坛发现有各种大神都已经diy出来了。本着自己动手丰衣足食的精神。我也买来板子准备自己做一个。首先说明，如果你不想深入地了解整个原理，可以看其他帖子，按其他帖子来是可以做出来的。这个帖子我希望有更多的大神来给我指导一下，因为我想借做头追，了解一下mpu6050以及卡尔曼滤波。后面呢我还想自己做一个简单的飞控。废话少说。先说一下材料：

1：Arduino Uno R3板一块；

2：MPU6050一块

硬件准备好，接下来就要软件了。使用arduino，当然用arduinoIDE了，找度娘很简单就找到了。还有一个看串口波形的软件，我用的Serialchart 挺简单的（我是新手，也只用过那个。。。）
接下来说程序

1. /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
   
2. 
   
3. This software may be distributed and modified under the terms of the GNU
   
4. General Public License version 2 (GPL2) as published by the Free Software
   
5. Foundation and appearing in the file GPL2.TXT included in the packaging of
   
6. this file. Please note that GPL2 Section 2[b] requires that all works based
   
7. on this software must also be made publicly available under the terms of
   
8. the GPL2 ("Copyleft").
   
9. 
   
10. Contact information
    
11. -------------------
    
12. 
    
13. Kristian Lauszus, TKJ Electronics
    
14. Web : http://www.tkjelectronics.com
    
15. e-mail : [email][email protected][/email]
    
16. */
    
17. 
    
18. #include <Wire.h> //mpu6050和arduino通过I2C通信，所以要包含这个库
    
19. #include <Kalman.h> //卡尔曼滤波函数
    
20. Kalman kalmanX; // 对X,Y轴创建卡尔曼实例
    
21. Kalman kalmanY;
    
22. /* IMU Data */
    
23. double accX, accY, accZ;
    
24. double gyroX, gyroY, gyroZ;//从mpu6050读出的六个数据
    
25. 
    
26. double gyroXangle, gyroYangle; // Angle calculate using the gyro only
    
27. double compAngleX, compAngleY; // Calculated angle using a complementary filter
    
28. double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
    
29. double angleX, angleY, angle, angleZ; //finall angle
    
30. uint32_t timer;
    
31. uint8_t i2cData[14]; // Buffer for I2C data
    
32. 
    
33. // TODO: Make calibration routine
    
34. 
    
35. void setup() {
    
36. Serial.begin(115200);
    
37. Wire.begin();
    
38. #if ARDUINO >= 157
    
39. Wire.setClock(400000UL); // Set I2C frequency to 400kHz
    
40. #else
    
41. TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
    
42. #endif
    
43. 
    
44. i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
    
45. i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
    
46. i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
    
47. i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
    
48. while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
    
49. while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
    
50. 
    
51. while (i2cRead(0x75, i2cData, 1));
    
52. if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    
53. Serial.print(F("Error reading sensor"));
    
54. while (1);
    
55. }
    
56. 
    
57. delay(100); // Wait for sensor to stabilize
    
58. 
    
59. /* Set kalman and gyro starting angle */
    
60. while (i2cRead(0x3B, i2cData, 6));
    
61. accX = (i2cData[0] << 8) | i2cData[1];
    
62. accY = (i2cData[2] << 8) | i2cData[3];
    
63. accZ = (i2cData[4] << 8) | i2cData[5];
    
64. 
    
65. // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
    
66. // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
    
67. // It is then converted from radians to degrees
    
68. #ifdef RESTRICT_PITCH // Eq. 25 and 26
    
69. double roll = atan2(accY, accZ) * RAD_TO_DEG;
    
70. double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
    
71. #else // Eq. 28 and 29
    
72. double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
    
73. double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
    
74. #endif
    
75. 
    
76. kalmanX.setAngle(roll); // Set starting angle
    
77. kalmanY.setAngle(pitch);
    
78. gyroXangle = roll;
    
79. gyroYangle = pitch;
    
80. compAngleX = roll;
    
81. compAngleY = pitch;
    
82. 
    
83. //Calibration();
    
84. timer = micros();
    
85. }
    
86. 
    
87. void loop() {
    
88. /* Update all the values */
    
89. while (i2cRead(0x3B, i2cData, 14));
    
90. accX = ((i2cData[0] << 8) | i2cData[1]);
    
91. accY = ((i2cData[2] << 8) | i2cData[3]);
    
92. accZ = ((i2cData[4] << 8) | i2cData[5]);
    
93. //tempRaw = (i2cData[6] << 8) | i2cData[7];
    
94. gyroX = (i2cData[8] << 8) | i2cData[9];
    
95. gyroY = (i2cData[10] << 8) | i2cData[11];
    
96. gyroZ = (i2cData[12] << 8) | i2cData[13];
    
97. 
    
98. double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
    
99. timer = micros();
    
100. 
     
101. // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
     
102. // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
     
103. // It is then converted from radians to degrees
     
104. #ifdef RESTRICT_PITCH // Eq. 25 and 26
     
105. double roll = atan2(accY, accZ) * RAD_TO_DEG;
     
106. double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
     
107. #else // Eq. 28 and 29
     
108. double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
     
109. double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
     
110. #endif
     
111. 
     
112. double gyroXrate = gyroX / 131.0; // Convert to deg/s
     
113. double gyroYrate = gyroY / 131.0; // Convert to deg/s
     
114. 
     
115. #ifdef RESTRICT_PITCH
     
116. // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
     
117. if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
     
118. kalmanX.setAngle(roll);
     
119. compAngleX = roll;
     
120. kalAngleX = roll;
     
121. gyroXangle = roll;
     
122. } else
     
123. kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
     
124. 
     
125. if (abs(kalAngleX) > 90)
     
126. gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
     
127. kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
     
128. #else
     
129. // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
     
130. if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
     
131. kalmanY.setAngle(pitch);
     
132. compAngleY = pitch;
     
133. kalAngleY = pitch;
     
134. gyroYangle = pitch;
     
135. } else
     
136. kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
     
137. 
     
138. if (abs(kalAngleY) > 90)
     
139. gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
     
140. kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
     
141. #endif
     
142. 
     
143. gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
     
144. gyroYangle += gyroYrate * dt;
     
145. angle += gyroZ * dt / 131.0;
     
146. 
     
147. //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
     
148. //gyroYangle += kalmanY.getRate() * dt;
     
149. 
     
150. compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
     
151. compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
     
152. 
     
153. // Reset the gyro angle when it has drifted too much
     
154. if (gyroXangle < -180 || gyroXangle > 180)
     
155. gyroXangle = kalAngleX;
     
156. if (gyroYangle < -180 || gyroYangle > 180)
     
157. gyroYangle = kalAngleY;
     
158. 
     
159. angleX = kalAngleX - 2.32;
     
160. angleY = kalAngleY + 0.38;
     
161. angleZ = angle + 5;
     
162. 
     
163. /* Print Data */
     
164. #if 0 // Set to 1 to activate
     
165. Serial.print(accX); Serial.print(",");
     
166. Serial.print(accY); Serial.print(",");
     
167. Serial.print(accZ); Serial.print(",");
     
168. Serial.print(tempRaw); Serial.print(",");
     
169. Serial.print(gyroX / 131.0); Serial.print(",");
     
170. Serial.print(gyroY / 131.0); Serial.print(",");
     
171. Serial.print(gyroZ / 131.0); Serial.print(",");
     
172. 
     
173. Serial.print("\t");
     
174. #endif
     
175. //Serial.print(roll); Serial.print("\t");
     
176. //Serial.print(gyroXangle); Serial.print("\t");
     
177. //Serial.print(compAngleX); Serial.print("\t");
     
178. Serial.print(angleX); Serial.print(",");
     
179. 
     
180. //Serial.print("\t");
     
181. 
     
182. //Serial.print(pitch); Serial.print("\t");
     
183. //Serial.print(gyroYangle); Serial.print("\t");
     
184. //Serial.print(compAngleY); Serial.print("\t");
     
185. Serial.print(angleY); Serial.print(",");
     
186. Serial.print(angleZ); Serial.print(",");
     
187. 
     
188. }

复制代码

以上代码主要部分来自github里kalman函数带的一个例子mpu6050，做了一些细微的改变，毕竟新手嘛。现在呢是可以读出三轴的角度，接下来就是通过遥控器教练通道将数据通过遥控器发给接受器了。这里我还没有弄懂怎么将俩路pwm信号通过教练通道发给遥控器。等了解了再写。第一次接触mpu6050，第一次接触卡尔曼，第一次写帖子。希望大家轻拍。如果有问题，我尽我所知道的给予解答。也希望大神能够指点一二。谢谢。所需的库文件我都放在附件里。

hello-mygril

先收藏着，以后试试！

wetnt

先收藏，以后试试！