How can an Arduino-Quadcopter be stabilized?

Question

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));

  NWPower += adjust;
  NEPower -= adjust;
  SWPower += adjust;
  SEPower -= adjust;

  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.
2. Bad PID algorithm.
3. Bad PID gains.
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.

Thanks for any help you have to offer!

Answer 1

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.

Answer 2

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.

Answer 3

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;
//add/soustract from THROTTLE (pulse_duration[throttle_chanel])
//change the sign (+ / -) acc
pulse_length_esc1=throttle_gaz + roll_length + pitch_length - yaw_length;
pulse_length_esc2=throttle_gaz - roll_length +  pitch_length +  yaw_length;
pulse_length_esc3=throttle_gaz + roll_length - pitch_length  + yaw_length;
pulse_length_esc4=throttle_gaz - roll_length -  pitch_length  -  yaw_length;