MPU6050_DMP6 example crashing

[opvini]

Hi everyone.
I am using an Arduino Mega, and the MPU6050. I tested the MPU6050_DMP6 example and the Arduino crashing after a while. after extensive testing, I am reading raw data directly with getMotion6().

I posted on http://forum.arduino.cc/index.php?topic=166938.msg2163176#msg2163176

Some one had the same problem?
Thanks a lot!

[opvini]

The line
TWBR = 24;
is correct to my Arduino Mega with 16MHz?

[opvini]

Here is the code, using 2 servos and without serial.

[code]

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif

MPU6050 mpu;

#include <Servo.h>

Servo myservo;
Servo myservo2;
int valAtual = 0;
int valAtual2 = 0;
int lastVal = 0;

#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

// MPU control/status vars
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

// orientation/motion vars
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
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}

// ================================================================
// === INITIAL SETUP ===
// ================================================================

void setup() {

// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    Wire.begin();
    // antes era 24
    TWBR = 12; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
    Fastwire::setup(400, true);
#endif

// initialize device
mpu.initialize();

// load and configure the DMP
devStatus = mpu.dmpInitialize();

// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

// make sure it worked (returns 0 if so)
if (devStatus == 0) {
    // turn on the DMP, now that it's ready
    mpu.setDMPEnabled(true);

    // enable Arduino interrupt detection
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
} 

// configure LED for output
pinMode(LED_PIN, OUTPUT);

myservo.attach(9);
myservo2.attach(10);

}

// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================

void loop() {
// if programming failed, don't try to do anything
while (!dmpReady);

// 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
    // .
    // .
    // .
}    

// 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();

// 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);
        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);
         */           

    valAtual  = 90 - (ypr[1] * 180/M_PI);          
    valAtual2 = 90 - (ypr[2] * 180/M_PI);

    myservo.write(valAtual);
    myservo2.write(valAtual2); 

    // blink LED to indicate activity
    blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);
}

}
[/code]

[wolframalpha]

Same problem here ... I guess the problem is with the buffer since sometimes it ain't able to read the register ! Did you get a solution to it ??

[aborjesson-zz]

I have the same problem. After a while reading the registers, either FIFO Count or Raw acc/gyro values, makes it crash. Any new findings to that problem?

[wolframalpha]

@aborjesson Okay ... There was a problem reading the on-board registers in I2Cdev.cpp,which was annoying and frustrating,huh!! Anyways Fixed it , here's the link https://drive.google.com/file/d/0BwUusXozWAMjVWhMYkZ4NlZwYlk/view?usp=sharing to the modified library. Override the original files and you are good to go. Ping me if it works.

[aborjesson-zz]

@wolframalpha: I diffed your I2CDev.cpp with the latest verison from @jrowberg and there was no code changes. So it did'nt sort out my mystery.

I think my problem is related to that I use the FIFO for DMP values for Yaw, Pitch, Roll and at the same time (while FIFO is getting filled from DMP 100Hz) read raw values from the registers for gyro and accel. Do anyone know if this is doable, to use the DMP/FIFO and at the same time read raw values from the registers ? Cannot find anything related to that in the data sheet.

[opvini]

COOL!!! Now it is working fine!!!! Congrats.

[opvini]

If I use a Servo, the program is crashing yet... I dont know if it is interference...
I am use a simple raw reading example. With no servos, it works fine.
What do you think?

In some part of the library (I2C or MPU6050) the code stay on a infinity loop, becouse the arduino is freezing. I believe this is occurring in some part of the code that handles incoming data. When it happens an interference by the servo or motor, the arduino freezing...

[ranqingfa]

@opvini Maybe the power supply is not enough！

[opvini]

@ranqingfa I think can be a bug on wire arduino library. I am using a PC power supply (250W) and just 2 servos.

[aborjesson-zz]

@opvini : Those MPU-9150s are mysterious. Seem to be very sensitive to noise of all sort. I bought a new one, resoldered my whole sketch onto a pcb hole bread board. Now it seems to work rock solid even if I use the DMP. I do not use interrupts though, havent had the time to check that.

It is powered by a separate power supply, soldered onto the board.

[opvini]

Hi guys! I solved the problem:

1. I changed the pull-ups resistors to 2k2
2. I put a ceramic capacitor between VCC and GND (104)
3. The main thing for me: I put a Ferrite Bead on my PWM line

Now it works very well and not freezing :D
Thanks a lot for everybody!

[opvini]

Other important consideration: when I use some delay's on my firmware, it crashes. Then I am using micros() to count time and execute the correct actions at a correct time.

[109JB]

I am just learning arduino and C/C++ programming so I don't know if this will help, but I also found that the MPU6050 DMP demo program would crash. I was able to get it to work reliably by eliminating the interrupt detection for the FIFO buffer and instead just poll the MPU6050 for the buffer state. Doing this eliminated the problems.I was having.

Here is my modified Arduino code. I changed the extension from .ino to .txt in order to upload. Just change it back and it will work fine.

I noted where I commented out portions of the code.

MPU6050_DMP6_mod.txt

[109JB]

I forgot to mention that I alsi had to unhook the interrupt wire to get it to work properly.

[jaundice]

I didn't have issues with the 6050 but had trouble with fifo overruns on 9150. Switching from wire to fastwire fixed it for me.

[opvini]

@jrowberg @wolframalpha After I updated the library, the DMP mode is working fine, but now, the raw values example firmware is crashing o.O

[prvnkmr96]

What is the use of INT pin on MPU6050. Is it give interrupt only when the MPU in the motion or something else.

[IshankAgarwal]

I am also using MPU6050 with Arduino Mega and motors with motor driver-Cytron MDDS-10A.
whenever I turn motors ON , Arduino Serial freezes up. But without motors connected Mpu6050 works correct . If this is due to interference from motors ,
@opvini Then approx. what parameters of ferrite bead and ceramic capacitor should I use to decrease the noise from motors .Motors I am using are 12V DC 350rpm encoder motors (IG42E).

I tried with internal and external pullups of 2k2 both but still the same problem .