MPU 6050 crashes with my sketch

Hi, I need help in understanding why the MPU 6050 cannot display the data on serial monitor. I had tried compiling the MPU 6050 dmp code and it was successful in giving me the data. However, when I tried compiling with my sketch it couldn't work. It only work when i comment out the rest of the code in the void loop except for the digitalWrite. Thanks

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <Servo.h>
//#include <SoftwareSerial.h>
//SoftwareSerial XBee(2, 3);

Servo esc1;
Servo esc2;
Servo esc3;
Servo esc4;

int ch1 = 4;
int ch2 = 5;
int ch3 = 6;
int ch4 = 7;
int ch6 = 8;
int led_red_light = 13;
int trigPin = A1;
int echoPin = A2;

int roll_channel;
int pitch_channel;
int throttle_channel;
int yaw_channel;
int alt_hold_switch;
int alt_hold_switch_value;
int throttle_channel_low;
int throttle_channel_high;
long duration;
float distance;

int hovering_pulse_function();
int throttle_channel_function();
int hovering_function();
int led_light_blink();

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

MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL
#define LED_PIN 13
bool blinkState = false;

bool dmpReady = false;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];

Quaternion q;
VectorInt16 aa;
VectorInt16 aaReal;
VectorInt16 aaWorld;
VectorFloat gravity;
float euler[3];
float ypr[3];

