Chris Seto / Mbed 2 deprecated MPU6050-DMP2

ported from https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

Dependencies:   mbed

Fork of MPU6050 by Shundo Kishi

main.cpp@6:0bb3c17bdaf1, 2014-06-25 (annotated)

Committer:
chris1seto
Date:
Wed Jun 25 19:53:14 2014 +0000
Revision:
6:0bb3c17bdaf1
init

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chris1seto 6:0bb3c17bdaf1 1 #include "mbed.h"
chris1seto 6:0bb3c17bdaf1 2 #include "I2Cdev.h"
chris1seto 6:0bb3c17bdaf1 3 #include "helper_3dmath.h"
chris1seto 6:0bb3c17bdaf1 4 #include "MPU6050_6Axis_MotionApps20.h"
chris1seto 6:0bb3c17bdaf1 5 #include "MPU6050.h"
chris1seto 6:0bb3c17bdaf1 6
chris1seto 6:0bb3c17bdaf1 7
chris1seto 6:0bb3c17bdaf1 8 MPU6050 mpu;
chris1seto 6:0bb3c17bdaf1 9 Serial serOut(D8, D2);
chris1seto 6:0bb3c17bdaf1 10
chris1seto 6:0bb3c17bdaf1 11 // MPU control/status vars
chris1seto 6:0bb3c17bdaf1 12 bool dmpReady = false; // set true if DMP init was successful
chris1seto 6:0bb3c17bdaf1 13 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
chris1seto 6:0bb3c17bdaf1 14 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
chris1seto 6:0bb3c17bdaf1 15 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
chris1seto 6:0bb3c17bdaf1 16 uint16_t fifoCount; // count of all bytes currently in FIFO
chris1seto 6:0bb3c17bdaf1 17 uint8_t fifoBuffer[64]; // FIFO storage buffer
chris1seto 6:0bb3c17bdaf1 18
chris1seto 6:0bb3c17bdaf1 19 // orientation/motion vars
chris1seto 6:0bb3c17bdaf1 20 Quaternion q; // [w, x, y, z] quaternion container
chris1seto 6:0bb3c17bdaf1 21 VectorInt16 aa; // [x, y, z] accel sensor measurements
chris1seto 6:0bb3c17bdaf1 22 VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
chris1seto 6:0bb3c17bdaf1 23 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
chris1seto 6:0bb3c17bdaf1 24 VectorFloat gravity; // [x, y, z] gravity vector
chris1seto 6:0bb3c17bdaf1 25 float euler[3]; // [psi, theta, phi] Euler angle container
chris1seto 6:0bb3c17bdaf1 26 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
chris1seto 6:0bb3c17bdaf1 27
chris1seto 6:0bb3c17bdaf1 28 int count = 0;
chris1seto 6:0bb3c17bdaf1 29
chris1seto 6:0bb3c17bdaf1 30 volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
chris1seto 6:0bb3c17bdaf1 31 void dmpDataReady() {
chris1seto 6:0bb3c17bdaf1 32 mpuInterrupt = true;
chris1seto 6:0bb3c17bdaf1 33 }
chris1seto 6:0bb3c17bdaf1 34
chris1seto 6:0bb3c17bdaf1 35 int main()
chris1seto 6:0bb3c17bdaf1 36 {
chris1seto 6:0bb3c17bdaf1 37 serOut.baud(115200);
chris1seto 6:0bb3c17bdaf1 38 /*
chris1seto 6:0bb3c17bdaf1 39 // Join the bus here
chris1seto 6:0bb3c17bdaf1 40
chris1seto 6:0bb3c17bdaf1 41 // initialize device
chris1seto 6:0bb3c17bdaf1 42 serOut.printf("Initializing I2C devices...\r\n");
chris1seto 6:0bb3c17bdaf1 43 mpu.initialize();
chris1seto 6:0bb3c17bdaf1 44
chris1seto 6:0bb3c17bdaf1 45 // verify connection
chris1seto 6:0bb3c17bdaf1 46 serOut.printf("Testing device connections...\r\n");
chris1seto 6:0bb3c17bdaf1 47 serOut.printf(mpu.testConnection() ? "MPU6050 connection successful\r\n" : "MPU6050 connection failed\r\n");
chris1seto 6:0bb3c17bdaf1 48
chris1seto 6:0bb3c17bdaf1 49 // load and configure the DMP
chris1seto 6:0bb3c17bdaf1 50 serOut.printf("Initializing DMP...\r\n");
chris1seto 6:0bb3c17bdaf1 51 devStatus = mpu.dmpInitialize();
chris1seto 6:0bb3c17bdaf1 52
chris1seto 6:0bb3c17bdaf1 53 // supply your own gyro offsets here, scaled for min sensitivity
chris1seto 6:0bb3c17bdaf1 54 mpu.setXGyroOffset(0);
chris1seto 6:0bb3c17bdaf1 55 mpu.setYGyroOffset(0);
chris1seto 6:0bb3c17bdaf1 56 mpu.setZGyroOffset(0);
chris1seto 6:0bb3c17bdaf1 57 mpu.setZAccelOffset(0); // 1688 factory default for my test chip
chris1seto 6:0bb3c17bdaf1 58
chris1seto 6:0bb3c17bdaf1 59 // make sure it worked (returns 0 if so)
chris1seto 6:0bb3c17bdaf1 60 if (devStatus == 0)
chris1seto 6:0bb3c17bdaf1 61 {
chris1seto 6:0bb3c17bdaf1 62 // turn on the DMP, now that it's ready
chris1seto 6:0bb3c17bdaf1 63 serOut.printf("Enabling DMP...\r\n");
chris1seto 6:0bb3c17bdaf1 64 mpu.setDMPEnabled(true);
chris1seto 6:0bb3c17bdaf1 65
chris1seto 6:0bb3c17bdaf1 66 // enable Arduino interrupt detection
chris1seto 6:0bb3c17bdaf1 67 serOut.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
chris1seto 6:0bb3c17bdaf1 68 //attachInterrupt(0, dmpDataReady, RISING);
chris1seto 6:0bb3c17bdaf1 69 mpuIntStatus = mpu.getIntStatus();
chris1seto 6:0bb3c17bdaf1 70
chris1seto 6:0bb3c17bdaf1 71 // set our DMP Ready flag so the main loop() function knows it's okay to use it
chris1seto 6:0bb3c17bdaf1 72 serOut.printf("DMP ready! Waiting for first interrupt...\r\n");
chris1seto 6:0bb3c17bdaf1 73 dmpReady = true;
chris1seto 6:0bb3c17bdaf1 74
chris1seto 6:0bb3c17bdaf1 75 // get expected DMP packet size for later comparison
chris1seto 6:0bb3c17bdaf1 76 packetSize = mpu.dmpGetFIFOPacketSize();
chris1seto 6:0bb3c17bdaf1 77 }
chris1seto 6:0bb3c17bdaf1 78 else
chris1seto 6:0bb3c17bdaf1 79 {
chris1seto 6:0bb3c17bdaf1 80 // ERROR!
chris1seto 6:0bb3c17bdaf1 81 // 1 = initial memory load failed
chris1seto 6:0bb3c17bdaf1 82 // 2 = DMP configuration updates failed
chris1seto 6:0bb3c17bdaf1 83 // (if it's going to break, usually the code will be 1)
chris1seto 6:0bb3c17bdaf1 84 serOut.printf("DMP Initialization failed (code \r\n");
chris1seto 6:0bb3c17bdaf1 85 serOut.printf("%d", devStatus);
chris1seto 6:0bb3c17bdaf1 86 serOut.printf(")\r\n");
chris1seto 6:0bb3c17bdaf1 87 }
chris1seto 6:0bb3c17bdaf1 88 */
chris1seto 6:0bb3c17bdaf1 89 }