ported from https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
Fork of MPU6050 by
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?
User | Revision | Line number | New 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 | } |