MPU6050 Freeze, Possible Solution

[navidre91]

Hi,

I have been trying to figure out why sometimes MPU6050 freezes when I'm using this library. I tracked it down to this part of the code.

int8_t MPU6050_Base::GetCurrentFIFOPacket(uint8_t *data, uint8_t length) { // overflow proof
     Serial.println("a");
     int16_t fifoC;
     // This section of code is for when we allowed more than 1 packet to be acquired
     uint32_t BreakTimer = micros();
     bool packetReceived = false;
     do {
         if ((fifoC = getFIFOCount())  > length) {
             Serial.println("b");
             Serial.println(fifoC);
             if (fifoC > 200) { // if you waited to get the FIFO buffer to > 200 bytes it will take longer to get the last packet in the FIFO Buffer than it will take to  reset the buffer and wait for the next to arrive
                 Serial.println("b1");
                 resetFIFO(); // Fixes any overflow corruption
                 Serial.println("b2");
                 fifoC = 0;
                 while (!(fifoC = getFIFOCount()) && ((micros() - BreakTimer) <= (getFIFOTimeout()))); // Get Next New Packet
                 Serial.println("b3");
                 } else { //We have more than 1 packet but less than 200 bytes of data in the FIFO Buffer
                 Serial.println("b4");
                 uint8_t Trash[I2CDEVLIB_WIRE_BUFFER_LENGTH];
                 Serial.println("b5");
                 while ((fifoC = getFIFOCount()) > length) {  // Test each time just in case the MPU is writing to the FIFO Buffer
                     Serial.println("b6");
                     fifoC = fifoC - length; // Save the last packet
                     uint16_t  RemoveBytes;
                     while (fifoC) { // fifo count will reach zero so this is safe
                         Serial.println("b7");
                         RemoveBytes = (fifoC < I2CDEVLIB_WIRE_BUFFER_LENGTH) ? fifoC : I2CDEVLIB_WIRE_BUFFER_LENGTH; // Buffer Length is different than the packet length this will efficiently clear the buffer
                         getFIFOBytes(Trash, (uint8_t)RemoveBytes);
                         Serial.println("b8");
                         fifoC -= RemoveBytes;
                     }
                     Serial.println("b9");
                 }
                 Serial.println("b10");
             }
         }
         if (!fifoC) return 0; // Called too early no data or we timed out after FIFO Reset
         // We have 1 packet
         packetReceived = fifoC == length;
         if (!packetReceived && (micros() - BreakTimer) > (getFIFOTimeout())) return 0;
     } while (!packetReceived);
     getFIFOBytes(data, length); //Get 1 packet
     return 1;
}

The code keeps looping between b6,b7, and b8 and after a while, it keeps looping in b7 and b8. When the inner while loop is called (the case where fifoC < 200)), the process takes too long and the buffer keeps getting filled faster than it is being read. So, it keeps getting stuck in while(fifoC = getFIFOCount()) > length).
Also by not reading the packages and only resetting the buffer, what happens is that the code keeps returning 0. That's because it can get stuck in a state that the buffer fills up so it has to be reset and there won't be any packages, after returning 0 and next time calling this function, the buffer is full again. :)

[ZHomeSlice]

@navidre91

The code keeps looping between b6,b7, and b8 and after a while, it keeps looping in b7 and b8.

