Chris Seto / Mbed 2 deprecated MPU6050-DMP-fixpossible

Resolve STM issues

Dependencies:   mbed

Fork of MPU6050 by Shundo Kishi

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?

UserRevisionLine numberNew 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 }