陀螺仪加速度计 MPU6050 启用DMP运算角度代码

xcl3721

本人新人——废话不多说了……直接上代码……
官网也有这个代码，但是发这里大家用的方便……：）
记得把int接入D2口作为中断
接线和以前一样接arduino 的 I2C接口
SDL SCL两个口
3.3v电源

1. // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
   
2. // is used in I2Cdev.h
   
3. #include "Wire.h"
   
4. 
   
5. // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
   
6. // for both classes must be in the include path of your project
   
7. #include "I2Cdev.h"
   
8. 
   
9. #include "MPU6050_6Axis_MotionApps20.h"
   
10. MPU6050 mpu(0x68);
    
11. 
    
12. // MPU control/status vars
    
13. bool dmpReady = false;  // set true if DMP init was successful
    
14. uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
    
15. 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 FIFO
    
18. uint8_t fifoBuffer[64]; // FIFO storage buffer
    
19. 
    
20. // orientation/motion vars
    
21. Quaternion q;           // [w, x, y, z]         quaternion container
    
22. VectorFloat gravity;    // [x, y, z]            gravity vector
    
23. float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
    
24. 
    
25. 
    
26. // ================================================================
    
27. // ===               INTERRUPT DETECTION ROUTINE                ===
    
28. // ================================================================
    
29. 
    
30. volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
    
31. void dmpDataReady() {
    
32.   mpuInterrupt = true;
    
33. }
    
34. 
    
35. 
    
36. 
    
37. // ================================================================
    
38. // ===                      INITIAL SETUP                       ===
    
39. // ================================================================
    
40. 
    
41. void mpu_setup() {
    
42.   //Serial.begin(115200);        // opens serial port, sets data rate to 9600 bps
    
43. 
    
44.   // join I2C bus (I2Cdev library doesn't do this automatically)
    
45.   Wire.begin();
    
46. 
    
47.   // initialize device
    
48.   Serial.println("Initializing I2C devices...");
    
49.   mpu.initialize();
    
50. 
    
51.   // verify connection
    
52.   Serial.println("Testing device connections...");
    
53.   Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
    
54. 
    
55.   delay(2);
    
56. 
    
57.   // load and configure the DMP
    
58.   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 it's ready
    
64.     Serial.println("Enabling DMP...");
    
65.     mpu.setDMPEnabled(true);
    
66. 
    
67.     // enable Arduino interrupt detection
    
68.     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 main loop() function knows it's okay to use it
    
73.     Serial.println("DMP ready! Waiting for first interrupt...");
    
74.     dmpReady = true;
    
75. 
    
76.     // get expected DMP packet size for later comparison
    
77.     packetSize = mpu.dmpGetFIFOPacketSize();
    
78.   }
    
79.   else {
    
80.     // ERROR!
    
81.     // 1 = initial memory load failed
    
82.     // 2 = DMP configuration updates failed
    
83.     // (if it's 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 mpu_loop()
    
91. {
    
92.   float alpha,omiga;
    
93. 
    
94.   // if programming failed, don't try to do anything
    
95.   if (!dmpReady)
    
96.     return;
    
97. 
    
98.   // wait for MPU interrupt or extra packet(s) available
    
99.   if (!mpuInterrupt && fifoCount < packetSize)
    
100.     return;
     
101. 
     
102.   // reset interrupt flag and get INT_STATUS byte
     
103.   mpuInterrupt = false;
     
104.   mpuIntStatus = mpu.getIntStatus();
     
105. 
     
106.   // get current FIFO count
     
107.   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 cleanly
     
112.     mpu.resetFIFO();
     
113.     Serial.println("FIFO overflow!");
     
114. 
     
115.     // otherwise, check 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 wait
     
119.     while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
     
120. 
     
121.     // read a packet from FIFO
     
122.     mpu.getFIFOBytes(fifoBuffer, packetSize);
     
123. 
     
124.     // track FIFO count here in case there is > 1 packet available
     
125.     // (this lets us immediately read more without waiting for an interrupt)
     
126.     fifoCount -= packetSize;
     
127. 
     
128.     mpu.dmpGetQuaternion(&q, fifoBuffer);
     
129.     mpu.dmpGetGravity(&gravity, &q);
     
130.     mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);  //从DMP中取出Yaw、Pitch、Roll三个轴的角度，放入数组ypr。单位：弧度
     
131.     alpha=-ypr[2] * 180/M_PI;
     
132. 
     
133.     omiga=mpu.getRotationX()/16.4;        //配置是16位表示正负2000°/s, 65536/4000
     
134. 
     
135.     Serial.print("Alpha ");
     
136.     Serial.print(alpha);//x轴平行偏移角度
     
137.     Serial.print("\tOmiga ");
     
138.     Serial.println(omiga);//对应加速度力道
     
139. 
     
140.   }
     
141. }

复制代码

另外谁研究过dmp的电子罗盘接入？
请分享下

隨風大俠

您好:
請問陀螺儀或加速器的中斷口是做什麼用的啊??

謝謝

xcl3721

TTTTTTT33 发表于 2013-9-5 15:19
您好:
請問陀螺儀或加速器的中斷口是做什麼用的啊??

中断功能很多，比如fifo缓冲溢出、数据准备完毕，都会使用这个中断进行触发，还有我记得文档内还有mpu的跌落中断，好像是当失控跌落的时候也会产生中断
使用mpu.getIntStatus(); 可以获取到此次中断的状态

chqiyi

有空找个机会测试一下楼主的代码，谢谢楼主分享

小桥流水

楼主，试过滚角Roll没？很怪异，数值乱变！

qq367176275

有空测试一下

zgkjzf

好多东西没搞明白 尤其是中断

雨轩

1.6.5版本编译通不过

YiuCheong

敢问楼主的库在哪里下载？

商朕

DMP 运算角度的精度如何