mpu6050 dmp test
Fork of MPU6050-DMP_6DOF by
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0) 00002 // 6/21/2012 by Jeff Rowberg <jeff@rowberg.net> 00003 // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 00004 // 00005 // Changelog: 00006 // 2013-05-08 - added seamless Fastwire support 00007 // - added note about gyro calibration 00008 // 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error 00009 // 2012-06-20 - improved FIFO overflow handling and simplified read process 00010 // 2012-06-19 - completely rearranged DMP initialization code and simplification 00011 // 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly 00012 // 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING 00013 // 2012-06-05 - add gravity-compensated initial reference frame acceleration output 00014 // - add 3D math helper file to DMP6 example sketch 00015 // - add Euler output and Yaw/Pitch/Roll output formats 00016 // 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee) 00017 // 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250 00018 // 2012-05-30 - basic DMP initialization working 00019 00020 /* ============================================ 00021 I2Cdev device library code is placed under the MIT license 00022 Copyright (c) 2012 Jeff Rowberg 00023 00024 Permission is hereby granted, free of charge, to any person obtaining a copy 00025 of this software and associated documentation files (the "Software"), to deal 00026 in the Software without restriction, including without limitation the rights 00027 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 00028 copies of the Software, and to permit persons to whom the Software is 00029 furnished to do so, subject to the following conditions: 00030 00031 The above copyright notice and this permission notice shall be included in 00032 all copies or substantial portions of the Software. 00033 00034 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 00035 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 00036 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 00037 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 00038 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 00039 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 00040 THE SOFTWARE. 00041 =============================================== 00042 */ 00043 00044 // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files 00045 // for both classes must be in the include path of your project 00046 #include "I2Cdev.h" 00047 #include "mbed.h" 00048 #include <math.h> 00049 DigitalOut leds[] = {(LED1), (LED2),(LED3),(LED4)}; 00050 00051 00052 #include "MPU6050_6Axis_MotionApps20.h" // works 00053 //#include "MPU6050_9Axis_MotionApps41.h" 00054 00055 //#include "MPU6050.h" // not necessary if using MotionApps include file 00056 00057 00058 // class default I2C address is 0x68 00059 // specific I2C addresses may be passed as a parameter here 00060 // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board) 00061 // AD0 high = 0x69 00062 MPU6050 mpu; 00063 //MPU6050 mpu(0x69); // <-- use for AD0 high 00064 00065 /* ========================================================================= 00066 NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch 00067 depends on the MPU-6050's INT pin being connected to the Arduino's 00068 external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is 00069 digital I/O pin 2. 00070 * ========================================================================= */ 00071 00072 /* ========================================================================= 00073 NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error 00074 when using Serial.write(buf, len). The Teapot output uses this method. 00075 The solution requires a modification to the Arduino USBAPI.h file, which 00076 is fortunately simple, but annoying. This will be fixed in the next IDE 00077 release. For more info, see these links: 00078 00079 http://arduino.cc/forum/index.php/topic,109987.0.html 00080 http://code.google.com/p/arduino/issues/detail?id=958 00081 * ========================================================================= */ 00082 00083 00084 #ifndef M_PI 00085 #define M_PI 3.1415 00086 #endif 00087 00088 // uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual 00089 // quaternion components in a [w, x, y, z] format (not best for parsing 00090 // on a remote host such as Processing or something though) 00091 //#define OUTPUT_READABLE_QUATERNION 00092 00093 // uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles 00094 // (in degrees) calculated from the quaternions coming from the FIFO. 00095 // Note that Euler angles suffer from gimbal lock (for more info, see 00096 // http://en.wikipedia.org/wiki/Gimbal_lock) 00097 //#define OUTPUT_READABLE_EULER 00098 00099 // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/ 00100 // pitch/roll angles (in degrees) calculated from the quaternions coming 00101 // from the FIFO. Note this also requires gravity vector calculations. 00102 // Also note that yaw/pitch/roll angles suffer from gimbal lock (for 00103 // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock) 00104 //#define OUTPUT_READABLE_YAWPITCHROLL 00105 00106 // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration 00107 // components with gravity removed. This acceleration reference frame is 00108 // not compensated for orientation, so +X is always +X according to the 00109 // sensor, just without the effects of gravity. If you want acceleration 00110 // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead. 00111 //#define OUTPUT_READABLE_REALACCEL 00112 00113 // uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration 00114 // components with gravity removed and adjusted for the world frame of 00115 // reference (yaw is relative to initial orientation, since no magnetometer 00116 // is present in this case). Could be quite handy in some cases. 00117 #define OUTPUT_READABLE_WORLDACCEL 00118 00119 // uncomment "OUTPUT_TEAPOT" if you want output that matches the 00120 // format used for the InvenSense teapot demo 00121 //#define OUTPUT_TEAPOT 00122 00123 00124 00125 bool blinkState = false; 00126 00127 // MPU control/status vars 00128 bool dmpReady = false; // set true if DMP init was successful 00129 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 00130 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) 00131 uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 00132 uint16_t fifoCount; // count of all bytes currently in FIFO 00133 uint8_t fifoBuffer[64]; // FIFO storage buffer 00134 00135 // orientation/motion vars 00136 Quaternion q; // [w, x, y, z] quaternion container 00137 VectorInt16 aa; // [x, y, z] accel sensor measurements 00138 VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements 00139 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements 00140 VectorFloat gravity; // [x, y, z] gravity vector 00141 float euler[3]; // [psi, theta, phi] Euler angle container 00142 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector 00143 00144 // packet structure for InvenSense teapot demo 00145 uint8_t teapotPacket[15] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n',0 }; 00146 00147 // ================================================================ 00148 // === INTERRUPT DETECTION ROUTINE === 00149 // ================================================================ 00150 00151 volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high 00152 void dmpDataReady() { 00153 mpuInterrupt = true; 00154 } 00155 00156 // ================================================================ 00157 // === INITIAL SETUP === 00158 // ================================================================ 00159 00160 int main() 00161 { 00162 00163 //Pin Defines for I2C Bus 00164 #define D_SDA p9 00165 #define D_SCL p10 00166 //#define D_SDA p28 00167 //#define D_SCL p27 00168 I2C i2c(D_SDA, D_SCL); 00169 00170 00171 // mbed Interface Hardware definitions 00172 DigitalOut myled1(LED1); 00173 DigitalOut myled2(LED2); 00174 DigitalOut myled3(LED3); 00175 DigitalOut heartbeatLED(LED4); 00176 00177 // initialize serial communication 00178 // (115200 chosen because it is required for Teapot Demo output, but it's 00179 // really up to you depending on your project) 00180 //Host PC Baudrate (Virtual Com Port on USB) 00181 #define D_BAUDRATE 115200 00182 00183 // Host PC Communication channels 00184 Serial pc(USBTX, USBRX); // tx, rx 00185 00186 pc.baud(D_BAUDRATE); 00187 // initialize device 00188 pc.printf("Initializing I2C devices...\n"); 00189 mpu.initialize(); 00190 00191 // verify connection 00192 pc.printf("Testing device connections...\n"); 00193 00194 bool mpu6050TestResult = mpu.testConnection(); 00195 if(mpu6050TestResult){ 00196 pc.printf("MPU6050 test passed \n"); 00197 } else{ 00198 pc.printf("MPU6050 test failed \n"); 00199 } 00200 00201 // wait for ready 00202 pc.printf("\nSend any character to begin DMP programming and demo: "); 00203 00204 while(!pc.readable()); 00205 pc.getc(); 00206 pc.printf("\n"); 00207 00208 // load and configure the DMP 00209 pc.printf("Initializing DMP...\n"); 00210 devStatus = mpu.dmpInitialize(); 00211 00212 // supply your own gyro offsets here, scaled for min sensitivity 00213 mpu.setXGyroOffset(-61); 00214 mpu.setYGyroOffset(-127); 00215 mpu.setZGyroOffset(19); 00216 mpu.setZAccelOffset(16282); // 1688 factory default for my test chip 00217 00218 // make sure it worked (returns 0 if so) 00219 if (devStatus == 0) { 00220 // turn on the DMP, now that it's ready 00221 pc.printf("Enabling DMP...\n"); 00222 mpu.setDMPEnabled(true); 00223 00224 // enable Arduino interrupt detection 00225 pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\n"); 00226 // attachInterrupt(0, dmpDataReady, RISING); 00227 mpuIntStatus = mpu.getIntStatus(); 00228 00229 // set our DMP Ready flag so the main loop() function knows it's okay to use it 00230 pc.printf("DMP ready! Waiting for first interrupt...\n"); 00231 dmpReady = true; 00232 00233 // get expected DMP packet size for later comparison 00234 packetSize = mpu.dmpGetFIFOPacketSize(); 00235 } else { 00236 // ERROR! 00237 // 1 = initial memory load failed 00238 // 2 = DMP configuration updates failed 00239 // (if it's going to break, usually the code will be 1) 00240 pc.printf("DMP Initialization failed (code "); 00241 pc.printf("%u",devStatus); 00242 pc.printf(")\n"); 00243 } 00244 00245 00246 // ================================================================ 00247 // === MAIN PROGRAM LOOP === 00248 // ================================================================ 00249 00250 while(1) 00251 { 00252 // if programming failed, don't try to do anything 00253 if (!dmpReady) continue; 00254 myled2=0; 00255 00256 // wait for MPU interrupt or extra packet(s) available 00257 // while (!mpuInterrupt && fifoCount < packetSize) { 00258 // while (!mpuIntStatus && fifoCount < packetSize) 00259 { 00260 // other program behavior stuff here 00261 // . 00262 // . 00263 // . 00264 // if you are really paranoid you can frequently test in between other 00265 // stuff to see if mpuInterrupt is true, and if so, "break;" from the 00266 // while() loop to immediately process the MPU data 00267 // . 00268 // . 00269 // . 00270 // fifoCount= mpu.getFIFOCount(); 00271 // mpuIntStatus = mpu.getIntStatus(); 00272 } 00273 wait_us(500); 00274 00275 // reset interrupt flag and get INT_STATUS byte 00276 mpuInterrupt = false; 00277 mpuIntStatus = mpu.getIntStatus(); 00278 00279 // get current FIFO count 00280 fifoCount = mpu.getFIFOCount(); 00281 00282 // check for overflow (this should never happen unless our code is too inefficient) 00283 if ((mpuIntStatus & 0x10) || fifoCount == 1024) { 00284 // reset so we can continue cleanly 00285 mpu.resetFIFO(); 00286 pc.printf("FIFO overflow!"); 00287 00288 // otherwise, check for DMP data ready interrupt (this should happen frequently) 00289 } else if (mpuIntStatus & 0x02) { 00290 // wait for correct available data length, should be a VERY short wait 00291 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); 00292 00293 // read a packet from FIFO 00294 mpu.getFIFOBytes(fifoBuffer, packetSize); 00295 00296 // track FIFO count here in case there is > 1 packet available 00297 // (this lets us immediately read more without waiting for an interrupt) 00298 fifoCount -= packetSize; 00299 00300 #ifdef OUTPUT_READABLE_QUATERNION 00301 // display quaternion values in easy matrix form: w x y z 00302 mpu.dmpGetQuaternion(&q, fifoBuffer); 00303 // pc.printf("quat\t"); 00304 pc.printf("%f",q.w); 00305 pc.printf(","); 00306 pc.printf("%f",q.x); 00307 pc.printf(","); 00308 pc.printf("%f",q.y); 00309 pc.printf(","); 00310 pc.printf("%f\n",q.z); 00311 #endif 00312 00313 #ifdef OUTPUT_READABLE_EULER 00314 // display Euler angles in degrees 00315 mpu.dmpGetQuaternion(&q, fifoBuffer); 00316 mpu.dmpGetEuler(euler, &q); 00317 pc.printf("euler\t"); 00318 pc.printf("%f",euler[0] * 180/M_PI); 00319 pc.printf("\t"); 00320 pc.printf("%f",euler[1] * 180/M_PI); 00321 pc.printf("\t"); 00322 pc.printf("%f\n",euler[2] * 180/M_PI); 00323 #endif 00324 00325 #ifdef OUTPUT_READABLE_YAWPITCHROLL 00326 // display Euler angles in degrees 00327 mpu.dmpGetQuaternion(&q, fifoBuffer); 00328 mpu.dmpGetGravity(&gravity, &q); 00329 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); 00330 pc.printf("ypr\t\r\n"); 00331 pc.printf("%f3.2\r\n",ypr[0] * 180/M_PI); 00332 pc.printf("\t"); 00333 pc.printf("%f3.2\r\n",ypr[1] * 180/M_PI); 00334 pc.printf("\t"); 00335 pc.printf("%f3.2\r\n",ypr[2] * 180/M_PI); 00336 #endif 00337 00338 #ifdef OUTPUT_READABLE_REALACCEL 00339 // display real acceleration, adjusted to remove gravity 00340 mpu.dmpGetQuaternion(&q, fifoBuffer); 00341 mpu.dmpGetAccel(&aa, fifoBuffer); 00342 mpu.dmpGetGravity(&gravity, &q); 00343 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); 00344 pc.printf("areal\t"); 00345 pc.printf("%d",aaReal.x); 00346 pc.printf("\t"); 00347 pc.printf("%d",aaReal.y); 00348 pc.printf("\t"); 00349 pc.printf("%d\n",aaReal.z); 00350 #endif 00351 00352 #ifdef OUTPUT_READABLE_WORLDACCEL 00353 // display initial world-frame acceleration, adjusted to remove gravity 00354 // and rotated based on known orientation from quaternion 00355 mpu.dmpGetQuaternion(&q, fifoBuffer); 00356 mpu.dmpGetAccel(&aa, fifoBuffer); 00357 mpu.dmpGetGravity(&gravity, &q); 00358 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); 00359 mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); 00360 pc.printf("aworld\t"); 00361 pc.printf("%d",aaWorld.x); 00362 pc.printf("\t"); 00363 pc.printf("%d",aaWorld.y); 00364 pc.printf("\t"); 00365 pc.printf("%d\n",aaWorld.z); 00366 #endif 00367 #ifdef OUTPUT_TEAPOT 00368 // display quaternion values in InvenSense Teapot demo format: 00369 teapotPacket[2] = fifoBuffer[0]; 00370 teapotPacket[3] = fifoBuffer[1]; 00371 teapotPacket[4] = fifoBuffer[4]; 00372 teapotPacket[5] = fifoBuffer[5]; 00373 teapotPacket[6] = fifoBuffer[8]; 00374 teapotPacket[7] = fifoBuffer[9]; 00375 teapotPacket[8] = fifoBuffer[12]; 00376 teapotPacket[9] = fifoBuffer[13]; 00377 00378 for(int i=0;i<14;i++) 00379 { 00380 pc.putc(teapotPacket[i]); 00381 } 00382 // pc.printf("%d",teapotPacket, 14); 00383 teapotPacket[11]++; // packetCount, loops at 0xFF on purpose 00384 #endif 00385 00386 // blink LED to indicate activity 00387 blinkState = !blinkState; 00388 myled1 = blinkState; 00389 } 00390 } 00391 }
Generated on Wed Jul 13 2022 05:19:09 by
1.7.2