uint8_t teapotPacket[14] = { '

, 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

void setup()
{
  Serial.begin(115200);
  pinMode(ch1, INPUT);
  esc1.attach(9);
  pinMode(ch2, INPUT);
  esc2.attach(10);
  pinMode(ch3, INPUT);
  esc3.attach(11);
  pinMode(ch4, INPUT);
  esc4.attach(12);
  pinMode(ch6, INPUT);
  pinMode(echoPin, INPUT);

pinMode(trigPin, OUTPUT);
  pinMode(led_red_light, OUTPUT);

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
      TWBR = 24;
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
    Fastwire::setup(400, true);
  #endif
  mpu.initialize();
  devStatus = mpu.dmpInitialize();

mpu.setXGyroOffset(0);
  mpu.setYGyroOffset(-1);
  mpu.setZGyroOffset(1);
  mpu.setZAccelOffset(16365);

if (devStatus == 0)
  {
    mpu.setDMPEnabled(true);
    mpuIntStatus = mpu.getIntStatus();
    dmpReady = true;
    packetSize = mpu.dmpGetFIFOPacketSize();
  }

else
  {
    Serial.print(devStatus);
    Serial.println(F(")"));
  }

pinMode(LED_PIN, OUTPUT);
}

int hovering_pulse_function()
{
  alt_hold_switch = pulseIn(ch6, HIGH);
  if (alt_hold_switch < 970)
  {
    alt_hold_switch_value = HIGH;
  }
  else if (alt_hold_switch > 1460)
  {
    alt_hold_switch_value = LOW;
  }
  return 0;
}

int throttle_channel_function()
{
  throttle_channel_high = 1500;
  throttle_channel = throttle_channel_high;
  return 0;
}

int hovering_function()
{
  if (alt_hold_switch_value == HIGH && distance == 100)
  {
    esc1.writeMicroseconds(roll_channel);
    esc2.writeMicroseconds(pitch_channel);
    esc3.writeMicroseconds(throttle_channel_high);
    esc4.writeMicroseconds(yaw_channel);
  }
  return 0;
}

int led_light_blink(){
//***************************************************************************************************************************************************** 
//Run throttle_channel_function(); and turn on led light when meet condition of both "distance >= 95 && distance <=105 && alt_hold_switch_value"

if (distance >= 95 && distance <=105 && alt_hold_switch_value)
  {
    throttle_channel_function();
    digitalWrite(led_red_light, LOW);
  }
//*****************************************************************************************************************************************************
//LED blink when condition is "alt_hold_switch_value == LOW && distance >= 100"

else if(alt_hold_switch_value == LOW && distance > 0)
  {
    digitalWrite(led_red_light, LOW);
    delay(100);
    digitalWrite(led_red_light, HIGH);
  }
//*****************************************************************************************************************************************************
  return 0;
}

void loop()
{
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;

roll_channel = pulseIn(ch1, HIGH);
  esc1.writeMicroseconds(roll_channel);
  pitch_channel = pulseIn(ch2, HIGH);
  esc2.writeMicroseconds(pitch_channel);
  throttle_channel = pulseIn(ch3, HIGH);
  esc3.writeMicroseconds(throttle_channel);
  yaw_channel = pulseIn(ch4, HIGH);
  esc4.writeMicroseconds(yaw_channel);

digitalWrite(led_red_light, HIGH);

if(!dmpReady) return;
  mpuIntStatus = mpu.getIntStatus();
  fifoCount = mpu.getFIFOCount();

if((mpuIntStatus & 0x10) || fifoCount == 1024)
  {
    mpu.resetFIFO();   
  }

else if (mpuIntStatus & 0x02)
  {
    while (fifoCount < packetSize)
    fifoCount = mpu.getFIFOCount();
    mpu.getFIFOBytes(fifoBuffer, packetSize);
    fifoCount -= packetSize;

#ifdef OUTPUR_READABLE_QUATERNION
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      Serial.print("quat\t");
      Serial.print(q.w);
      Serial.print("\t");
      Serial.print(q.x);
      Serial.print("\t");
      Serial.print(q.y);
      Serial.print("\t");
      Serial.println(q.z);
    #endif

#ifdef OUTPUT_READABLE_EULER
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetEuler(euler, &q);
      Serial.print("euler\t");
      Serial.print(euler[0] * 180/M_PI);
      Serial.print("\t");
      Serial.print(euler[1] * 180/M_PI);
      Serial.print("\t");
      Serial.println(euler[2] * 180/M_PI);
    #endif

#ifdef OUTPUT_READABLE_YAWPITCHROLL
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
      Serial.print("Phi: ");
      Serial.print(ypr[1] * 18/M_PI);
      Serial.print("\t theta: ");
      Serial.print(" ");
      Serial.print(ypr[2] * 180/M_PI);
      Serial.print("\t Psi: ");
      Serial.print(" ");
      Serial.println(ypr[0] * 180/M_PI);
    #endif

#ifdef OUTPUT_READABLE_REALACCEL
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetAccel(&aa, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
      Serial.print("areal\t");
      Serial.print(aaReal.x);
      Serial.print("\t");
      Serial.print(aaReal.y);
      Serial.print("\t");
      Serial.println(aaReal.z);
    #endif

#ifdef OUTPUT_READABLE_WORLDACCEL
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetAccel(&aa, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
      mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
      Serial.print("aworld\t");
      Serial.print(aaWorld.x);
      Serial.print("\t");
      Serial.print(aaWorld.y);
      Serial.print("\t");
      Serial.println(aaWorld.z);
    #endif

#ifdef OUTPUT_TEAPOT
      teapotPacket[2] = fifoBuffer[0];
      teapotPacket[3] = fifoBuffer[1];
      teapotPacket[4] = fifoBuffer[4];
      teapotPacket[5] = fifoBuffer[5];
      teapotPacket[6] = fifoBuffer[8];
      teapotPacket[7] = fifoBuffer[9];
      teapotPacket[8] = fifoBuffer[12];
      teapotPacket[9] = fifoBuffer[13];
      Serial.write(teapotPacket, 14);
      teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
    #endif

blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);
  }

hovering_pulse_function();
  hovering_function();
  led_light_blink();

Serial.print("Channel 1 (roll) = ");
  Serial.print(roll_channel);
  Serial.print("\t\tChannel 2 (pitch) = ");
  Serial.print(pitch_channel);
  Serial.print("\t\tChannel 3 (throttle) = ");
  Serial.print(throttle_channel);
  Serial.print("\t\tChannel 4 (yaw) = ");
  Serial.print(yaw_channel);
  Serial.print("\t\tDistance: ");
  Serial.println(distance);
  delay(500);
}

Or is there a way to prevent the FIFO from overflowing as it is always overflowing when I upload the sketch. Thanks

Hi,
Does YOUR sketch work without the MPU6050 code?

What is you code doing, drone or similar?

Tom... :slight_smile:

I believe that if the FIFO is overflowing then you need to read it more often, or detect the overflow and clear it.

You can change the FIFO refresh rate in line 305 of the MPU6050_6Axis_MotionApps20.h file:

    0x02,   0x16,   0x02,   0x00, 0x01                // D_0_22 inv_set_fifo_rate

For information on detecting overflow and clearing the FIFO (if that's what you need to do ) I would look at the I2CDev library first and then also the MPU6050 datasheet.

You are using the pulseIn function

roll_channel = pulseIn(ch1, HIGH);

This may be where your issue is coming from. In general use of this function is discouraged for time critical applications since this function blocks the program from doing anything else while it it measuring, and waiting for, the pulse. It would be best to to use interrupts to measure these pulses instead.

Try removing all the pulseIn statements from your code and checking if you are still getting the FIFO overflow.

Although looking at your code, are you reading the MPU at all? The example that I think you have based this code on relies on receiving an interrupt from the MPU when it populates the FIFO buffer, but in your setup() it looks like you have removed the attachInterrupt(...) and other lines.

Have you used this approach to read the MPU only (i.e. without any other functionality included) and did that work?

TomGeorge:
Hi,
Does YOUR sketch work without the MPU6050 code?

What is you code doing, drone or similar?

Tom... :slight_smile:

Hi Tom, My sketch is able to work without the MPU6050 code

bms001:
I believe that if the FIFO is overflowing then you need to read it more often, or detect the overflow and clear it.

You can change the FIFO refresh rate in line 305 of the MPU6050_6Axis_MotionApps20.h file:

    0x02,   0x16,   0x02,   0x00, 0x01                // D_0_22 inv_set_fifo_rate

For information on detecting overflow and clearing the FIFO (if that's what you need to do ) I would look at the I2CDev library first and then also the MPU6050 datasheet.

You are using the pulseIn function

roll_channel = pulseIn(ch1, HIGH);

This may be where your issue is coming from. In general use of this function is discouraged for time critical applications since this function blocks the program from doing anything else while it it measuring, and waiting for, the pulse. It would be best to to use interrupts to measure these pulses instead.

Try removing all the pulseIn statements from your code and checking if you are still getting the FIFO overflow.

Although looking at your code, are you reading the MPU at all? The example that I think you have based this code on relies on receiving an interrupt from the MPU when it populates the FIFO buffer, but in your setup() it looks like you have removed the attachInterrupt(...) and other lines.

Have you used this approach to read the MPU only (i.e. without any other functionality included) and did that work?

Hi bms001, the mpu 6050 works even though the interrupt function is removed. The pulseIn function is needed to allow me to take in the pulse from the receiver and sent it to the flight controller even though I removed the pulseIn function as mentioned I wasn't able to receive the result from the MPU 6050 and it kept printing "FIFO overflow".

Panda01:
Hi, I need help in understanding why the MPU 6050 cannot display the data on serial monitor. I had tried compiling the MPU 6050 dmp code and it was successful in giving me the data. However, when I tried compiling with my sketch it couldn't work.

I'm thinking that compiling the MPU6050 code is the same as compiling it in a regular sketch, right? Probably best to start from the beginning again - which means get the serial data values from the MPU6050 to show up properly with minimal code ie. clean slate. Then save a version of the working code. And then proceed to figure out what is happening as you add extra bits to your code.....making sure to save newer versions of the code (and keep working code).

Also, make sure 'delay' commands eg delay(500) etc isn't interfering with your system.

Panda01:
Hi bms001, the mpu 6050 works even though the interrupt function is removed.

So if you have gone from code where the mpu is working to code where the mpu is not working (and by 'working' I'm assuming you mean something like it is consistently outputting values to the arduino with no overflow issue etc) then you should go back to the point where it worked and build up from there, checking after each change that it still 'works'. Basically what Southpark suggested in the previous post. (Also check the suggestion about delays)

Panda01:
The pulseIn function is needed to allow me to take in the pulse from the receiver and sent it to the flight controller even though I removed the pulseIn function as mentioned I wasn't able to receive the result from the MPU 6050 and it kept printing "FIFO overflow".

I have not used the pulseIn approach myself so I am only guessing but you may want to change this in the future if it causes problems. Perhaps if your receiver is sending the pulses very frequently it might be ok? If not, then this example shows how to read pulses with interrupts instead.