For understanding Sake :) we will go through the process so I can understand what is happening and what needs to change.
We have >= 200 bytes in the FIFO buffer.
@ "b1" we have determined that we filled the FIFO buffer too much to empty it fast enough to catch with the latest packet. The process will take longer than 10ms it takes to get a new packet so we resetFIFO(); // Fixes any overflow corruption reset the FIFO buffer to Zero and set ourselves in a "blocking" While loop until the next packet appears which will take < 10 ms so no harm done.
ELSE
you currently have < 200 bytes of FIFO data we know that the FIFO buffer corruption free and we should be able to catch up before the next packet arrives. @ position "b4"
while ((fifoC = getFIFOCount()) > length) {
and
fifoC = fifoC - length; // Save the last packet
shrinks the buffer down to 1 packet;
while (fifoC) { // FIFO count will reach zero so this is safe
This will remove the bytes from the FIFO buffer using the largest i2c request possible currently 32 bytes for the atmega328p.
If a new packet arrives while we are cleaning the buffer we have 2 packets and need to loop back to the while ((fifoC = getFIFOCount()) > length) { to fix the newly arrive packet.

we depart from the While loops and get the last packet before leaving.

So I ran a test

in my loop() I Delayed "delay(77);" for 77 Milliseconds
this filled the FIFO buffer to a point just less than 200 bytes
Fifo Buffer Size 196 Bytes
It took 8.916 milliseconds to transfer the 196 Bytes across the i2c Bus
packet_length = 28;
196/28 = 7 packets available to get
8.916 / 7 =

Each packet takes 1.273 milliseconds to retrieve
Each packet takes 10 milliseconds to create

your Serial.println("b6"); takes an additional 0.2 milliseconds to execute
if the packet is filled as shown above
8.9ms to empty the buffer down to 1 packet
plus 0.2 (to print "b6" )
plus another 2.4ms to execute 6 loops while emptying the buffer 32 bytes at a time while printing "b7" and "b8"
8.9+.2+2.4 = 11.5
and so we check the packet size and have 2 packets equalling equaling 56 bytes
while ((fifoC = getFIFOCount()) > length) {

this will execute with your print statements in (1.27 + .2 + .4) = 1.8 milliseconds
and we will have the latest packet even with your print statements included.

So when you say:
When the inner while loop is called (the case where fifoC < 200)), the process takes too long and the buffer keeps getting filled faster than it is being read. So, it keeps getting stuck in while(fifoC = getFIFOCount()) > length).
The buffer empties packets at a rate of 1.273ms per packet and the max packets that can fit in < 200 bytes is 7
Filling the butter takes 10ms per packet. So something else is affecting the rate you are emptying the buffer.

Also by not reading the packages and only resetting the buffer, what happens is that the code keeps returning 0. That's because it can get stuck in a state that the buffer fills up so it has to be reset and there won't be any packages, after returning 0 and next time calling this function, the buffer is full again. :)"

this is where we literally wait for that next packet because our code took longer than 77ms to get to here! an extra 10ms won't hurt anything if we tried to empty the buffer one byte at a time it would take longer than getting the next packet which would take no more than 10ms to arrive.

             if (fifoC > 200) {
 // if you waited more than 77ms allowing the FIFO buffer to become > 200 bytes it will take longer to get the last packet in the FIFO Buffer than it will take to  reset the buffer and wait for the next to arrive
//just tested this and it takes 8.916ms to empty the buffer of 7 packets
//also the buffer could have overflowed and corrupted the data so let's reset the buffer
                 resetFIFO(); // Fixes any overflow corruption 
//we have no packets
                 fifoC = 0;
// since the program is expecting a packet we will just wait here for the next packet.
                 while (!(fifoC = getFIFOCount()) && ((micros() - BreakTimer) <= (getFIFOTimeout()))); // Get Next New Packet
// we have 1 packet because fifoC is greater than 0 and we havent timed out.
             } else { //We have more than 1 packet but less than 200 bytes of data in the FIFO Buffer
// where your described problem is 

Conclusion
I've run several tests including some now, to confirm this routine and have determined that it solves all the problems I've faced in the past.

If you would like to see the process in coming up with this routing, see the lengthy thread here: #479
Initially, we were trying to combat the FIFO overflow but realized it takes longer to empty the buffer than it does to reset it fixing the overflow, and waiting patiently for the next packet to arrive.

Hope this helps.

Z

[navidre91]

Thank you so much Z!

So my thoughts around creating a packet and retrieving it and the timings around it is that the inner loop has a time-based robustness control and there is no feedback loop to take care of the robustness of the inner while algorithm.
So I think that maybe because I have other members on the I2C bus, that could be a factor slowing that the process of emptying the buffer.
After posting my comment I figure out that the last part of my statement was trivially not correct.

I didn't quite get what code are you referring to that solved your problems.
One other thing that I noticed is that when I call getFIFOCount() several times, it slows that the code significantly. Did you experience the same thing?

[ZHomeSlice]

Yes, getFIFOCount() does take up valuable time.
how often do you need to get the data from the MPU6050? I am assuming less often than 10 ms?

Z

[navidre91]

Yes, 10ms sounds reasonable.

[ZHomeSlice]

I asked if less is reasonable because you could decrease the rate at which the FIFO buffer fills allowing you to check every 100ms or longer.
If you only need this data every second then you could have the FIFO get a packet every second allowing the i2c bus to be kept free for other communications.

While I have this feature in my Simple_MPU6050 library it shouldn't be too difficult to modify Jeffs to have the same features.

[Vedant-Joshi-trying-hard]

@jrowberg

Guyz I think I have found a solution here: https://www.youtube.com/watch?v=n5jQXuhLJtU

Just add these lines at the end of the 'setup' function,

Wire.setWireTimeout(3000, true) ;
int wireTimeoutCount = 0;
Wire.clearWireTimeoutFlag();

My code is not frozen till now by using this, tried 10 times by commenting these lines and uncommenting it again....belive me it is not freezing now....Please check and let me know if it works for you as well