Chris Seto
/
MPU6050-DMP-fixpossible
Resolve STM issues
Fork of MPU6050 by
Diff: main.cpp
- Revision:
- 6:b272bd888e98
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Jul 02 19:38:47 2014 +0000 @@ -0,0 +1,124 @@ +#include "mbed.h" +#include "I2Cdev.h" +#include "MPU6050_6Axis_MotionApps20.h" +#include "shared.h" + +#ifndef M_PI +#define M_PI 3.1415 +#endif + +MPU6050 mpu; + +// MPU control/status vars +bool dmpReady = false; // set true if DMP init was successful +uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU +uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) +uint16_t packetSize; // expected DMP packet size (default is 42 bytes) +uint16_t fifoCount; // count of all bytes currently in FIFO +uint8_t fifoBuffer[64]; // FIFO storage buffer + +// orientation/motion vars +Quaternion q; // [w, x, y, z] quaternion container +VectorInt16 aa; // [x, y, z] accel sensor measurements +VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements +VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements +VectorFloat gravity; // [x, y, z] gravity vector +float euler[3]; // [psi, theta, phi] Euler angle container +float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector +int count = 0; + +int main() +{ + pc.baud(115200); + pc.printf("MPU6050 initialize \r\n"); + + mpu.initialize(); + pc.printf("MPU6050 testConnection \r\n"); + + if(mpu.testConnection()) + { + pc.printf("MPU6050 test passed \r\n"); + } + else + { + pc.printf("MPU6050 test failed \r\n"); + } + + devStatus = mpu.dmpInitialize(); + + // supply your own gyro offsets here, scaled for min sensitivity + mpu.setXGyroOffset(-61); + mpu.setYGyroOffset(-127); + mpu.setZGyroOffset(19); + mpu.setZAccelOffset(16282); // 1688 factory default for my test chip + + // make sure it worked (returns 0 if so) + if (devStatus == 0) + { + // turn on the DMP, now that it's ready + pc.printf("Enabling DMP...\r\n"); + mpu.setDMPEnabled(true); + + mpuIntStatus = mpu.getIntStatus(); + + // set our DMP Ready flag so the main loop() function knows it's okay to use it + pc.printf("DMP ready!\r\n"); + dmpReady = true; + + // get expected DMP packet size for later comparison + packetSize = mpu.dmpGetFIFOPacketSize(); + } + else + { + // ERROR! + // 1 = initial memory load failed + // 2 = DMP configuration updates failed + // (if it's going to break, usually the code will be 1) + pc.printf("DMP Initialization failed (code %d )\r\n", devStatus); + while (1); + } + + while (1); + + while (1) + { + mpuIntStatus = mpu.getIntStatus(); + fifoCount = mpu.getFIFOCount(); + + // Check overflow + if ((mpuIntStatus & 0x10) || fifoCount == 1024) + { + // reset so we can continue cleanly + mpu.resetFIFO(); + pc.printf("FIFO overflow!\r\n"); + } + + if (mpuIntStatus & 0x02) + { + // wait for correct available data length, should be a VERY short wait + while (fifoCount < packetSize) + { + fifoCount = mpu.getFIFOCount(); + } + + mpu.getFIFOBytes(fifoBuffer, packetSize); + + fifoCount -= packetSize; + } + + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetEuler(euler, &q); + + for (int i = 0; i < 3; i++) + { + euler[i] *= 180/M_PI; + } + + if (count++ > 20) + { + pc.printf("euler\t%d\t%d\t%d\r\n", euler[0], euler[1], euler[2]); + count = 0; + } + } + +} \ No newline at end of file