int main()
{
MPU6050 *mpu = new MPU6050();
mpu->setDebug(true);
mpu->reset();
if (mpu->whoAmI())
{
printf("WhoAmI was okay\n");
// i2c bypass enabled
mpu->setBypassEnable(true);
printf("Set and Get BypassEnable to true - %s\n", mpu->getBypassEnable() ? "SUCCESS" : "FAILED");
mpu->setBypassEnable(false);
printf("Set and Get BypassEnable to false - %s\n", !mpu->getBypassEnable() ? "SUCCESS" : "FAILED");
// gyro ranges
mpu->setFullScaleGyroRange(fullScaleGyroRange::FS_GYRO_250DEG_S);
printf("Set and Get FullScaleGyroRange to 250deg/sec - %s\n", (mpu->getFullScaleGyroRange() == fullScaleGyroRange::FS_GYRO_250DEG_S) ? "SUCCESS" : "FAILED");
mpu->setFullScaleGyroRange(fullScaleGyroRange::FS_GYRO_500DEG_S);
printf("Set and Get FullScaleGyroRange to 500deg/sec - %s\n", (mpu->getFullScaleGyroRange() == fullScaleGyroRange::FS_GYRO_500DEG_S) ? "SUCCESS" : "FAILED");
mpu->setFullScaleGyroRange(fullScaleGyroRange::FS_GYRO_1000DEG_S);
printf("Set and Get FullScaleGyroRange to 1000deg/sec - %s\n", (mpu->getFullScaleGyroRange() == fullScaleGyroRange::FS_GYRO_1000DEG_S) ? "SUCCESS" : "FAILED");
mpu->setFullScaleGyroRange(fullScaleGyroRange::FS_GYRO_2000DEG_S);
printf("Set and Get FullScaleGyroRange to 2000deg/sec - %s\n", (mpu->getFullScaleGyroRange() == fullScaleGyroRange::FS_GYRO_2000DEG_S) ? "SUCCESS" : "FAILED");
// accelerometer ranges
mpu->setFullScaleAccRange(fullScaleAccRange::FS_ACCL_2G);
printf("Set and Get FullScaleAccRange to 2G - %s\n", (mpu->getFullScaleAccRange() == fullScaleAccRange::FS_ACCL_2G) ? "SUCCESS" : "FAILED");
mpu->setFullScaleAccRange(fullScaleAccRange::FS_ACCL_4G);
printf("Set and Get FullScaleAccRange to 4G - %s\n", (mpu->getFullScaleAccRange() == fullScaleAccRange::FS_ACCL_4G) ? "SUCCESS" : "FAILED");
mpu->setFullScaleAccRange(fullScaleAccRange::FS_ACCL_8G);
printf("Set and Get FullScaleAccRange to 8G - %s\n", (mpu->getFullScaleAccRange() == fullScaleAccRange::FS_ACCL_8G) ? "SUCCESS" : "FAILED");
mpu->setFullScaleAccRange(fullScaleAccRange::FS_ACCL_16G);
printf("Set and Get FullScaleAccRange to 16G - %s\n", (mpu->getFullScaleAccRange() == fullScaleAccRange::FS_ACCL_16G) ? "SUCCESS" : "FAILED");
return 1;
}
return 0;
}
void loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
Serial.println("fireeer!");
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
// other program behavior stuff here
// .
// .
// .
// if you are really paranoid you can frequently test in between other
// stuff to see if mpuInterrupt is true, and if so, "break;" from the
// while() loop to immediately process the MPU data
// .
// .
// .
// harry: try to reset here
Serial.println("hang? reset!");
mpu.reset();
}
// 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();
Serial.println("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);
Serial.print("quat\t");
Serial.print(q.w);
Serial.print("\t");
Serial.print(q.x);
Serial.print("\t");
Serial.print(q.y);;
Serial.print("\t");
Serial.println(q.z);
quaternionW = q.w;
#endif
#ifdef OUTPUT_READABLE_EULER
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
Serial.print("euler\t");
Serial.print(euler[0] * 180/M_PI);
Serial.print("\t");
Serial.print(euler[1] * 180/M_PI);
Serial.print("\t");
Serial.println(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);
Serial.print("ypr\t");
Serial.print(ypr[0] * 180/M_PI);
Serial.print("\t");
Serial.print(ypr[1] * 180/M_PI);
Serial.print("\t");
Serial.println(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);
Serial.print("areal\t");
Serial.print(aaReal.x);
Serial.print("\t");
Serial.print(aaReal.y);
Serial.print("\t");
Serial.println(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);
Serial.print("aworld\t");
Serial.print(aaWorld.x);
Serial.print("\t");
Serial.print(aaWorld.y);
Serial.print("\t");
Serial.println(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];
Serial.write(teapotPacket, 14);
teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
#endif
// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
}
void imu_reset()
{
mpu.reset();
imu_init();
}
void imu_read()
{
uint8_t count = 3;
// if programming failed, don't try to do anything
if (!dmpReady) return;
#ifdef IRQ_DEBUG
#ifdef __BOARD_YUN__
Console.println(F("DMP is ready!\nAwaiting for IRQ ready flag"));
#else
Serial.println(F("DMP is ready!\nAwaiting for IRQ ready flag"));
#endif
#endif
// wait for MPU interrupt or extra packet(s) available
/* while (!mpuInterrupt && fifoCount < packetSize) {
// Serial.println("Shouldn't get here...");
}
Serial.println("DMP flag OK");
*/
while(!mpuInterrupt && fifoCount < packetSize) ;
// 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();
#ifdef __BOARD_YUN__
Console.println(F("FIFO overflow!"));
#else
Serial.println(F("FIFO overflow!"));
#endif
// 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;
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
#ifdef __USE_ANTILOCK__
if(lastQ == q)
{
if(--count)
{
#ifdef __BOARD_YUN__
Console.println(F("\nLOCKED\n"));
dmpReady = false;
Console.println(F("DMP is stalled, reinitializing..."));
#else
Serial.println(F("\nLOCKED\n"));
dmpReady = false;
Serial.println(F("DMP stalled, reinitializing..."));
#endif
mpu.reset();
if ((devStatus = mpu.dmpInitialize()) == 0)
{
mpu.setDMPEnabled(true);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
else {
#ifdef __BOARD_YUN__
Console.print(F("DMP reinitialization failed (code "));
Console.print(devStatus);
Console.println(F(")"));
#else
Serial.print(F("DMP reinitialization failed (code "));
Serial.print(devStatus);
Serial.println(")");
#endif
while (true) {
//delay(300);
//nilThdSleep(300);
sleep(300);
digitalWrite(SOL_LED, 0);
//delay(300);
//nilThdSleep(300);
sleep(300);
digitalWrite(SOL_LED, 1);
}
}
}
}
#endif
}
else {
#ifdef IRQ_DEBUG
#ifdef __BOARD_YUN__
Console.print(F("OK"));
#else
Serial.print(F("OK "));
#endif
#endif
count = 3;
lastQ = q;
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
mpu.dmpGetGyro(gyro, fifoBuffer);
#ifdef DEBUG
#ifdef __BOARD_YUN__
Console.print(F("ypr gyro\t"));
Console.print(ypr[0] * 180/M_PI);
Console.print(F("\t"));
Console.print(ypr[1] * 180/M_PI);
Console.print(F("\t"));
Console.print(ypr[2] * 180/M_PI);
Console.print(F("\t"));
Console.println(gyro);
#else
Serial.print(F("ypr gyro\t"));
Serial.print(ypr[0] * 180/M_PI);
Serial.print(F("\t"));
Serial.print(ypr[1] * 180/M_PI);
Serial.print(F("\t"));
Serial.print(ypr[2] * 180/M_PI);
Serial.print(F("\t"));
Serial.println(gyro);
#endif
#endif
}
// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(SOL_LED, blinkState);
//delay(1);
//nilThdSleep(1);
sleep(1);
}
