I am using the MPU-6050 for an Arduino Project with the I2Cdev library and have used code that I have found form other sources.
I was wondering if there was any way to speed up the code? I found that to go through the loop its takes about 4200us. After reading the MPU-6050 datasheet I found that the DMP refreshes at a rate of 1000Hz so this 4200us time should not be limited by the MPU. After testing the code, I found that one line in the code takes up 2400us alone.
mpu.getFIFOBytes(fifoBuffer, packetSize);
I am assuming this is where the Arduino gives the MPU its DMP data. Is there anyway to speed this process up? I have attached the full code below.
#include "MPU6050_6Axis_MotionApps20.h"
#define interruptPin 7
volatile byte MPUinterrupt;
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
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
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
MPU6050 mpu;
float roll, pitch, yaw;
float yawLast, yawX;
unsigned long timer;
void setup() {
Serial.begin(115200);
attachInterrupt(digitalPinToInterrupt(interruptPin), [ ] ( ) { MPUinterrupt = 1;}, FALLING); //inline autonomous function "lambda function" sets MPUinterrupt variable to 1 this is a cool trick!
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
mpu.initialize();
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(30); //70
mpu.setYGyroOffset(4); //23
mpu.setZGyroOffset(12); //-55
mpu.setZAccelOffset(1906); // 1688 factory default for my test chip
if (devStatus == 0) {
mpu.setDMPEnabled(true);
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
}
void loop() {
timer = micros();
if (MPUinterrupt){ MPUinterrupt=0; //clear the flag
fifoCount = mpu.getFIFOCount(); //300us
if ((!fifoCount) || (fifoCount % packetSize)) {//36us
mpu.resetFIFO();
} else {
while (fifoCount >= packetSize) {
mpu.getFIFOBytes(fifoBuffer, packetSize);//2400us!!!!!!!!!!
fifoCount -= packetSize;
}
//////////////////////////////////////////////MPU-LOOP-Math/////////////////////////////////////////////////////
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
yawX = ypr[0] * 180 / M_PI;
roll = ypr[1] * 180 / M_PI;
pitch = ypr[2] * 180 / M_PI;
///////////////////////MPU Acclermeter//////////////
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
///////Compounded Yaw Calculations///////
if ( abs(yawX-yawLast) < 11) {
yaw += (yawX-yawLast);
}
yawLast=yawX;
// Serial.print("\tYaw: "); Serial.print(yaw);
// Serial.print("\tRoll: "); Serial.print(roll);
// Serial.print("\tPitch: "); Serial.print(pitch); Serial.print("\t");
//
// Serial.print("aworld\t"); Serial.print(aaWorld.x); //1g = 8192
// Serial.print("\t"); Serial.print(aaWorld.y);
// Serial.print("\t"); Serial.print(aaWorld.z); //set for +- 4g's
// Serial.print("\t");
Serial.println(micros()-timer);
}
}
}
