Chris Seto
/
MPU6050-DMP-fixpossible
Resolve STM issues
Fork of MPU6050 by
main.cpp@6:b272bd888e98, 2014-07-02 (annotated)
- Committer:
- chris1seto
- Date:
- Wed Jul 02 19:38:47 2014 +0000
- Revision:
- 6:b272bd888e98
inital, still having issues starting DMP
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
chris1seto | 6:b272bd888e98 | 1 | #include "mbed.h" |
chris1seto | 6:b272bd888e98 | 2 | #include "I2Cdev.h" |
chris1seto | 6:b272bd888e98 | 3 | #include "MPU6050_6Axis_MotionApps20.h" |
chris1seto | 6:b272bd888e98 | 4 | #include "shared.h" |
chris1seto | 6:b272bd888e98 | 5 | |
chris1seto | 6:b272bd888e98 | 6 | #ifndef M_PI |
chris1seto | 6:b272bd888e98 | 7 | #define M_PI 3.1415 |
chris1seto | 6:b272bd888e98 | 8 | #endif |
chris1seto | 6:b272bd888e98 | 9 | |
chris1seto | 6:b272bd888e98 | 10 | MPU6050 mpu; |
chris1seto | 6:b272bd888e98 | 11 | |
chris1seto | 6:b272bd888e98 | 12 | // MPU control/status vars |
chris1seto | 6:b272bd888e98 | 13 | bool dmpReady = false; // set true if DMP init was successful |
chris1seto | 6:b272bd888e98 | 14 | uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU |
chris1seto | 6:b272bd888e98 | 15 | uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) |
chris1seto | 6:b272bd888e98 | 16 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes) |
chris1seto | 6:b272bd888e98 | 17 | uint16_t fifoCount; // count of all bytes currently in FIFO |
chris1seto | 6:b272bd888e98 | 18 | uint8_t fifoBuffer[64]; // FIFO storage buffer |
chris1seto | 6:b272bd888e98 | 19 | |
chris1seto | 6:b272bd888e98 | 20 | // orientation/motion vars |
chris1seto | 6:b272bd888e98 | 21 | Quaternion q; // [w, x, y, z] quaternion container |
chris1seto | 6:b272bd888e98 | 22 | VectorInt16 aa; // [x, y, z] accel sensor measurements |
chris1seto | 6:b272bd888e98 | 23 | VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements |
chris1seto | 6:b272bd888e98 | 24 | VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements |
chris1seto | 6:b272bd888e98 | 25 | VectorFloat gravity; // [x, y, z] gravity vector |
chris1seto | 6:b272bd888e98 | 26 | float euler[3]; // [psi, theta, phi] Euler angle container |
chris1seto | 6:b272bd888e98 | 27 | float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector |
chris1seto | 6:b272bd888e98 | 28 | int count = 0; |
chris1seto | 6:b272bd888e98 | 29 | |
chris1seto | 6:b272bd888e98 | 30 | int main() |
chris1seto | 6:b272bd888e98 | 31 | { |
chris1seto | 6:b272bd888e98 | 32 | pc.baud(115200); |
chris1seto | 6:b272bd888e98 | 33 | pc.printf("MPU6050 initialize \r\n"); |
chris1seto | 6:b272bd888e98 | 34 | |
chris1seto | 6:b272bd888e98 | 35 | mpu.initialize(); |
chris1seto | 6:b272bd888e98 | 36 | pc.printf("MPU6050 testConnection \r\n"); |
chris1seto | 6:b272bd888e98 | 37 | |
chris1seto | 6:b272bd888e98 | 38 | if(mpu.testConnection()) |
chris1seto | 6:b272bd888e98 | 39 | { |
chris1seto | 6:b272bd888e98 | 40 | pc.printf("MPU6050 test passed \r\n"); |
chris1seto | 6:b272bd888e98 | 41 | } |
chris1seto | 6:b272bd888e98 | 42 | else |
chris1seto | 6:b272bd888e98 | 43 | { |
chris1seto | 6:b272bd888e98 | 44 | pc.printf("MPU6050 test failed \r\n"); |
chris1seto | 6:b272bd888e98 | 45 | } |
chris1seto | 6:b272bd888e98 | 46 | |
chris1seto | 6:b272bd888e98 | 47 | devStatus = mpu.dmpInitialize(); |
chris1seto | 6:b272bd888e98 | 48 | |
chris1seto | 6:b272bd888e98 | 49 | // supply your own gyro offsets here, scaled for min sensitivity |
chris1seto | 6:b272bd888e98 | 50 | mpu.setXGyroOffset(-61); |
chris1seto | 6:b272bd888e98 | 51 | mpu.setYGyroOffset(-127); |
chris1seto | 6:b272bd888e98 | 52 | mpu.setZGyroOffset(19); |
chris1seto | 6:b272bd888e98 | 53 | mpu.setZAccelOffset(16282); // 1688 factory default for my test chip |
chris1seto | 6:b272bd888e98 | 54 | |
chris1seto | 6:b272bd888e98 | 55 | // make sure it worked (returns 0 if so) |
chris1seto | 6:b272bd888e98 | 56 | if (devStatus == 0) |
chris1seto | 6:b272bd888e98 | 57 | { |
chris1seto | 6:b272bd888e98 | 58 | // turn on the DMP, now that it's ready |
chris1seto | 6:b272bd888e98 | 59 | pc.printf("Enabling DMP...\r\n"); |
chris1seto | 6:b272bd888e98 | 60 | mpu.setDMPEnabled(true); |
chris1seto | 6:b272bd888e98 | 61 | |
chris1seto | 6:b272bd888e98 | 62 | mpuIntStatus = mpu.getIntStatus(); |
chris1seto | 6:b272bd888e98 | 63 | |
chris1seto | 6:b272bd888e98 | 64 | // set our DMP Ready flag so the main loop() function knows it's okay to use it |
chris1seto | 6:b272bd888e98 | 65 | pc.printf("DMP ready!\r\n"); |
chris1seto | 6:b272bd888e98 | 66 | dmpReady = true; |
chris1seto | 6:b272bd888e98 | 67 | |
chris1seto | 6:b272bd888e98 | 68 | // get expected DMP packet size for later comparison |
chris1seto | 6:b272bd888e98 | 69 | packetSize = mpu.dmpGetFIFOPacketSize(); |
chris1seto | 6:b272bd888e98 | 70 | } |
chris1seto | 6:b272bd888e98 | 71 | else |
chris1seto | 6:b272bd888e98 | 72 | { |
chris1seto | 6:b272bd888e98 | 73 | // ERROR! |
chris1seto | 6:b272bd888e98 | 74 | // 1 = initial memory load failed |
chris1seto | 6:b272bd888e98 | 75 | // 2 = DMP configuration updates failed |
chris1seto | 6:b272bd888e98 | 76 | // (if it's going to break, usually the code will be 1) |
chris1seto | 6:b272bd888e98 | 77 | pc.printf("DMP Initialization failed (code %d )\r\n", devStatus); |
chris1seto | 6:b272bd888e98 | 78 | while (1); |
chris1seto | 6:b272bd888e98 | 79 | } |
chris1seto | 6:b272bd888e98 | 80 | |
chris1seto | 6:b272bd888e98 | 81 | while (1); |
chris1seto | 6:b272bd888e98 | 82 | |
chris1seto | 6:b272bd888e98 | 83 | while (1) |
chris1seto | 6:b272bd888e98 | 84 | { |
chris1seto | 6:b272bd888e98 | 85 | mpuIntStatus = mpu.getIntStatus(); |
chris1seto | 6:b272bd888e98 | 86 | fifoCount = mpu.getFIFOCount(); |
chris1seto | 6:b272bd888e98 | 87 | |
chris1seto | 6:b272bd888e98 | 88 | // Check overflow |
chris1seto | 6:b272bd888e98 | 89 | if ((mpuIntStatus & 0x10) || fifoCount == 1024) |
chris1seto | 6:b272bd888e98 | 90 | { |
chris1seto | 6:b272bd888e98 | 91 | // reset so we can continue cleanly |
chris1seto | 6:b272bd888e98 | 92 | mpu.resetFIFO(); |
chris1seto | 6:b272bd888e98 | 93 | pc.printf("FIFO overflow!\r\n"); |
chris1seto | 6:b272bd888e98 | 94 | } |
chris1seto | 6:b272bd888e98 | 95 | |
chris1seto | 6:b272bd888e98 | 96 | if (mpuIntStatus & 0x02) |
chris1seto | 6:b272bd888e98 | 97 | { |
chris1seto | 6:b272bd888e98 | 98 | // wait for correct available data length, should be a VERY short wait |
chris1seto | 6:b272bd888e98 | 99 | while (fifoCount < packetSize) |
chris1seto | 6:b272bd888e98 | 100 | { |
chris1seto | 6:b272bd888e98 | 101 | fifoCount = mpu.getFIFOCount(); |
chris1seto | 6:b272bd888e98 | 102 | } |
chris1seto | 6:b272bd888e98 | 103 | |
chris1seto | 6:b272bd888e98 | 104 | mpu.getFIFOBytes(fifoBuffer, packetSize); |
chris1seto | 6:b272bd888e98 | 105 | |
chris1seto | 6:b272bd888e98 | 106 | fifoCount -= packetSize; |
chris1seto | 6:b272bd888e98 | 107 | } |
chris1seto | 6:b272bd888e98 | 108 | |
chris1seto | 6:b272bd888e98 | 109 | mpu.dmpGetQuaternion(&q, fifoBuffer); |
chris1seto | 6:b272bd888e98 | 110 | mpu.dmpGetEuler(euler, &q); |
chris1seto | 6:b272bd888e98 | 111 | |
chris1seto | 6:b272bd888e98 | 112 | for (int i = 0; i < 3; i++) |
chris1seto | 6:b272bd888e98 | 113 | { |
chris1seto | 6:b272bd888e98 | 114 | euler[i] *= 180/M_PI; |
chris1seto | 6:b272bd888e98 | 115 | } |
chris1seto | 6:b272bd888e98 | 116 | |
chris1seto | 6:b272bd888e98 | 117 | if (count++ > 20) |
chris1seto | 6:b272bd888e98 | 118 | { |
chris1seto | 6:b272bd888e98 | 119 | pc.printf("euler\t%d\t%d\t%d\r\n", euler[0], euler[1], euler[2]); |
chris1seto | 6:b272bd888e98 | 120 | count = 0; |
chris1seto | 6:b272bd888e98 | 121 | } |
chris1seto | 6:b272bd888e98 | 122 | } |
chris1seto | 6:b272bd888e98 | 123 | |
chris1seto | 6:b272bd888e98 | 124 | } |