# How can an Arduino-Quadcopter be stabilized?

I have built an arduino quad-copter and I am using an MPU6050 to read the rotational position and velocity of the aircraft. The problem is that I cannot get the thing to stabilize. Watch this video here to get an idea of what I mean.

So as you can see from the footage (and you did watch it, right?), I'm not even getting steady oscillations, it's just random. What I don't understand is how people get their quad-copters to oscillate very quickly (maybe 10 times per second) and then trim up their PID gains to smooth it out, whereas if I raise my p-gain high enough for it to adjust that quickly, the quad-copter just goes berserk and tries to kill me.

And, speaking of p-gains, here's the PID I wrote:

``````float KrP = 0.002000;
float KrI = 0.000001;
float KrD = 0.200000;

void adjustRoll(float currentRoll, int newRoll) {
if (thrust <= 1200){
inAutoRoll = false;
NWPower = thrust;
NEPower = thrust;        //PID not ready to turn on yet? Just set motor powers
SWPower = thrust;        //equal to the throttle.
SEPower = thrust;

I_roll = 0;              // Reset the integral.
return;                  // Don't do anything else
}

float newRate = 2 * (newRoll - currentRoll); // How fast should the drone try to
// adjust, based on the error.

if (newRate > 50)newRate = 50;        // Don't adjust too fast!
else if (newRate < -50)newRate = -50;

float offset = newRate - rollRate;

I_roll += KrI * offset;

if(!inAutoRoll) {        // Is the PID just now turning on? (Throttle requirement met).
lastRollRateOffset = offset;  //Smooth transition.
inAutoRoll = true;
}

float adjust = (KrP * offset) + (KrI * I_roll) + (KrD * (offset - lastRollRateOffset));

if (NWPower > maxOut) NWPower = maxOut;
else if (NWPower < minOut) NWPower = minOut;

if (NEPower > maxOut) NEPower = maxOut;
else if (NEPower < minOut) NEPower = minOut;

if (SWPower > maxOut) SWPower = maxOut;
else if (SWPower < minOut) SWPower = minOut;

if (SEPower > maxOut) SEPower = maxOut;
else if (SEPower < minOut) SEPower = minOut;

lastRollRateOffset = offset;
}
``````

And in case you were wondering... yes, I do know about integral windup. The reason I have not dealt with that in my code is because the quadcopter is no where near stable, so I'm not worrying about perfection just yet. Also, I have my I-gain set to 0.000001, so it doesn't matter. The reason I have done this is because (as far as I know) a quadcopter can fly with only P and D, and with the way mine is behaving, I think a larger I-gain would only make things worse.

More detail about my PID: The PID corrects for the rotational velocity of the quad-copter, which, as I understand it, is the best way to control a quadcopter. The correction-rotation-rate is determined by multiplying the error (in degrees) by two. Why two? I dunno. I experimented with some other multipliers and two seems to be okay. Its also a prime number, so, yeah...

So anyways! What do I need to do to make the quad-copter stable? I have seen amazingly perfect quad-copters using the same arduino and the same MPU6050, but mine just tilts all over the place.

Some possible problems off the top of my head:

1. Bad algorithm for calculating the needed rotational velocity based on error.
4. I didn't balance my props, but seriously, would a couple of pieces of electrical tape stuck to the props really fix this? It doesn't seem like it would.

• Have you run some subsystem testing sketches? Eg did you specifically test whether the MPU6050 is working correctly, motor drivers ok, etc? Feb 27, 2017 at 9:11
• The MPU seems fine. When I tilt it it gives me reasonable values for yaw pitch roll, as well as for rotational velocity. The drivers work fine too. I have the quadcopter plugged into my computer so I have all the data I could possibly want on my screen while it's running. Feb 27, 2017 at 14:51
• Re 4: The cool thing about quadcopters is that all the control trimming and balancing /can/ be done in math, rather than physically. Feb 27, 2017 at 19:54
• This is far, far, far beyond the scope of this site. If you want to build something that flies, spend some time studying existing solutions and the control algorithms they use, and read the replacement firmware threads at rcgroups; also don't use an actual arduino - pretty much every project that started there has long since moved on to better choices. Feb 27, 2017 at 20:02
• random behavior is a big problem with Drone. Did you solve this problem ? My drone has got the same random behavior. infrare4ever Aug 1, 2022 at 19:36

For stability and tuning you need to make the adjustment cycle time compatible with the physical system. How fast does this adjustment code get called relative to how fast the physical system oscillates? Without that under control, tuning the gains will be difficult, particularly for krI and krD.

For instance, if you are updating every millisecond, and it takes a 1/10th of a second to physically flop over from -90 to 0 at the current roll_thrust setting, there's only enough kP to adjust the roll_thrust `kP*(RollDelta/2)*DT/dt=0.002*(90/2)*0.1/0.001=9` Is that a sane adjustment for a thrust/motorPower of 1200+? If you sample fast enough to make it capable of a full-throttle (+/-800?) adjustment with this particular kP, you might need to sample 88x faster (1ms/(800/9)=0.01125ms). From the other direction, I'd figure out how fast you can sample, then choose a krP that could get you a full-bias adjustment in reasonable time.

