carlesgel

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("")); }