Chris Seto
/
MPU6050-DMP-4
MPU6050 issues
Fork of MPU6050 by
Diff: main.cpp
- Revision:
- 6:6300d9561dfd
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Jul 01 16:56:00 2014 +0000 @@ -0,0 +1,123 @@ +#include "mbed.h" +#include "I2Cdev.h" +#include "MPU6050_6Axis_MotionApps20.h" +#include "shared.h" + +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 + +volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high +void dmpDataReady() +{ + mpuInterrupt = true; +} + +#ifndef M_PI +#define M_PI 3.1415 +#endif + +I2C i2c(); + +int main() +{ + pc.baud(115200); + + mpu.initialize(); + + // verify connection + pc.printf("Testing device connections...\r\n"); + + bool mpu6050TestResult = mpu.testConnection(); + if(mpu6050TestResult) + { + pc.printf("MPU6050 test passed \r\n"); + } + else + { + pc.printf("MPU6050 test failed \r\n"); + } + + // load and configure the DMP + pc.printf("Initializing DMP...\n"); + devStatus = mpu.dmpInitialize(); + + // supply your own gyro offsets here, scaled for min sensitivity + mpu.setXGyroOffset(0); + mpu.setYGyroOffset(0); + mpu.setZGyroOffset(0); + mpu.setZAccelOffset(16282); // 1688 factory default for my test chip + + if (devStatus==0) + { + pc.printf("OK!\r\n"); + mpu.setDMPEnabled(true); + + mpuIntStatus = mpu.getIntStatus(); + + packetSize = mpu.dmpGetFIFOPacketSize(); + } + else + { + pc.printf("Failed with code %d\r\n", devStatus); + while(1); + } + + // Main prog loop + while (1) + { + wait_us(500); + + mpuIntStatus = mpu.getIntStatus(); + + // get current FIFO count + fifoCount = mpu.getFIFOCount(); + + // check for overflow (this should never happen unless our code is too inefficient) + if ((mpuIntStatus & 0x10) || fifoCount == 1024) + { + // reset so we can continue cleanly + mpu.resetFIFO(); + //pc.printf("FIFO overflow!"); + } + else if (mpuIntStatus & 0x02) + { + // wait for correct available data length, should be a VERY short wait + while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); + + // read a packet from FIFO + mpu.getFIFOBytes(fifoBuffer, packetSize); + + // track FIFO count here in case there is > 1 packet available + // (this lets us immediately read more without waiting for an interrupt) + fifoCount -= packetSize; + + // display Euler angles in degrees + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); + /*pc.printf("ypr\t"); + pc.printf("%f3.2",ypr[0] * 180/M_PI); + pc.printf("\t"); + pc.printf("%f3.2",ypr[1] * 180/M_PI); + pc.printf("\t"); + pc.printf("%f3.2\n",ypr[2] * 180/M_PI);*/ + + } + } +} \ No newline at end of file