0

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;
5
  • What does the Arduino IDE say about memory usage when building the sketch? Apr 8 '16 at 13:09
  • 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.
    – suky
    Apr 8 '16 at 13:13
  • Hint: Can "do { } while ((avg3 != autothrottle)||(avg3>autothrottle));" lock up? Apr 8 '16 at 13:46
  • I commented those two voids out, and it is still freezing. I believe it is some sort of track overflow.
    – suky
    Apr 8 '16 at 13:50
  • I am receiving data from both the MPU6050 and my Hobby-king RC Receiver simultaneously .
    – suky
    Apr 8 '16 at 14:37
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.

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.