Arduino Quadcopter code help

Before I explain my problem, let me first explain my project.

I am currently trying to make an autonomous quadcopter with an Arduino Uno as the "brain". To save processing power (and time) I am having the Kkmulticopter V5.5 Controller BlackBoard handle the PID, balancing, etc. I am having the Arduino Uno act as a RC receiver, as its ports connect directly into the kkboard, and output the PWM signals needed to imitate specific channels. It is through this way that the Arduino Uno is able to control the quad. If you want to see pictures of my setup I'll be glad to post them.

Now for my problem...

As I stated above, the uno is the "brain" of the quad, so it controls navigation, direction and other autonomous functions. I have gotten the code written to where it reads the gyro sensor (x,y,z), and makes sure the quad stays within a set window of values.

Thats what it should do, but it doesn't.

From what I am seeing from motor movement, is that it's taking too much time to execute certain lines of code, which is messing with the PWM outputs to the kkboard.

Below is my code (not including the gyro sensor library)

My code exceeds the 9000 character limit, so here is a link to my repository. My main programs are:

QuadMain.ino

QuadFunctions.h

The rest are for the gyro.

My question is this: Do I need another board (for more processing power), or am I just really inefficient in how I programmed my code?

Thanks ahead of time. :)

We used 8-bit MCUs for APs when that was the only MCUs available, but the ARM processors win out so you might want to look at the Teensy’s for more speed, memory, pins etc. at about $15 which makes them an amazing value.

 // display Euler angles in degrees
 mpu.dmpGetQuaternion(&q, fifoBuffer);
 mpu.dmpGetGravity(&gravity, &q);
 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

 mpu.dmpGetQuaternion(&q, fifoBuffer);
 mpu.dmpGetGravity(&gravity, &q);
 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); 
 Serial.print(ypr[0] * 180/M_PI);
 Serial.print(", "); 
 Serial.print(ypr[1] * 180/M_PI);
 Serial.print(", "); 
 Serial.println(ypr[2] * 180/M_PI);

 Yaw = ypr[0] * 180/M_PI;
 Pitch = ypr[1] * 180/M_PI;
 Roll = ypr[2] * 180/M_PI;

Why fetch the quaternion and calculate the angles twice? Why convert to degrees twice?

wwbrown: We used 8-bit MCUs for APs when that was the only MCUs available, but the ARM processors win out so you might want to look at the Teensy's for more speed, memory, pins etc. at about $15 which makes them an amazing value.

So you recommend getting one of these? Will the processing power be faster and do you think it will solve the timing issue?

johnwasser: Why fetch the quaternion and calculate the angles twice?

I'm not sure why. As I wasn't the one who wrote that part of the code.I did however, try commenting one of the occurrences out, but that messed up the program. So I I'm going to have to keep it the way it was.

johnwasser: Why convert to degrees twice?

I now changed it to this:

            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);           

            Yaw = ypr[0] * 180/M_PI;
            Pitch = ypr[1] * 180/M_PI;
            Roll = ypr[2] * 180/M_PI;            
            
            Serial.print(Yaw);
            Serial.print(", ");             
            Serial.print(Pitch);
            Serial.print(", ");                         
            Serial.println(Roll);