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;
