sem kleyman / MPU6050

1

Dependencies:   ArduinoSerial I2Cdev

Fork of MPU6050 by Shundo Kishi

main.cpp@10:d0c43cee874b, 2017-06-15 (annotated)

Committer:
sem40590
Date:
Thu Jun 15 17:19:14 2017 +0000
Revision:
10:d0c43cee874b
Parent:
9:338c5b334fa6 
1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sem40590 9:338c5b334fa6 1 #include "mbed.h"
sem40590 9:338c5b334fa6 2 #include <math.h>
sem40590 9:338c5b334fa6 3 DigitalOut leds[] = {(LED1), (LED2),(LED3),(LED4)};
sem40590 9:338c5b334fa6 4
sem40590 9:338c5b334fa6 5
sem40590 9:338c5b334fa6 6 #include "MPU6050_6Axis_MotionApps20.h" // works
sem40590 9:338c5b334fa6 7 //#include "MPU6050_9Axis_MotionApps41.h"
sem40590 9:338c5b334fa6 8
sem40590 9:338c5b334fa6 9 //#include "MPU6050.h" // not necessary if using MotionApps include file
sem40590 9:338c5b334fa6 10
sem40590 9:338c5b334fa6 11 MPU6050 mpu;
sem40590 10:d0c43cee874b 12
sem40590 10:d0c43cee874b 13
sem40590 9:338c5b334fa6 14 #ifndef M_PI
sem40590 9:338c5b334fa6 15 #define M_PI 3.1415
sem40590 9:338c5b334fa6 16 #endif
sem40590 10:d0c43cee874b 17
sem40590 9:338c5b334fa6 18 #define OUTPUT_TEAPOT
sem40590 9:338c5b334fa6 19
sem40590 9:338c5b334fa6 20
sem40590 9:338c5b334fa6 21
sem40590 9:338c5b334fa6 22 bool blinkState = false;
sem40590 9:338c5b334fa6 23
sem40590 9:338c5b334fa6 24 // MPU control/status vars
sem40590 9:338c5b334fa6 25 bool dmpReady = false; // set true if DMP init was successful
sem40590 9:338c5b334fa6 26 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
sem40590 9:338c5b334fa6 27 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
sem40590 9:338c5b334fa6 28 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
sem40590 9:338c5b334fa6 29 uint16_t fifoCount; // count of all bytes currently in FIFO
sem40590 9:338c5b334fa6 30 uint8_t fifoBuffer[64]; // FIFO storage buffer
sem40590 9:338c5b334fa6 31
sem40590 9:338c5b334fa6 32 // orientation/motion vars
sem40590 9:338c5b334fa6 33 Quaternion q; // [w, x, y, z] quaternion container
sem40590 9:338c5b334fa6 34 VectorInt16 aa; // [x, y, z] accel sensor measurements
sem40590 9:338c5b334fa6 35 VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
sem40590 9:338c5b334fa6 36 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
sem40590 9:338c5b334fa6 37 VectorFloat gravity; // [x, y, z] gravity vector
sem40590 9:338c5b334fa6 38 float euler[3]; // [psi, theta, phi] Euler angle container
sem40590 9:338c5b334fa6 39 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
sem40590 9:338c5b334fa6 40
sem40590 9:338c5b334fa6 41 // packet structure for InvenSense teapot demo
sem40590 9:338c5b334fa6 42 uint8_t teapotPacket[15] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n',0 };
sem40590 9:338c5b334fa6 43
sem40590 9:338c5b334fa6 44 // ================================================================
sem40590 9:338c5b334fa6 45 // === INTERRUPT DETECTION ROUTINE ===
sem40590 9:338c5b334fa6 46 // ================================================================
sem40590 9:338c5b334fa6 47
sem40590 9:338c5b334fa6 48 volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
sem40590 9:338c5b334fa6 49 void dmpDataReady() {
sem40590 9:338c5b334fa6 50 mpuInterrupt = true;
sem40590 9:338c5b334fa6 51 }
sem40590 9:338c5b334fa6 52
sem40590 9:338c5b334fa6 53 // ================================================================
sem40590 9:338c5b334fa6 54 // === INITIAL SETUP ===
sem40590 9:338c5b334fa6 55 // ================================================================
sem40590 9:338c5b334fa6 56
sem40590 9:338c5b334fa6 57 int main()
sem40590 9:338c5b334fa6 58 {
sem40590 9:338c5b334fa6 59
sem40590 9:338c5b334fa6 60 #define D_SDA D14
sem40590 9:338c5b334fa6 61 #define D_SCL D15
sem40590 9:338c5b334fa6 62 I2C i2c(D_SDA, D_SCL);
sem40590 9:338c5b334fa6 63
sem40590 9:338c5b334fa6 64
sem40590 9:338c5b334fa6 65 // mbed Interface Hardware definitions
sem40590 9:338c5b334fa6 66 DigitalOut myled1(LED1);
sem40590 9:338c5b334fa6 67 DigitalOut myled2(LED2);
sem40590 9:338c5b334fa6 68 DigitalOut myled3(LED3);
sem40590 9:338c5b334fa6 69 DigitalOut heartbeatLED(LED4);
sem40590 9:338c5b334fa6 70
sem40590 9:338c5b334fa6 71 #define D_BAUDRATE 115200
sem40590 9:338c5b334fa6 72
sem40590 10:d0c43cee874b 73
sem40590 9:338c5b334fa6 74 Serial pc(USBTX, USBRX); // tx, rx
sem40590 9:338c5b334fa6 75
sem40590 9:338c5b334fa6 76 pc.baud(D_BAUDRATE);
sem40590 10:d0c43cee874b 77
sem40590 9:338c5b334fa6 78 pc.printf("Initializing I2C devices...\n");
sem40590 9:338c5b334fa6 79 mpu.initialize();
sem40590 9:338c5b334fa6 80
sem40590 9:338c5b334fa6 81 pc.printf("Testing device connections...\n");
sem40590 9:338c5b334fa6 82
sem40590 9:338c5b334fa6 83 bool mpu6050TestResult = mpu.testConnection();
sem40590 9:338c5b334fa6 84 if(mpu6050TestResult){
sem40590 9:338c5b334fa6 85 pc.printf("MPU6050 test passed \n");
sem40590 9:338c5b334fa6 86 } else{
sem40590 9:338c5b334fa6 87 pc.printf("MPU6050 test failed \n");
sem40590 9:338c5b334fa6 88 }
sem40590 9:338c5b334fa6 89
sem40590 9:338c5b334fa6 90 // wait for ready
sem40590 9:338c5b334fa6 91 pc.printf("\nSend any character to begin DMP programming and demo: ");
sem40590 9:338c5b334fa6 92
sem40590 9:338c5b334fa6 93 while(!pc.readable());
sem40590 9:338c5b334fa6 94 pc.getc();
sem40590 9:338c5b334fa6 95 pc.printf("\n");
sem40590 9:338c5b334fa6 96
sem40590 9:338c5b334fa6 97 // load and configure the DMP
sem40590 9:338c5b334fa6 98 pc.printf("Initializing DMP...\n");
sem40590 9:338c5b334fa6 99 devStatus = mpu.dmpInitialize();
sem40590 9:338c5b334fa6 100
sem40590 9:338c5b334fa6 101 // supply your own gyro offsets here, scaled for min sensitivity
sem40590 9:338c5b334fa6 102 mpu.setXGyroOffset(-61);
sem40590 9:338c5b334fa6 103 mpu.setYGyroOffset(-127);
sem40590 9:338c5b334fa6 104 mpu.setZGyroOffset(19);
sem40590 9:338c5b334fa6 105 mpu.setZAccelOffset(16282); // 1688 factory default for my test chip
sem40590 9:338c5b334fa6 106
sem40590 9:338c5b334fa6 107 // make sure it worked (returns 0 if so)
sem40590 9:338c5b334fa6 108 if (devStatus == 0) {
sem40590 9:338c5b334fa6 109 // turn on the DMP, now that it's ready
sem40590 9:338c5b334fa6 110 pc.printf("Enabling DMP...\n");
sem40590 9:338c5b334fa6 111 mpu.setDMPEnabled(true);
sem40590 9:338c5b334fa6 112
sem40590 9:338c5b334fa6 113 // enable Arduino interrupt detection
sem40590 9:338c5b334fa6 114 pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\n");
sem40590 9:338c5b334fa6 115 // attachInterrupt(0, dmpDataReady, RISING);
sem40590 9:338c5b334fa6 116 mpuIntStatus = mpu.getIntStatus();
sem40590 9:338c5b334fa6 117
sem40590 9:338c5b334fa6 118 // set our DMP Ready flag so the main loop() function knows it's okay to use it
sem40590 9:338c5b334fa6 119 pc.printf("DMP ready! Waiting for first interrupt...\n");
sem40590 9:338c5b334fa6 120 dmpReady = true;
sem40590 9:338c5b334fa6 121
sem40590 9:338c5b334fa6 122 // get expected DMP packet size for later comparison
sem40590 9:338c5b334fa6 123 packetSize = mpu.dmpGetFIFOPacketSize();
sem40590 9:338c5b334fa6 124 } else {
sem40590 9:338c5b334fa6 125 // ERROR!
sem40590 9:338c5b334fa6 126 // 1 = initial memory load failed
sem40590 9:338c5b334fa6 127 // 2 = DMP configuration updates failed
sem40590 9:338c5b334fa6 128 // (if it's going to break, usually the code will be 1)
sem40590 9:338c5b334fa6 129 pc.printf("DMP Initialization failed (code ");
sem40590 9:338c5b334fa6 130 pc.printf("%u",devStatus);
sem40590 9:338c5b334fa6 131 pc.printf(")\n");
sem40590 9:338c5b334fa6 132 }
sem40590 9:338c5b334fa6 133
sem40590 9:338c5b334fa6 134
sem40590 9:338c5b334fa6 135 // ================================================================
sem40590 9:338c5b334fa6 136 // === MAIN PROGRAM LOOP ===
sem40590 9:338c5b334fa6 137 // ================================================================
sem40590 9:338c5b334fa6 138
sem40590 9:338c5b334fa6 139 while(1)
sem40590 9:338c5b334fa6 140 {
sem40590 9:338c5b334fa6 141 // if programming failed, don't try to do anything
sem40590 9:338c5b334fa6 142 if (!dmpReady) continue;
sem40590 9:338c5b334fa6 143 myled2=0;
sem40590 9:338c5b334fa6 144
sem40590 9:338c5b334fa6 145 {
sem40590 10:d0c43cee874b 146
sem40590 9:338c5b334fa6 147 }
sem40590 9:338c5b334fa6 148 wait_us(500);
sem40590 9:338c5b334fa6 149
sem40590 9:338c5b334fa6 150 // reset interrupt flag and get INT_STATUS byte
sem40590 9:338c5b334fa6 151 mpuInterrupt = false;
sem40590 9:338c5b334fa6 152 mpuIntStatus = mpu.getIntStatus();
sem40590 9:338c5b334fa6 153
sem40590 9:338c5b334fa6 154 // get current FIFO count
sem40590 9:338c5b334fa6 155 fifoCount = mpu.getFIFOCount();
sem40590 9:338c5b334fa6 156
sem40590 9:338c5b334fa6 157 // check for overflow (this should never happen unless our code is too inefficient)
sem40590 9:338c5b334fa6 158 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
sem40590 9:338c5b334fa6 159 // reset so we can continue cleanly
sem40590 9:338c5b334fa6 160 mpu.resetFIFO();
sem40590 9:338c5b334fa6 161 pc.printf("FIFO overflow!");
sem40590 9:338c5b334fa6 162
sem40590 9:338c5b334fa6 163 // otherwise, check for DMP data ready interrupt (this should happen frequently)
sem40590 9:338c5b334fa6 164 } else if (mpuIntStatus & 0x02) {
sem40590 9:338c5b334fa6 165 // wait for correct available data length, should be a VERY short wait
sem40590 9:338c5b334fa6 166 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
sem40590 9:338c5b334fa6 167
sem40590 9:338c5b334fa6 168 // read a packet from FIFO
sem40590 9:338c5b334fa6 169 mpu.getFIFOBytes(fifoBuffer, packetSize);
sem40590 9:338c5b334fa6 170
sem40590 9:338c5b334fa6 171 // track FIFO count here in case there is > 1 packet available
sem40590 9:338c5b334fa6 172 // (this lets us immediately read more without waiting for an interrupt)
sem40590 9:338c5b334fa6 173 fifoCount -= packetSize;
sem40590 9:338c5b334fa6 174
sem40590 9:338c5b334fa6 175 #ifdef OUTPUT_READABLE_QUATERNION
sem40590 9:338c5b334fa6 176 // display quaternion values in easy matrix form: w x y z
sem40590 9:338c5b334fa6 177 mpu.dmpGetQuaternion(&q, fifoBuffer);
sem40590 9:338c5b334fa6 178 // pc.printf("quat\t");
sem40590 9:338c5b334fa6 179 pc.printf("%f",q.w);
sem40590 9:338c5b334fa6 180 pc.printf(",");
sem40590 9:338c5b334fa6 181 pc.printf("%f",q.x);
sem40590 9:338c5b334fa6 182 pc.printf(",");
sem40590 9:338c5b334fa6 183 pc.printf("%f",q.y);
sem40590 9:338c5b334fa6 184 pc.printf(",");
sem40590 9:338c5b334fa6 185 pc.printf("%f\n",q.z);
sem40590 9:338c5b334fa6 186 #endif
sem40590 9:338c5b334fa6 187
sem40590 9:338c5b334fa6 188 #ifdef OUTPUT_READABLE_EULER
sem40590 9:338c5b334fa6 189 // display Euler angles in degrees
sem40590 9:338c5b334fa6 190 mpu.dmpGetQuaternion(&q, fifoBuffer);
sem40590 9:338c5b334fa6 191 mpu.dmpGetEuler(euler, &q);
sem40590 9:338c5b334fa6 192 pc.printf("euler\t");
sem40590 9:338c5b334fa6 193 pc.printf("%f",euler[0] * 180/M_PI);
sem40590 9:338c5b334fa6 194 pc.printf("\t");
sem40590 9:338c5b334fa6 195 pc.printf("%f",euler[1] * 180/M_PI);
sem40590 9:338c5b334fa6 196 pc.printf("\t");
sem40590 9:338c5b334fa6 197 pc.printf("%f\n",euler[2] * 180/M_PI);
sem40590 9:338c5b334fa6 198 #endif
sem40590 9:338c5b334fa6 199
sem40590 9:338c5b334fa6 200 #ifdef OUTPUT_READABLE_YAWPITCHROLL
sem40590 9:338c5b334fa6 201 // display Euler angles in degrees
sem40590 9:338c5b334fa6 202 mpu.dmpGetQuaternion(&q, fifoBuffer);
sem40590 9:338c5b334fa6 203 mpu.dmpGetGravity(&gravity, &q);
sem40590 9:338c5b334fa6 204 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
sem40590 9:338c5b334fa6 205 pc.printf("ypr\t");
sem40590 9:338c5b334fa6 206 pc.printf("%f3.2",ypr[0] * 180/M_PI);
sem40590 9:338c5b334fa6 207 pc.printf("\t");
sem40590 9:338c5b334fa6 208 pc.printf("%f3.2",ypr[1] * 180/M_PI);
sem40590 9:338c5b334fa6 209 pc.printf("\t");
sem40590 9:338c5b334fa6 210 pc.printf("%f3.2\n",ypr[2] * 180/M_PI);
sem40590 9:338c5b334fa6 211 #endif
sem40590 9:338c5b334fa6 212
sem40590 9:338c5b334fa6 213 #ifdef OUTPUT_READABLE_REALACCEL
sem40590 9:338c5b334fa6 214 // display real acceleration, adjusted to remove gravity
sem40590 9:338c5b334fa6 215 mpu.dmpGetQuaternion(&q, fifoBuffer);
sem40590 9:338c5b334fa6 216 mpu.dmpGetAccel(&aa, fifoBuffer);
sem40590 9:338c5b334fa6 217 mpu.dmpGetGravity(&gravity, &q);
sem40590 9:338c5b334fa6 218 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
sem40590 9:338c5b334fa6 219 pc.printf("areal\t");
sem40590 9:338c5b334fa6 220 pc.printf("%d",aaReal.x);
sem40590 9:338c5b334fa6 221 pc.printf("\t");
sem40590 9:338c5b334fa6 222 pc.printf("%d",aaReal.y);
sem40590 9:338c5b334fa6 223 pc.printf("\t");
sem40590 9:338c5b334fa6 224 pc.printf("%d\n",aaReal.z);
sem40590 9:338c5b334fa6 225 #endif
sem40590 9:338c5b334fa6 226
sem40590 9:338c5b334fa6 227 #ifdef OUTPUT_READABLE_WORLDACCEL
sem40590 9:338c5b334fa6 228 // display initial world-frame acceleration, adjusted to remove gravity
sem40590 9:338c5b334fa6 229 // and rotated based on known orientation from quaternion
sem40590 9:338c5b334fa6 230 mpu.dmpGetQuaternion(&q, fifoBuffer);
sem40590 9:338c5b334fa6 231 mpu.dmpGetAccel(&aa, fifoBuffer);
sem40590 9:338c5b334fa6 232 mpu.dmpGetGravity(&gravity, &q);
sem40590 9:338c5b334fa6 233 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
sem40590 9:338c5b334fa6 234 mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
sem40590 9:338c5b334fa6 235 pc.printf("aworld\t");
sem40590 9:338c5b334fa6 236 pc.printf("%d",aaWorld.x);
sem40590 9:338c5b334fa6 237 pc.printf("\t");
sem40590 9:338c5b334fa6 238 pc.printf("%d",aaWorld.y);
sem40590 9:338c5b334fa6 239 pc.printf("\t");
sem40590 9:338c5b334fa6 240 pc.printf("%d\n",aaWorld.z);
sem40590 9:338c5b334fa6 241 #endif
sem40590 9:338c5b334fa6 242 #ifdef OUTPUT_TEAPOT
sem40590 9:338c5b334fa6 243 // display quaternion values in InvenSense Teapot demo format:
sem40590 9:338c5b334fa6 244 teapotPacket[2] = fifoBuffer[0];
sem40590 9:338c5b334fa6 245 teapotPacket[3] = fifoBuffer[1];
sem40590 9:338c5b334fa6 246 teapotPacket[4] = fifoBuffer[4];
sem40590 9:338c5b334fa6 247 teapotPacket[5] = fifoBuffer[5];
sem40590 9:338c5b334fa6 248 teapotPacket[6] = fifoBuffer[8];
sem40590 9:338c5b334fa6 249 teapotPacket[7] = fifoBuffer[9];
sem40590 9:338c5b334fa6 250 teapotPacket[8] = fifoBuffer[12];
sem40590 9:338c5b334fa6 251 teapotPacket[9] = fifoBuffer[13];
sem40590 9:338c5b334fa6 252
sem40590 9:338c5b334fa6 253 for(int i=0;i<14;i++)
sem40590 9:338c5b334fa6 254 {
sem40590 9:338c5b334fa6 255 pc.putc(teapotPacket[i]);
sem40590 9:338c5b334fa6 256 }
sem40590 9:338c5b334fa6 257 // pc.printf("%d",teapotPacket, 14);
sem40590 9:338c5b334fa6 258 teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
sem40590 9:338c5b334fa6 259 #endif
sem40590 9:338c5b334fa6 260
sem40590 9:338c5b334fa6 261 // blink LED to indicate activity
sem40590 9:338c5b334fa6 262 blinkState = !blinkState;
sem40590 9:338c5b334fa6 263 myled1 = blinkState;
sem40590 9:338c5b334fa6 264 }
sem40590 9:338c5b334fa6 265 }
sem40590 10:d0c43cee874b 266 }