I am writing a small abstraction class for the MPU6050 to be used with a scheduler.
I call the update function at 100hr and it seems to work for a while (fro 20sec to 4min) but eventually crashes. I think I have located the point of failure but I cannot understand what is the problem.
this is the update function: (it is based on the DMP example but i got rid of the interrupts)
void update() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
Serial.print(F("a")); // +
// get current FIFO count
fifoCount = mpu.getFIFOCount();
Serial.print(F("b"));
// check for overflow (this should never happen unless our code is too inefficient)
if (fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
overflowCount += 1;
} else if (fifoCount == 0) {
pullEmptyMissCount += 1;
} else if (fifoCount < packetSize) {
pullMissCount += 1;
} else { // in this case the fifo has one or more packets
Serial.print(F("c"));
// if there are more than on packet, will read all of them and keep the newest
lastBuferReadCount = 0;
while (fifoCount >= packetSize)
{
// read a packet from FIFO
Serial.print(F("d")); // ++
mpu.getFIFOBytes(fifoBuffer, packetSize);
lastBuferReadCount += 1;
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
//fifoCount -= mpu.getFIFOCount(); // It is slower than subtracting the packet size, but in case new data comes in it will allow to read it.
}
Serial.print(F("e"));
// Update Quaternion, Euler angles and Yay Pich Roll angles
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
}
float* ypr = get_ypr();
int ofc = overflowCount;
int pm = pullMissCount;
int pem = pullEmptyMissCount;
int rc = lastBuferReadCount;
cout <<"- "<<ypr[0]<<" - "<<ypr[1]<<" - "<<ypr[2]<<" - "<<" - overflow: "<<ofc<<" - empty miss: "<<pem<<" - miss: "<<pm<<" - readCount: "<<rc<< endl;
Serial.println(F(""));
}
