Chris Seto / Mbed 2 deprecated MPU6050-DMP-4

MPU6050 issues

Dependencies:   mbed

Fork of MPU6050 by Shundo Kishi

main.cpp@6:6300d9561dfd, 2014-07-01 (annotated)

Committer:
chris1seto
Date:
Tue Jul 01 16:56:00 2014 +0000
Revision:
6:6300d9561dfd
Initial MPU6050 issues

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chris1seto 6:6300d9561dfd 1 #include "mbed.h"
chris1seto 6:6300d9561dfd 2 #include "I2Cdev.h"
chris1seto 6:6300d9561dfd 3 #include "MPU6050_6Axis_MotionApps20.h"
chris1seto 6:6300d9561dfd 4 #include "shared.h"
chris1seto 6:6300d9561dfd 5
chris1seto 6:6300d9561dfd 6 MPU6050 mpu;
chris1seto 6:6300d9561dfd 7
chris1seto 6:6300d9561dfd 8 // MPU control/status vars
chris1seto 6:6300d9561dfd 9 bool dmpReady = false; // set true if DMP init was successful
chris1seto 6:6300d9561dfd 10 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
chris1seto 6:6300d9561dfd 11 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
chris1seto 6:6300d9561dfd 12 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
chris1seto 6:6300d9561dfd 13 uint16_t fifoCount; // count of all bytes currently in FIFO
chris1seto 6:6300d9561dfd 14 uint8_t fifoBuffer[64]; // FIFO storage buffer
chris1seto 6:6300d9561dfd 15
chris1seto 6:6300d9561dfd 16 // orientation/motion vars
chris1seto 6:6300d9561dfd 17 Quaternion q; // [w, x, y, z] quaternion container
chris1seto 6:6300d9561dfd 18 VectorInt16 aa; // [x, y, z] accel sensor measurements
chris1seto 6:6300d9561dfd 19 VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
chris1seto 6:6300d9561dfd 20 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
chris1seto 6:6300d9561dfd 21 VectorFloat gravity; // [x, y, z] gravity vector
chris1seto 6:6300d9561dfd 22 float euler[3]; // [psi, theta, phi] Euler angle container
chris1seto 6:6300d9561dfd 23 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
chris1seto 6:6300d9561dfd 24
chris1seto 6:6300d9561dfd 25 volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
chris1seto 6:6300d9561dfd 26 void dmpDataReady()
chris1seto 6:6300d9561dfd 27 {
chris1seto 6:6300d9561dfd 28 mpuInterrupt = true;
chris1seto 6:6300d9561dfd 29 }
chris1seto 6:6300d9561dfd 30
chris1seto 6:6300d9561dfd 31 #ifndef M_PI
chris1seto 6:6300d9561dfd 32 #define M_PI 3.1415
chris1seto 6:6300d9561dfd 33 #endif
chris1seto 6:6300d9561dfd 34
chris1seto 6:6300d9561dfd 35 I2C i2c();
chris1seto 6:6300d9561dfd 36
chris1seto 6:6300d9561dfd 37 int main()
chris1seto 6:6300d9561dfd 38 {
chris1seto 6:6300d9561dfd 39 pc.baud(115200);
chris1seto 6:6300d9561dfd 40
chris1seto 6:6300d9561dfd 41 mpu.initialize();
chris1seto 6:6300d9561dfd 42
chris1seto 6:6300d9561dfd 43 // verify connection
chris1seto 6:6300d9561dfd 44 pc.printf("Testing device connections...\r\n");
chris1seto 6:6300d9561dfd 45
chris1seto 6:6300d9561dfd 46 bool mpu6050TestResult = mpu.testConnection();
chris1seto 6:6300d9561dfd 47 if(mpu6050TestResult)
chris1seto 6:6300d9561dfd 48 {
chris1seto 6:6300d9561dfd 49 pc.printf("MPU6050 test passed \r\n");
chris1seto 6:6300d9561dfd 50 }
chris1seto 6:6300d9561dfd 51 else
chris1seto 6:6300d9561dfd 52 {
chris1seto 6:6300d9561dfd 53 pc.printf("MPU6050 test failed \r\n");
chris1seto 6:6300d9561dfd 54 }
chris1seto 6:6300d9561dfd 55
chris1seto 6:6300d9561dfd 56 // load and configure the DMP
chris1seto 6:6300d9561dfd 57 pc.printf("Initializing DMP...\n");
chris1seto 6:6300d9561dfd 58 devStatus = mpu.dmpInitialize();
chris1seto 6:6300d9561dfd 59
chris1seto 6:6300d9561dfd 60 // supply your own gyro offsets here, scaled for min sensitivity
chris1seto 6:6300d9561dfd 61 mpu.setXGyroOffset(0);
chris1seto 6:6300d9561dfd 62 mpu.setYGyroOffset(0);
chris1seto 6:6300d9561dfd 63 mpu.setZGyroOffset(0);
chris1seto 6:6300d9561dfd 64 mpu.setZAccelOffset(16282); // 1688 factory default for my test chip
chris1seto 6:6300d9561dfd 65
chris1seto 6:6300d9561dfd 66 if (devStatus==0)
chris1seto 6:6300d9561dfd 67 {
chris1seto 6:6300d9561dfd 68 pc.printf("OK!\r\n");
chris1seto 6:6300d9561dfd 69 mpu.setDMPEnabled(true);
chris1seto 6:6300d9561dfd 70
chris1seto 6:6300d9561dfd 71 mpuIntStatus = mpu.getIntStatus();
chris1seto 6:6300d9561dfd 72
chris1seto 6:6300d9561dfd 73 packetSize = mpu.dmpGetFIFOPacketSize();
chris1seto 6:6300d9561dfd 74 }
chris1seto 6:6300d9561dfd 75 else
chris1seto 6:6300d9561dfd 76 {
chris1seto 6:6300d9561dfd 77 pc.printf("Failed with code %d\r\n", devStatus);
chris1seto 6:6300d9561dfd 78 while(1);
chris1seto 6:6300d9561dfd 79 }
chris1seto 6:6300d9561dfd 80
chris1seto 6:6300d9561dfd 81 // Main prog loop
chris1seto 6:6300d9561dfd 82 while (1)
chris1seto 6:6300d9561dfd 83 {
chris1seto 6:6300d9561dfd 84 wait_us(500);
chris1seto 6:6300d9561dfd 85
chris1seto 6:6300d9561dfd 86 mpuIntStatus = mpu.getIntStatus();
chris1seto 6:6300d9561dfd 87
chris1seto 6:6300d9561dfd 88 // get current FIFO count
chris1seto 6:6300d9561dfd 89 fifoCount = mpu.getFIFOCount();
chris1seto 6:6300d9561dfd 90
chris1seto 6:6300d9561dfd 91 // check for overflow (this should never happen unless our code is too inefficient)
chris1seto 6:6300d9561dfd 92 if ((mpuIntStatus & 0x10) || fifoCount == 1024)
chris1seto 6:6300d9561dfd 93 {
chris1seto 6:6300d9561dfd 94 // reset so we can continue cleanly
chris1seto 6:6300d9561dfd 95 mpu.resetFIFO();
chris1seto 6:6300d9561dfd 96 //pc.printf("FIFO overflow!");
chris1seto 6:6300d9561dfd 97 }
chris1seto 6:6300d9561dfd 98 else if (mpuIntStatus & 0x02)
chris1seto 6:6300d9561dfd 99 {
chris1seto 6:6300d9561dfd 100 // wait for correct available data length, should be a VERY short wait
chris1seto 6:6300d9561dfd 101 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
chris1seto 6:6300d9561dfd 102
chris1seto 6:6300d9561dfd 103 // read a packet from FIFO
chris1seto 6:6300d9561dfd 104 mpu.getFIFOBytes(fifoBuffer, packetSize);
chris1seto 6:6300d9561dfd 105
chris1seto 6:6300d9561dfd 106 // track FIFO count here in case there is > 1 packet available
chris1seto 6:6300d9561dfd 107 // (this lets us immediately read more without waiting for an interrupt)
chris1seto 6:6300d9561dfd 108 fifoCount -= packetSize;
chris1seto 6:6300d9561dfd 109
chris1seto 6:6300d9561dfd 110 // display Euler angles in degrees
chris1seto 6:6300d9561dfd 111 mpu.dmpGetQuaternion(&q, fifoBuffer);
chris1seto 6:6300d9561dfd 112 mpu.dmpGetGravity(&gravity, &q);
chris1seto 6:6300d9561dfd 113 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
chris1seto 6:6300d9561dfd 114 /*pc.printf("ypr\t");
chris1seto 6:6300d9561dfd 115 pc.printf("%f3.2",ypr[0] * 180/M_PI);
chris1seto 6:6300d9561dfd 116 pc.printf("\t");
chris1seto 6:6300d9561dfd 117 pc.printf("%f3.2",ypr[1] * 180/M_PI);
chris1seto 6:6300d9561dfd 118 pc.printf("\t");
chris1seto 6:6300d9561dfd 119 pc.printf("%f3.2\n",ypr[2] * 180/M_PI);*/
chris1seto 6:6300d9561dfd 120
chris1seto 6:6300d9561dfd 121 }
chris1seto 6:6300d9561dfd 122 }
chris1seto 6:6300d9561dfd 123 }