Skip to content

Instantly share code, notes, and snippets.

@ikr7
Created October 26, 2014 03:46
Show Gist options
  • Save ikr7/45d2d64a69ffddababbf to your computer and use it in GitHub Desktop.
Save ikr7/45d2d64a69ffddababbf to your computer and use it in GitHub Desktop.
#include "MPU6050_6Axis_MotionApps20.h"
Serial pc(USBTX, USBRX);
MPU6050 mpu;
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
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
int main(){
mpu.dmpInitialize();
if(mpu.testConnection()){
pc.printf("MPU6050 test passed. \n");
}else{
pc.printf("MPU6050 test failed. \n");
return 1;
}
while(true){
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
mpu.resetFIFO();
}else if(mpuIntStatus & 0x02){
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
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];
pc.printf("%s\n", teapotPacket);
teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
}
wait(0.3);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment