1
Dependencies: ArduinoSerial I2Cdev
Fork of MPU6050 by
main.cpp
- Committer:
- sem40590
- Date:
- 2017-06-15
- Revision:
- 10:d0c43cee874b
- Parent:
- 9:338c5b334fa6
File content as of revision 10:d0c43cee874b:
#include "mbed.h" #include <math.h> DigitalOut leds[] = {(LED1), (LED2),(LED3),(LED4)}; #include "MPU6050_6Axis_MotionApps20.h" // works //#include "MPU6050_9Axis_MotionApps41.h" //#include "MPU6050.h" // not necessary if using MotionApps include file MPU6050 mpu; #ifndef M_PI #define M_PI 3.1415 #endif #define OUTPUT_TEAPOT bool blinkState = false; // 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 // packet structure for InvenSense teapot demo uint8_t teapotPacket[15] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n',0 }; // ================================================================ // === INTERRUPT DETECTION ROUTINE === // ================================================================ volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high void dmpDataReady() { mpuInterrupt = true; } // ================================================================ // === INITIAL SETUP === // ================================================================ int main() { #define D_SDA D14 #define D_SCL D15 I2C i2c(D_SDA, D_SCL); // mbed Interface Hardware definitions DigitalOut myled1(LED1); DigitalOut myled2(LED2); DigitalOut myled3(LED3); DigitalOut heartbeatLED(LED4); #define D_BAUDRATE 115200 Serial pc(USBTX, USBRX); // tx, rx pc.baud(D_BAUDRATE); pc.printf("Initializing I2C devices...\n"); mpu.initialize(); pc.printf("Testing device connections...\n"); bool mpu6050TestResult = mpu.testConnection(); if(mpu6050TestResult){ pc.printf("MPU6050 test passed \n"); } else{ pc.printf("MPU6050 test failed \n"); } // wait for ready pc.printf("\nSend any character to begin DMP programming and demo: "); while(!pc.readable()); pc.getc(); pc.printf("\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(-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...\n"); mpu.setDMPEnabled(true); // enable Arduino interrupt detection pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\n"); // attachInterrupt(0, dmpDataReady, RISING); mpuIntStatus = mpu.getIntStatus(); // set our DMP Ready flag so the main loop() function knows it's okay to use it pc.printf("DMP ready! Waiting for first interrupt...\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 "); pc.printf("%u",devStatus); pc.printf(")\n"); } // ================================================================ // === MAIN PROGRAM LOOP === // ================================================================ while(1) { // if programming failed, don't try to do anything if (!dmpReady) continue; myled2=0; { } wait_us(500); // reset interrupt flag and get INT_STATUS byte mpuInterrupt = false; 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!"); // otherwise, check for DMP data ready interrupt (this should happen frequently) } 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; #ifdef OUTPUT_READABLE_QUATERNION // display quaternion values in easy matrix form: w x y z mpu.dmpGetQuaternion(&q, fifoBuffer); // pc.printf("quat\t"); pc.printf("%f",q.w); pc.printf(","); pc.printf("%f",q.x); pc.printf(","); pc.printf("%f",q.y); pc.printf(","); pc.printf("%f\n",q.z); #endif #ifdef OUTPUT_READABLE_EULER // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetEuler(euler, &q); pc.printf("euler\t"); pc.printf("%f",euler[0] * 180/M_PI); pc.printf("\t"); pc.printf("%f",euler[1] * 180/M_PI); pc.printf("\t"); pc.printf("%f\n",euler[2] * 180/M_PI); #endif #ifdef OUTPUT_READABLE_YAWPITCHROLL // 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); #endif #ifdef OUTPUT_READABLE_REALACCEL // display real acceleration, adjusted to remove gravity mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); pc.printf("areal\t"); pc.printf("%d",aaReal.x); pc.printf("\t"); pc.printf("%d",aaReal.y); pc.printf("\t"); pc.printf("%d\n",aaReal.z); #endif #ifdef OUTPUT_READABLE_WORLDACCEL // display initial world-frame acceleration, adjusted to remove gravity // and rotated based on known orientation from quaternion mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); pc.printf("aworld\t"); pc.printf("%d",aaWorld.x); pc.printf("\t"); pc.printf("%d",aaWorld.y); pc.printf("\t"); pc.printf("%d\n",aaWorld.z); #endif #ifdef OUTPUT_TEAPOT // display quaternion values in InvenSense Teapot demo format: teapotPacket[2] = fifoBuffer[0]; teapotPacket[3] = fifoBuffer[1]; teapotPacket[4] = fifoBuffer[4]; teapotPacket[5] = fifoBuffer[5]; teapotPacket[6] = fifoBuffer[8]; teapotPacket[7] = fifoBuffer[9]; teapotPacket[8] = fifoBuffer[12]; teapotPacket[9] = fifoBuffer[13]; for(int i=0;i<14;i++) { pc.putc(teapotPacket[i]); } // pc.printf("%d",teapotPacket, 14); teapotPacket[11]++; // packetCount, loops at 0xFF on purpose #endif // blink LED to indicate activity blinkState = !blinkState; myled1 = blinkState; } } }