arduino quadcopter freezing while reading MPU6050/RC Receiver

Question

I am trying to build an Arduino Flight controller. I have an MPU6050 (Gyroscope/Accelerometer) on board that is helping me configure my stabilization software. I also have my ESC's, and receiver plugged in to my Arduino. At some point during the code running, receiving and processing data from the Gyroscope and Receiver, the Arduino Uno freezes and I am not sure what is causing this problem. Any advice on how to fix this is greatly appreciated.

I am also getting FIFO overflows.

#include <Servo.h>
#include <Wire.h>
Servo esc1;
Servo esc2;
Servo esc3;
Servo esc4;
byte last_channel_1, last_channel_2, last_channel_3, last_channel_4, last_channel_5;
int receiver_input_channel_1, receiver_input_channel_2, receiver_input_channel_3;
int receiver_input_channel_4, receiver_input_channel_5;
unsigned long timer_1, timer_2, timer_3, timer_4, timer_5;
int reciever1, reciever2, reciever3, reciever4, reciever5;
int pastreciever1, pastreciever2, pastreciever3, pastreciever4, pastreciever5;
bool keycode;
int avg1, avg2, avg3, avg4;
int i;
const int avglen = 8;
int reciever1avg[avglen], reciever2avg[avglen], reciever3avg[avglen], reciever4avg[avglen];
int landingspeed = 1140;
int requestedspeed = 1400;
int autothrottle;
int motor1change, motor2change, motor3change, motor4change;
int yawchange, rollchange, nickchange;
int wait;
int set;
unsigned long interval=5000; // the time we need to wait
unsigned long previousMillis=0; // millis() returns an unsigned long.
unsigned long timer = 0;
//#define printdata
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#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' };
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
 mpuInterrupt = true;
}
float initialGyroX;
float initialGyroY;
float initialGyroZ;
const int autopilotkey = 1150;
int deg_x;
int deg_y;
int deg_z;
int requiredspeed;
void setup() 
{ 
 //Arduino (Atmega) pins default to inputs, so they don't need to be explicitly declared as inputs
 PCICR |= (1 << PCIE0); // set PCIE0 to enable PCMSK0 scan
 PCMSK0 |= (1 << PCINT0); // set PCINT0 (digital input 8) to trigger an interrupt on state change
 PCMSK0 |= (1 << PCINT1); // set PCINT1 (digital input 9)to trigger an interrupt on state change
 PCMSK0 |= (1 << PCINT2); // set PCINT2 (digital input 10)to trigger an interrupt on state change
 PCMSK0 |= (1 << PCINT3); // set PCINT3 (digital input 11)to trigger an interrupt on state change
 PCMSK0 |= (1 << PCINT4); // set PCINT4 (digital input 12)
 pastreciever1 = 1058;
 pastreciever2 = 1058;
 pastreciever3 = 1058;
 pastreciever4 = 1058;
 wait=0;
 set=0;
 keycode = false;
 esc1.writeMicroseconds(1000);
 esc2.writeMicroseconds(1000);
 esc3.writeMicroseconds(1000);
 esc4.writeMicroseconds(1000);
 esc1.attach(9);
 esc2.attach(6);
 esc3.attach(5);
 esc4.attach(3);
 #ifdef printdata 
 Serial.begin(115200);
 #endif
 #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
 Wire.begin();
 TWBR = 24; 
 #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
 Fastwire::setup(400, true);
 #endif
 mpu.initialize();
 #ifdef printdata
 Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
 #endif
 devStatus = mpu.dmpInitialize();
 mpu.setXGyroOffset(220);
 mpu.setYGyroOffset(76);
 mpu.setZGyroOffset(-85);
 mpu.setXAccelOffset(-2580); 
 mpu.setYAccelOffset(2567);
 mpu.setZAccelOffset(1920); 
 //Serial.println("The timer has begun.");
 if (devStatus == 0) 
 {
 #ifdef printdata
 Serial.println(F("Enabling DMP..."));
 #endif
 mpu.setDMPEnabled(true);
 //Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
 attachInterrupt(0, dmpDataReady, RISING);
 mpuIntStatus = mpu.getIntStatus();
 #ifdef printdata
 Serial.println(F("DMP ready! Waiting for first interrupt..."));
 #endif
 dmpReady = true;
 packetSize = mpu.dmpGetFIFOPacketSize();
 } else {
 // ERROR!
 // 1 = initial memory load failed
 // 2 = DMP configuration updates failed
 // (if it's going to break, usually the code will be 1)
 #ifdef printdata
 Serial.print(F("DMP Initialization failed (code "));
 #endif
 //Serial.print(devStatus);
 //Serial.println(F(")"));
 }
}
/*void ascend()
{
 Serial.println("Ascending.");
 for (autothrottle = landingspeed; autothrottle<requestedspeed; autothrottle++)
 {
 esc1.writeMicroseconds(autothrottle);
 esc1.writeMicroseconds(autothrottle);
 esc1.writeMicroseconds(autothrottle);
 esc1.writeMicroseconds(autothrottle);
 delay(100);
 //gyro auto-level code??
 if ((avg3 != autothrottle)&&(avg3 < autothrottle));
 { }
 if ((avg3==requestedspeed)||(avg3>requestedspeed)||(avg1>1532)||(avg1<1472))
 {
 break; //go back to reciever input
 }
 }
}
void descend()
{
 Serial.println ("Descending.");
 for (autothrottle = avg3; autothrottle>landingspeed; autothrottle--)
 {
 esc1.writeMicroseconds(autothrottle);
 esc1.writeMicroseconds(autothrottle);
 esc1.writeMicroseconds(autothrottle);
 esc1.writeMicroseconds(autothrottle);
 delay(100);
 //Gyro autolevel code??
 do
 { } while ((avg3 != autothrottle)||(avg3>autothrottle));
 if ((avg3 == autothrottle)||(avg3<autothrottle)||(avg1>1532)||(avg1<1472))
 {
 break; //go back to reciever input
 }
 }
}*/
void loop() 
{ 
 print_signals();
 motor1change=0;
 motor2change=0;
 motor3change=0;
 motor4change=0;
 if (wait==0)
 {
 delay(20000);
 //XBee.write the weather
 }
 wait=1;
 if (set==6)
 {
 initialGyroX = (ypr[1] * 180/M_PI)-0.34;//X
 initialGyroY = (ypr[2] * 180/M_PI)-0.5;//Y
 initialGyroZ = ypr[0] * 180/M_PI;//Z
 #ifdef printdata
 Serial.print ("STORED:");
 Serial.print("\t");
 Serial.print (initialGyroX);
 Serial.print("\t");
 Serial.print (initialGyroY);
 Serial.print("\t");
 Serial.println (initialGyroZ);
 #endif
 }
 //Automatic Take-off
 /*if ((set>=6)&&(average3<1300)&&(average4<1300)&&(average2<1300)&&(average1>1700))
 {
 Serial.println("Ascending..");
 ascend();
 }*/
 //Nick (Go forward/backward)------------------------------------------
 if(avg2 > 1532)
 {
 nickchange = (avg2 - 1532)/2;
 motor1change=motor1change+nickchange;
 motor2change=motor2change-nickchange;
 motor3change=motor3change-nickchange;
 motor4change=motor4change+nickchange;
 }
 if (avg2 < 1494)
 {
 nickchange = (1494-avg2)/2;
 motor1change=motor1change-nickchange;
 motor2change=motor2change+nickchange;
 motor3change=motor3change+nickchange;
 motor4change=motor4change-nickchange;
 }
 //Yaw-------------------------------------------------------------------
 if (avg1 > 1510)
 {
 yawchange = (avg1 - 1510)/2;
 motor1change=motor1change+yawchange;
 motor2change=motor2change-yawchange;
 motor3change=motor3change+yawchange;
 motor4change=motor4change-yawchange;
 }
 if (avg1 < 1470)
 {
 yawchange = (1470 - avg1)/2;
 motor1change=motor1change-yawchange;
 motor2change=motor2change+yawchange;
 motor3change=motor3change-yawchange;
 motor4change=motor4change+yawchange;
 }
 //Roll------------------------------------------------------------------
 if (avg4 > 1532)
 {
 rollchange = (avg4 - 1532)/2;
 motor1change=motor1change+rollchange;
 motor2change=motor2change+rollchange;
 motor3change=motor3change-rollchange;
 motor4change=motor4change-rollchange;
 }
 if (avg4 < 1472)
 {
 rollchange = (1472 - avg4)/2;
 motor1change=motor1change-rollchange;
 motor2change=motor2change-rollchange;
 motor3change=motor3change+rollchange;
 motor4change=motor4change+rollchange;
 }
 if (!dmpReady) return;
 while (!mpuInterrupt && fifoCount < packetSize) {
 }
 mpuInterrupt = false;
 mpuIntStatus = mpu.getIntStatus();
 fifoCount = mpu.getFIFOCount();
 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
 mpu.resetFIFO();
 #ifdef printdata
 Serial.println(F("FIFO overflow!"));
 #endif
 } else if (mpuIntStatus & 0x02) {
 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
 mpu.getFIFOBytes(fifoBuffer, packetSize);
 fifoCount -= packetSize;
 //Print Data
 mpu.dmpGetQuaternion(&q, fifoBuffer);
 mpu.dmpGetGravity(&gravity, &q);
 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
 /*Serial.print("XYZ:\t");
 Serial.print(ypr[1] * 180/M_PI);
 Serial.print("\t");
 Serial.print(ypr[2] * 180/M_PI);
 Serial.print("\t");
 Serial.println(ypr[0] * 180/M_PI);*/
 //#endif
//Stabilization----------------------------------------------------------------------------
if ((keycode==true)&&(avg3>autopilotkey))
{ 
 //X Axis----------------------------------------------------------
 if ((ypr[1]* 180/M_PI)>(initialGyroX + 2))
 {
 deg_x = initialGyroX - (ypr[1] * 180/M_PI);
 requiredspeed = (-4.5*deg_x);
 #ifdef printdata
 Serial.print ("GyroX++: ");
 #endif
 //Serial.println(requiredspeed);
 motor1change=motor1change+requiredspeed;
 motor2change=motor2change+requiredspeed;
 motor3change=motor3change-requiredspeed;
 motor4change=motor4change-requiredspeed;
 }
 if ((ypr[1]* 180/M_PI)<(initialGyroX - 2))
 {
 deg_x = initialGyroX - (ypr[1] * 180/M_PI);
 requiredspeed = (4.5*deg_x);
 #ifdef printdata
 Serial.print("GyroX--: ");
 #endif
 //Serial.println (requiredspeed);
 motor1change=motor1change-requiredspeed;
 motor2change=motor2change-requiredspeed;
 motor3change=motor3change+requiredspeed;
 motor4change=motor4change+requiredspeed;
 }
 //Y Axis------------------------------------------------------------
 if ((ypr[2] * 180/M_PI)>(initialGyroY + 2))
 {
 deg_y = initialGyroY - (ypr[2] * 180/M_PI);
 requiredspeed = (-4.5*deg_y);
 #ifdef printdata
 Serial.print ("GyroY++: ");
 #endif
 //Serial.println (requiredspeed);
 motor1change=motor1change+requiredspeed;
 motor2change=motor2change-requiredspeed;
 motor3change=motor3change-requiredspeed;
 motor4change=motor4change+requiredspeed;
 }
 if ((ypr[2] * 180/M_PI)<(initialGyroY - 2))
 {
 deg_y = initialGyroY - (ypr[2] * 180/M_PI);
 requiredspeed = (4.5*deg_y);
 #ifdef printdata
 Serial.print ("GyroY--: ");
 #endif
 //Serial.println (requiredspeed);
 motor1change=motor1change-requiredspeed;
 motor2change=motor2change+requiredspeed;
 motor3change=motor3change+requiredspeed;
 motor4change=motor4change-requiredspeed;
 }
 //Z Axis--------------------------------------------------------------
 if ((ypr[0]* 180/M_PI)>(initialGyroZ + 4))//decrease
 {
 deg_z = initialGyroZ - (ypr[0] * 180/M_PI);
 requiredspeed = (-1*deg_z);
 #ifdef printdata
 Serial.print ("GyroZ++: ");
 #endif
 //Serial.println (requiredspeed);
 motor1change=motor1change-requiredspeed;
 motor2change=motor2change+requiredspeed;
 motor3change=motor3change-requiredspeed;
 motor4change=motor4change+requiredspeed;
 }
 if ((ypr[0]* 180/M_PI)<(initialGyroZ - 4))//increase
 {
 deg_z = initialGyroZ - (ypr[0] * 180/M_PI);
 requiredspeed = (1*deg_z);
 #ifdef printdata
 Serial.print ("GyroZ--: ");
 #endif
 //Serial.println (requiredspeed);
 motor1change=motor1change+requiredspeed;
 motor2change=motor2change-requiredspeed;
 motor3change=motor3change+requiredspeed;
 motor4change=motor4change-requiredspeed;
 }
 } 
}
 motor1change=constrain(motor1change, -100, 100);
 motor2change=constrain(motor2change, -100, 100);
 motor3change=constrain(motor3change, -100, 100);
 motor4change=constrain(motor4change, -100, 100);
 #ifdef printdata
 //Elevation------------------------------------------------------------
 Serial.print ("Motor Changes (1-4): ");
 Serial.print (motor1change); 
 Serial.print (" ");
 Serial.print (motor2change);
 Serial.print (" ");
 Serial.print (motor3change);
 Serial.print (" ");
 Serial.println (motor4change);
 #endif
 if (avg3>1150)
 {
 esc1.writeMicroseconds((avg3+7)+motor1change);
 esc2.writeMicroseconds((avg3+7)+motor2change);
 esc3.writeMicroseconds(avg3+motor3change);
 esc4.writeMicroseconds(avg3+motor4change);
 }
}
//This routine is called every time input 8, 9, 10 or 11 changed state
ISR(PCINT0_vect){
 //Channel 1=========================================
 if(last_channel_1 == 0 && PINB & B00000001 ){ //Input 8 changed from 0 to 1
 last_channel_1 = 1; //Remember current input state
 timer_1 = micros(); //Set timer_1 to micros()
 }
 else if(last_channel_1 == 1 && !(PINB & B00000001)){ //Input 8 changed from 1 to 0
 last_channel_1 = 0; //Remember current input state
 receiver_input_channel_1 = micros() - timer_1; //Channel 1 is micros() - timer_1
 }
 //Channel 2=========================================
 if(last_channel_2 == 0 && PINB & B00000100 ){ //Input 10 changed from 0 to 1
 last_channel_2 = 1; //Remember current input state
 timer_2 = micros(); //Set timer_3 to micros()
 }
 else if(last_channel_2 == 1 && !(PINB & B00000100)){ //Input 10 changed from 1 to 0
 last_channel_2 = 0; //Remember current input state
 receiver_input_channel_2 = micros() - timer_2; //Channel 3 is micros() - timer_3
 }
 //Channel 3=========================================
 if(last_channel_3 == 0 && PINB & B00001000 ){ //Input 11 changed from 0 to 1
 last_channel_3 = 1; //Remember current input state
 timer_3 = micros(); //Set timer_4 to micros()
 }
 else if(last_channel_3 == 1 && !(PINB & B00001000)){ //Input 11 changed from 1 to 0
 last_channel_3 = 0; //Remember current input state
 receiver_input_channel_3 = micros() - timer_3; //Channel 4 is micros() - timer_4
 }
 //Channel 4=========================================
 if(last_channel_4 == 0 && PINB & B00010000 ){ //Input 12 changed from 0 to 1
 last_channel_4 = 1; //Remember current input state
 timer_4 = micros(); //Set timer_5 to micros()
 }
 else if(last_channel_4 == 1 && !(PINB & B00010000)){ //Input 12 changed from 1 to 0
 last_channel_4 = 0; //Remember current input state
 receiver_input_channel_4 = micros() - timer_4; //Channel 5 is micros() - timer_5
 }
}
//Subroutine for displaying the receiver signals
void print_signals()
{ 
 //Constrain
 reciever1 = constrain(receiver_input_channel_1, 1000, 2100);
 reciever2 = constrain(receiver_input_channel_2, 1000, 2100);
 reciever3 = constrain(receiver_input_channel_3, 1000, 2100);
 reciever4 = constrain(receiver_input_channel_4, 1000, 2100);
 reciever5 = constrain(receiver_input_channel_5, 1000, 2100); 
 set=set+1;
 //AVG 1-4-----------------------------------------------------
 for (i=0; i<avglen-1; i++)
 {
 reciever1avg[i]=reciever1avg[i+1];
 }
 reciever1avg[avglen-1]=reciever1;
 avg1=0;
 for (i=0; i<avglen; i++)
 {
 avg1+= reciever1avg[i];
 } avg1/=avglen;
 for (i=0; i<avglen-1; i++)
 {
 reciever2avg[i]=reciever2avg[i+1];
 }
 reciever2avg[avglen-1]=reciever2;
 avg2=0;
 for (i=0; i<avglen; i++)
 {
 avg2+= reciever2avg[i];
 } avg2/=avglen;
 for (i=0; i<avglen-1; i++)
 {
 reciever3avg[i]=reciever3avg[i+1];
 }
 reciever3avg[avglen-1]=reciever3;
 avg3=0;
 for (i=0; i<avglen; i++)
 {
 avg3+= reciever3avg[i];
 } avg3/=avglen;
 for (i=0; i<avglen-1; i++)
 {
 reciever4avg[i]=reciever4avg[i+1];
 }
 reciever4avg[avglen-1]=reciever4;
 avg4=0;
 for (i=0; i<avglen; i++)
 {
 avg4+= reciever4avg[i];
 } avg4/=avglen;
 //Activate Autopilot
 if ((avg1==pastreciever1)&&(avg2==pastreciever2)
 &&(avg3==pastreciever3)&&(avg4==pastreciever4)&&(avg3>autopilotkey))
 {
 keycode=true;
 } else {keycode=false;}
 pastreciever1 = avg1;
 pastreciever2 = avg2;
 pastreciever3 = avg3;
 pastreciever4 = avg4;

• Mikael Patel: What does the Arduino IDE say about memory usage when building the sketch?
• suky: Sketch uses 18,698 bytes (57%) of program storage space. Maximum is 32,256 bytes. Global variables use 597 bytes (29%) of dynamic memory, leaving 1,451 bytes for local variables. Maximum is 2,048 bytes.
• Mikael Patel: Hint: Can "do { } while ((avg3 != autothrottle)||(avg3>autothrottle));" lock up?
• suky: I commented those two voids out, and it is still freezing. I believe it is some sort of track overflow.
• suky: I am receiving data from both the MPU6050 and my Hobby-king RC Receiver simultaneously .

Answer 1

This can happen in I2C sometimes. Basically the controller asks the sensors for data but there may be a bit error in the address. If so the sensor thinks you are talking to another chip, meanwhile your I2C master code waits for a reply, forever.

To fix this problem I would recommend the use of a watchdog timer. You can find the many examples online of how to set this up.