I designed a mini quadcopter which is about 4.5x4.5cm(Main Body). The main body is the PCB.

![enter image description here][1]

It weighs about ~20 grams with the battery. I'm using the MPU6050 with the DMP using the i2cDevLib. I am using raw radians for pitch, roll, and yaw these measures are read from the MPU6050's DMP. The motors are attached to the body using electrical tape(Black thing around motors). The motors are 8.5mm in diameter and are driven by a n-channel mosfet. The mode of control right now is bluetooth(HC-05 module). The code being used is my own. I have a control loop on all axes, the pitch and roll have the same values since the quadcopter is symmetrical. The problem I have is that PID tuning is next to impossible, the best I got was a ~2 second flight ([Video in slow-motion][2]).

At first I was using my own code for the control loop, but it wasn't as effective as the Arduino PID library.

The output of the PID loops are mapped to -90 to 90 on all axes. This can be seen in the code

myPID.SetOutputLimits(-90, 90); //Y angle
myPID1.SetOutputLimits(-90, 90); // X angle
myPID2.SetOutputLimits(-90, 90); // Yaw angle
myPID.SetMode(AUTOMATIC);
myPID1.SetMode(AUTOMATIC);
myPID2.SetMode(AUTOMATIC);


My full code is below, but what do you think the problem is?

Code

http://pastebin.com/cnG6VXr8

• Hard to tell... Could be a bug or bugs in your code or it could hardware related (too much vibration, props mounted the wrong way, etc). I would try running a well known flight controller code like ardupilot just a test to make sure hardware is fine. And then you have a solid ref to compare your code with, I.e. compare the PID outputs for instance
– dm76
Apr 6 '15 at 7:00
• @dm76 Vibration is very minimal, and the props are balanced. The props are mounted correctly and spin in the correct direction. The problem with ardupilot is that it uses brushless motors, different sensors, and totally different configurations. Apr 6 '15 at 16:35
• So now the question, how did you tune the PID settings? You would need some kind of setup to hold it in place while allowing it to pitch and roll
– dm76
Apr 6 '15 at 17:01
• @dm76 Yep, that was exactly what I did. It works on one axis, but when I test all axes to fly out doesn't work. I think the code itself doesn't work. Apr 6 '15 at 17:04

It could be a number of things. You are clearly in a positive feedback loop. You can see this with the way the amplitude of the oscillation increases with time in your video. The most likely problem is that your gain is too high. You should be able to get flight that doesn't crash quickly with just P control. I would start with zero I and D and a low P value, then ramp up P value to get reasonable responsiveness without too much overshoot, then increase I to remove steady state error, and throw in a little D if necessary to reduce overshoot. You probably won't even need D if you back off the aggressiveness of P and I. Measuring the derivative of the error can also have a lot of noise in it.

• I've tried slowly increasing the P gain, but it's always either too sluggish, or to responsive. I will keep trying. Looking at the code, is the PID correctly implemented? Also thank you for your response. Apr 11 '15 at 17:17
• I can't tell if it is correct or not. You might try to verify that the PID output is controlling the right motors to counteract the error. I don't know what orientation the mpu is mounted relative to the motors. It looks like it might be rotated as it looks like you are mapping pitch errors to increasing speed of the right side motors relative to left. If it was mounted aligned with the robot, I would expect it to map to the front motors relative to back. Apr 15 '15 at 17:33
• I think I found the problem. Previously the control loops were running every 100ms meaning 10hz. I have now decreased it to every 5ms meaning 200hz. The IMU updates at 100hz. It is flying much higher, and a bit more stable. It's able to fly for about 3-4 seconds. Apr 15 '15 at 20:08
• Delay of signals from sensors to props can be an important factor, but you have probably done the best that could be done by increasing the runtime frequency. The next thing that can be done (if tuning the PID is not enough at all) would be to increase the stability of the quadrotor (like extending the motors away from the body, to have higher inertia, etc) or increase the acceleration performance of the motors (might even mean changing the motors). May 12 '15 at 20:00

Your derivative term is incorrect. You have:

dTerm = Kd * (error - last_error);

Note that this is not a derivative of the error, this is simply the difference between the previous error and the current error. To get the derivative of the error you need to divide by the sample time. I am not familiar with your platform so I cannot tell you how to get the sample time (loop time, sweep time, etc.), but you need a timer to start and stop every loop.

Anyways, assuming that $t_{elapsed} = dT$, and you can get this from a timer set to count and reset every time your PID code executes, then your sample time is $dT$ and your dTerm needs to become:

dTerm = Kd * (error - last_error)/dT;

If this isn't the root cause of your control issues it's a major contributing factor. It looks like your PID sections are commented out (not sure why), but I noticed this error everywhere I saw a "dTerm" defined.

Hope this helps!