Chris Seto
/
MPU6050-DMP-4
MPU6050 issues
Fork of MPU6050 by
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?
User | Revision | Line number | New 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 | } |