DMP參考數(shù)據(jù)手冊

1、參考數(shù)據(jù)手冊：PS-MPU-6000A使用帶有DMP的最新庫函數(shù)（角度： DMP庫函數(shù)的dmpGetYawPitchRoll，可以得到pitch(俯仰），yaw(偏航），roll（滾轉(zhuǎn)）角度。角速度： void getRotation(int16_t* x, int16_t* y, int16_t* z); int16_t getRotationX(); int16_t getRotationY(); int16_t getRotationZ();16位，配置時四個量程可選：250，500，1000，2000 度/s。在dmp的例程中初始化：setFullScaleGyroRange(MPU6

2、050_GYRO_FS_2000);說明是選用最高量程2000º/s，則換算系數(shù)=216/4000=16.4 LSB/(度/s)2012-11-13 15:57 上傳下載附件 (157.65 KB) 加速度庫函數(shù)： void getAcceleration(int16_t* x, int16_t* y, int16_t* z); int16_t getAccelerationX(); int16_t getAccelerationY(); int16_t getAccelerationZ();16位，配置時四個量程可選：2，4，8，16 g。在dmp的例程中沒有找到初始化部分，估計是

3、采用了缺省配置。通過觀察輸出值，采用的是最小量程2g，則換算系數(shù)=216/4=16384 LSB/g2012-11-13 15:57 上傳下載附件 (189.39 KB) 下面是程序，中斷方式，需要把MPU的中斷接到arduino數(shù)字2腳上。ARDUINO 代碼復制打印1. / Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation2. / is used in I2Cdev.h3. #include Wire.h4. 5. / I2Cdev and MPU6050 must be instal

4、led as libraries, or else the .cpp/.h files6. / for both classes must be in the include path of your project7. #include I2Cdev.h8. 9. #include MPU6050_6Axis_MotionApps20.h10. MPU6050 mpu(0x68);11. 12. / MPU control/status vars13. bool dmpReady = false;/ set true if DMP init was successful14. uint8_t

5、 mpuIntStatus; / holds actual interrupt status byte from MPU15. uint8_t devStatus; / return status after each device operation (0 = success, !0 = error)16. uint16_t packetSize; / expected DMP packet size (default is 42 bytes)17. uint16_t fifoCount; / count of all bytes currently in FIFO18. uint8_t f

6、ifoBuffer64; / FIFO storage buffer19. 20. / orientation/motion vars21. Quaternion q; / w, x, y, z quaternion container22. VectorFloat gravity; / x, y, z gravity vector23. float ypr3; / yaw, pitch, roll yaw/pitch/roll container and gravity vector24. 25. 26. / =27. / = INTERRUPT DETECTION ROUTINE =28.

7、 / =29. 30. volatile bool mpuInterrupt = false; / indicates whether MPU interrupt pin has gone high31. void dmpDataReady() 32. mpuInterrupt = true;33. 34. 35. 36. 37. / =38. / = INITIAL SETUP =39. / =40. 41. void setup() 42. Serial.begin(115200); / opens serial port, sets data rate to 9600 bps43. 44

8、. / join I2C bus (I2Cdev library doesnt do this automatically)45. Wire.begin();46. 47. / initialize device48. Serial.println(Initializing I2C devices.);49. mpu.initialize();50. 51. / verify connection52. Serial.println(Testing device connections.);53. Serial.println(mpu.testConnection() ? MPU6050 co

9、nnection successful : MPU6050 connection failed);54. 55. delay(2);56. 57. / load and configure the DMP58. Serial.println(Initializing DMP.);59. devStatus = mpu.dmpInitialize();60. 61. / make sure it worked (returns 0 if so)62. if (devStatus = 0) 63. / turn on the DMP, now that its ready64. Serial.pr

10、intln(Enabling DMP.);65. mpu.setDMPEnabled(true);66. 67. / enable Arduino interrupt detection68. Serial.println(Enabling interrupt detection (Arduino external interrupt 0).);69. attachInterrupt(0, dmpDataReady, RISING);70. mpuIntStatus = mpu.getIntStatus();71. 72. / set our DMP Ready flag so the mai

11、n loop() function knows its okay to use it73. Serial.println(DMP ready! Waiting for first interrupt.);74. dmpReady = true;75. 76. / get expected DMP packet size for later comparison77. packetSize = mpu.dmpGetFIFOPacketSize();78. 79. else 80. / ERROR!81. / 1 = initial memory load failed82. / 2 = DMP

12、configuration updates failed83. / (if its going to break, usually the code will be 1)84. Serial.print(DMP Initialization failed (code );85. Serial.print(devStatus);86. Serial.println();87. 88. 89. 90. void loop()91. 92. float alpha,omiga;93. 94. / if programming failed, dont try to do anything95. if

13、 (!dmpReady) 96. return;97. 98. / wait for MPU interrupt or extra packet(s) available99. if (!mpuInterrupt & fifoCount packetSize) 100. return;101. 102. / reset interrupt flag and get INT_STATUS byte103. mpuInterrupt = false;104. mpuIntStatus = mpu.getIntStatus();105. 106. / get current FIFO count10

14、7. fifoCount = mpu.getFIFOCount();108. 109. / check for overflow (this should never happen unless our code is too inefficient)110. if (mpuIntStatus & 0x10) | fifoCount = 1024) 111. / reset so we can continue cleanly112. mpu.resetFIFO();113. Serial.println(FIFO overflow!);114. 115. / otherwise, check

15、 for DMP data ready interrupt (this should happen frequently)116. 117. else if (mpuIntStatus & 0x02) 118. / wait for correct available data length, should be a VERY short wait119. while (fifoCount 1 packet available125. / (this lets us immediately read more without waiting for an interrupt)126. fifo

16、Count -= packetSize;127. 128. mpu.dmpGetQuaternion(&q, fifoBuffer);129. mpu.dmpGetGravity(&gravity, &q);130. mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);/從DMP中取出Yaw、Pitch、Roll三個軸的角度，放入數(shù)組ypr。單位：弧度131. alpha=-ypr2 * 180/M_PI;132. 133. omiga=mpu.getRotationX()/16.4; /配置是16位表示正負2000/s, 65536/4000134. 135.

17、 Serial.print(Alpha );136. Serial.print(alpha);137. Serial.print(tOmiga );138. Serial.println(omiga);139. 140. 141. 1. / Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation2. 3. / is used in I2Cdev.h4. 5. #include Wire.h6. 7. 8. 9. / I2Cdev and MPU6050 must be installed as

18、libraries, or else the .cpp/.h files10. 11. / for both classes must be in the include path of your project12. 13. #include I2Cdev.h14. 15. 16. 17. #include MPU6050_6Axis_MotionApps20.h18. 19. MPU6050 mpu(0x68);20. 21. 22. 23. / MPU control/status vars24. 25. bool dmpReady = false;/ set true if DMP i

19、nit was successful26. 27. uint8_t mpuIntStatus; / holds actual interrupt status byte from MPU28. 29. uint8_t devStatus; / return status after each device operation (0 = success, !0 = error)30. 31. uint16_t packetSize; / expected DMP packet size (default is 42 bytes)32. 33. uint16_t fifoCount; / coun

20、t of all bytes currently in FIFO34. 35. uint8_t fifoBuffer64; / FIFO storage buffer36. 37. 38. 39. / orientation/motion vars40. 41. Quaternion q; / w, x, y, z quaternion container42. 43. VectorFloat gravity; / x, y, z gravity vector44. 45. float ypr3; / yaw, pitch, roll yaw/pitch/roll container and

21、gravity vector46. 47. 48. 49. 50. 51. / =52. 53. / = INTERRUPT DETECTION ROUTINE =54. 55. / =56. 57. 58. 59. volatile bool mpuInterrupt = false; / indicates whether MPU interrupt pin has gone high60. 61. void dmpDataReady() 62. 63. mpuInterrupt = true;64. 65. 66. 67. 68. 69. 70. 71. 72. 73. / =74. 7

22、5. / = INITIAL SETUP =76. 77. / =78. 79. 80. 81. void setup() 82. 83. Serial.begin(115200); / opens serial port, sets data rate to 9600 bps84. 85. 86. 87. / join I2C bus (I2Cdev library doesnt do this automatically)88. 89. Wire.begin();90. 91. 92. 93. / initialize device94. 95. Serial.println(Initia

23、lizing I2C devices.);96. 97. mpu.initialize();98. 99. 100. 101. / verify connection102. 103. Serial.println(Testing device connections.);104. 105. Serial.println(mpu.testConnection() ? MPU6050 connection successful : MPU6050 connection failed);106. 107. 108. 109. delay(2);110. 111. 112. 113. / load

24、and configure the DMP114. 115. Serial.println(Initializing DMP.);116. 117. devStatus = mpu.dmpInitialize();118. 119. 120. 121. / make sure it worked (returns 0 if so)122. 123. if (devStatus = 0) 124. 125. / turn on the DMP, now that its ready126. 127. Serial.println(Enabling DMP.);128. 129. mpu.setD

25、MPEnabled(true);130. 131. 132. 133. / enable Arduino interrupt detection134. 135. Serial.println(Enabling interrupt detection (Arduino external interrupt 0).);136. 137. attachInterrupt(0, dmpDataReady, RISING);138. 139. mpuIntStatus = mpu.getIntStatus();140. 141. 142. 143. / set our DMP Ready flag s

26、o the main loop() function knows its okay to use it144. 145. Serial.println(DMP ready! Waiting for first interrupt.);146. 147. dmpReady = true;148. 149. 150. 151. / get expected DMP packet size for later comparison152. 153. packetSize = mpu.dmpGetFIFOPacketSize();154. 155. 156. 157. else 158. 159. /

27、 ERROR!160. 161. / 1 = initial memory load failed162. 163. / 2 = DMP configuration updates failed164. 165. / (if its going to break, usually the code will be 1)166. 167. Serial.print(DMP Initialization failed (code );168. 169. Serial.print(devStatus);170. 171. Serial.println();172. 173. 174. 175. 17

28、6. 177. 178. 179. void loop()180. 181. 182. 183. float alpha,omiga;184. 185. 186. 187. / if programming failed, dont try to do anything188. 189. if (!dmpReady) 190. 191. return;192. 193. 194. 195. / wait for MPU interrupt or extra packet(s) available196. 197. if (!mpuInterrupt & fifoCount packetSize

29、) 198. 199. return;200. 201. 202. 203. / reset interrupt flag and get INT_STATUS byte204. 205. mpuInterrupt = false;206. 207. mpuIntStatus = mpu.getIntStatus();208. 209. 210. 211. / get current FIFO count212. 213. fifoCount = mpu.getFIFOCount();214. 215. 216. 217. / check for overflow (this should n

30、ever happen unless our code is too inefficient)218. 219. if (mpuIntStatus & 0x10) | fifoCount = 1024) 220. 221. / reset so we can continue cleanly222. 223. mpu.resetFIFO();224. 225. Serial.println(FIFO overflow!);226. 227. 228. 229. / otherwise, check for DMP data ready interrupt (this should happen

31、 frequently)230. 231. 232. 233. else if (mpuIntStatus & 0x02) 234. 235. / wait for correct available data length, should be a VERY short wait236. 237. while (fifoCount 1 packet available248. 249. / (this lets us immediately read more without waiting for an interrupt)250. 251. fifoCount -= packetSize

32、;252. 253. 254. 255. mpu.dmpGetQuaternion(&q, fifoBuffer);256. 257. mpu.dmpGetGravity(&gravity, &q);258. 259. mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);/從DMP中取出Yaw、Pitch、Roll三個軸的角度，放入數(shù)組ypr。單位：弧度260. 261. alpha=-ypr2 * 180/M_PI;262. 263. 264. 265. omiga=mpu.getRotationX()/16.4; /配置是16位表示正負2000/s, 65536/4000266. 267. 268. 269. Serial.print(Alpha );270. 271. Serial.print(alpha);272. 273. Serial.print(tOmi