Maybe think of `adjust` as `roll_thrust` state variable measured in units of ESC_microseconds, and krP is the conversion factor from whatever error you are measuring. Is it roll angle error? roll rate error? Absolute roll rate error? It looks like this bit:

``````float newRate = 2 * (newRoll - currentRoll); // How fast should the drone try to
if (newRoll - currentRoll < 0) {             // adjust, based on the error.
newRate = 0 - newRate;
}
``````

is essentially:

``````float newRate = 2 * abs(newRoll - currentRoll);
``````

which might make control discontinuous. If currentRoll is a positional roll angle setpoint, it won't be able to tell if it is on one side or the other.

If you want a roll angle of -90deg to make full throttle one way and 90deg to full throttle the other, then krP should be on the order of 900us/90deg=10us/degree. And krI=krD=0 to start with.

I'd code up separate PIDs for `thrust`, `roll_thrust`, `pitch_thrust`, `yaw_thrust` control variables and then sum them together to get the `{NE,NW,SE,SW}Power` outputs. Then you could monitor and control in {thrust,roll_angle,pitch_angle,yaw_rate} space and the coding will be more straightforward and interpretable.

• Thanks for your feedback! I have some things to clarify though: 1) I think you may have misread my code, because there shouldn't be any accumulation in the P adjust, which has no lasting memory between individual loops in the PID. 2) My throttle increments between 1100us and 1900us, so there is an 800 range, not 2500 (ESC's accept a ppm signal of 1000 - 2000us). Feb 27, 2017 at 23:09
• Also, how can I write a positional PID to control the speed PID that I already have, and still have the slightest clue as to which of the now SIX pid gains I would have to adjust? I tried it, but I don't know how to look at my quadcopter's behavior and deduce which of the six gains I need to change. That's why I just stuck with the [2* error] to make it simple. Feb 27, 2017 at 23:15
• You are right. I missed that the thrusts were initialized in the loop--it is a plain old positional form of PID--I'm striking the velocity form bit. The krP, krI and krD, are just scaling factors from the various forms of error to the output variable. I think I'm still confused about what the measured inputs are. Feb 28, 2017 at 2:25
• Actually, it is a velocity form PID, let me explain: "thrust" is the 1100-1900ppm signal from the thrust stick on my transmitter. Each motor is assigned that thrust in the beginning, then the PID adjusts that thrust to each motor to try to get the quad-copter to move at the desired rotational velocity(determined by 2* error in degrees from the desired angle). So to answere your question: The measured inputs are 1) the current angle of the quad-copter, and 2) the current rotational rate of the quad-copter. Also, the if(newroll-current roll<0) thing is an error on my part. I took it out. Feb 28, 2017 at 15:03
• Are you sure that driving rotational_velocity(deg/sec) towards the 2*error_deg value is the right control scheme? This is beyond anything Arduino, but I think you'd want to work towards stability in degree-space. Maybe not tracking the planned velocity curve at higher gains is what is causing your instability. Feb 28, 2017 at 20:11

To help solve this problem, I wrote a program in Processing that graphs all the data from the quadcopter, including roll rate, roll angle, thrust, etc... After doing so I realized that the problem was not in the PID, but in how I handled the data coming out of MPU6050. You can read all about it in this question I posted here before I had discovered the full solution.

The quadcopter is now capable of taking off, though sadly it is not very stable. This could be from bad PID gains, but I suspect it is from vibration in the roll-rate values from the MPU6050. The reason I think this is because I don't see steady oscillations, but rather random movement. Anyway, If I find a more complete solution I will update this answer.

To be sure the receiver gets the right signal from the emitter it is mandatory to scrutinize the signal on the receiver for a while. Once the signal on the receiver is checked good it is possible to go ahead step by step. The first step is to drive the 4 ESC/motors with the same signal coming from the throttle. Once the behavior of the 4 motors complies with changes on the throttle coming from the emitter it is time to go to the next step. The next step stems from a mix with pitch/roll/yaw coming from the emitter. The mix is added/removed to/from the throttle addressing each motor. The behavior of the 4 motors must show exactly the mix that comes from the emitter. An "acceptable" mix is something like (in the same unit, degres or microseconds):

Something like this:

``````int throttle_gaz;
int roll_length, pitch_length, yaw_length;

throttle_gaz=pulse_duration[throttle_chanel];
if ( abs(throttle_gaz-1500) <= 50 )
{
throttle_gaz=1500;
}
roll_length=pulse_duration[roll_chanel]-1500;            //ROLL
if (abs (roll_length) <=100 )
{
roll_length=0;
}
roll_length/=3;

pitch_length=pulse_duration[pitch_chanel]-1500;           //PITCH
if (abs (pitch_length) <= 100 )
{
pitch_length=0;
}
pitch_length/=3;

yaw_length=pulse_duration[yaw_chanel]-1500;             //YAW
if (abs (yaw_length) <=100 )
{
yaw_length=0;
}
yaw_length/=3;