Quadcopter PID Algorithm

Question

I'm trying to implement a PID control on my quadcopter using the Tiva C series microcontroller but I have trouble making the PID stabilize the system.

While I was testing the PID, I noticed slow or weak response from PID controller (the quad shows no response at small angles). In other words, it seems that the quad's angle range has to be relatively large (above 15 degrees) for it to show a any response. Even then, the response always over shoots no matter what I, D gains I choose for my system. At low P, I can prevent overshoot but then it becomes too weak.

I am not sure if the PID algorithm is the problem or if its some kinda bad hardware configuration (low IMU sample rate or maybe bad PWM configurations), but I have strong doubts about my PID code as I noticed changing some of the gains did not improve the system response.

I will appreciate If someone can point out whether i'm doing anything wrong in the PID snippet for the pitch component I posted. I also have a roll PID but it is similar to the code I posted so I will leave that one out.

void pitchPID(int16_t pitch_conversion)
{
  float  current_pitch = pitch_conversion;
  //d_temp_pitch is global variable
  //i_temp_pitch is global variable
  float  pid_pitch=0; //pitch pid controller
  float  P_term, I_term, D_term;
  float  error_pitch = desired_pitch - current_pitch;

  //if statement checks for error pitch in negative or positive direction
  if ((error_pitch>error_max)||(error_pitch<error_min))
    {
      if (error_pitch > error_max) //negative pitch- rotor3&4 speed up
        {
          P_term = pitch_kp*error_pitch; //proportional
          i_temp_pitch += error_pitch;//accumulate error
          if (i_temp_pitch > iMax)
            {
            i_temp_pitch = iMax;
            }
          I_term = pitch_ki*i_temp_pitch;
          if(I_term < 0)
            {
            I_term=-1*I_term;
            }
          D_term = pitch_kd*(d_temp_pitch-error_pitch);
          if(D_term>0)
            {
            D_term=-1*D_term;
            }
          d_temp_pitch = error_pitch; //store current error for next iteration
          pid_pitch = P_term+I_term+D_term;
          if(pid_pitch<0)
            {
            pid_pitch=(-1)*pid_pitch;
            }
          //change rotor3&4
          pitchPID_adjustment (pid_pitch, 'n'); //n for negative pitch
        }
      else if (error_pitch < error_min) // positive pitch- rotor 1&2 speed up
        {
        P_term = pitch_kp*error_pitch; //proportional
        i_temp_pitch += error_pitch;
        if (i_temp_pitch < iMin)
          {
          i_temp_pitch = iMin;
          }
        I_term = pitch_ki*i_temp_pitch;
        if(I_term > 0)
          {
          I_term=-1*I_term;
          }
        D_term = pitch_kd*(d_temp_pitch - error_pitch);
        if(D_term < 0)
          {
          D_term=-1*D_term;
          }
        d_temp_pitch = error_pitch;
        pid_pitch = P_term+I_term+D_term;
        if(pid_pitch<0)
          {
          pid_pitch=(-1)*pid_pitch;
          }
        print(pid_pitch);//pitch
        printString("\r\n");
        //change rotor1&2
        pitchPID_adjustment(pid_pitch,'p'); //p for positive pitch
      }
    }
  }

 

void pitchPID_adjustment(float pitchPIDcontrol, unsigned char pitch_attitude)
  {
  if (pitchPIDcontrol>(maximum_dutyCycle-set_dutyCycle))
    {
    pitchPIDcontrol=maximum_dutyCycle-set_dutyCycle;
    }
  switch (pitch_attitude){
  //change rotor1&2
  case 'p': //positive status
    PWM0_2_CMPA_R += (pitchPIDcontrol);//(RED)//motor1
    PWM0_0_CMPA_R += (pitchPIDcontrol);//(Yellow)//motor2
    break;
  //change rotor 3&4
  case 'n': //negative status
    PWM0_1_CMPA_R += pitchPIDcontrol;//(ORANGE)//motor3
    PWM1_1_CMPA_R += pitchPIDcontrol;//(green)//motor4
    break;
  }

Also, can someone please tell me how this motor mixing works?:

Front =Throttle + PitchPID 
Back =Throttle - PitchPID 
Left =Throttle + RollPID 
Right =Throttle - RollPID

vs what I did in the function:

void pitchPID_adjustment(float pitchPIDcontrol, unsigned char pitch_attitude)

Answer 1

The glaring issue I see at the moment is that you are forcing polarity on the I and D terms. In general, you are using a lot of sign checks, sign assignments, and conditional programming.

None of that belongs in a PID controller. The entire controller should look like:

pError = Input - Output;
iError = iError + pError*dt;
dError = (pError - previousError)/dt;
previousError = pError;

ControlSignal = kp*pError + ki*iError + kd*dError;

That's it. No need for checks, assignments, etc. See my related answer on splitting the control signal as opposed to control each motor pair individually (which is what I believe your goal is with the if elseif code).

Imagine a PID controller as three people watching your vehicle, all trying to give advice based on their "experience" (mode). Each "person" will give you one of three "statements" So you have:

• Mr. Proportional - This person looks at where the output is and compares it to what you asked for. The statements given by this person are:

1. There is a large difference between what you want and what you have - take a big action.
2. There is a small difference between what you want and what you have - take a small action.
3. What you have is what you asked for - take no action.

• Mr. Integral - This person looks at the same error value Mr. Proportional does, but compares it to how long it's been that way. The statements given by this person are:

1. You have chronic/acute error (small error for a long time or big error for a small time) - take a big action.
2. You have mild error (small error for a short time) - take a small action.
3. Your error history is neutral (time * positive error is equal to time * negative error) - take no action.

• Mr. Derivative - This person looks at the same error value Mr. Proportional does, but compares it to how it's changing. The statements given by this person are:

1. Your error is getting bigger - take a bigger action.
2. Your error is getting smaller - take a negative action.
3. Your error is not changing - take no action.

It's important to note statement 2 of Mr. Derivative up there - imagine you're driving a car behind someone who is already at a stop. As you get closer to them (error is getting smaller), not only do you want to let off the gas, but you also want to brake! Derivative action is what gives you "braking" - no other term (P or I) gives you negative action until you're past the setpoint. Derivative is the only term that tells you to slow down because you're getting close.

Another way to help understand these terms is to understand physically what they mean. Say your reference is a speed. This means:

• Proportional error compares your speed to your target speed. Want to be going 60 and you're going 55? Speed up. Going 65? Slow down. This is easy.
• Integral error compares the integral of target speed to the integral of actual speed. This means that it's comparing a target position to your actual position. Were you supposed to be in the city already? Then you need to speed up. Were you supposed to be in the city and you're still at your house? SPEED THE ENTIRE WAY.
• Derivative error compares the derivative of the difference between target and actual speeds. Is the person in front of you pulling away? Speed up! Is the person in front of you pushing their brakes? Slow down! As I mentioned above, if your target is to be immediately behind the person in front of you, then proportional and integral will both "tell" you to speed up. Derivative is the only "person" to tell you you're going to rear-end them if you don't start braking.

SO, what happens when you force signs on the error terms?

Let's suppose you're in a car, trying to follow a friend to a restaurant. Here the friend's speed represents the speed reference and your speed represents your speed feedback. Speed limit on the road is 35mph (55kph). Here's what happens:

1. Your friend begins to move.
2. You are still stationary, so the following errors happen:

• Proportional error is positive (you want to go 35 and are actually going 0).
• Integral error is a little positive (your friend is farther from you).
• Derivative error is large and positive (your friend is quickly pulling away from you).
• You force signs: force integral error to be positive (already is) and derivative error to be negative.
• This means that derivative error is "telling" you that the car is getting farther away from you, but you invert that and assume that derivative error meant to say that you are getting closer to your friend. This is wrong.
• Actions: Proportional - press the gas a moderate amount. Integral - Press the gas pedal a little bit. Derivative - Should be press the gas pedal a lot, but you inverted it, so instead you press the brake a lot.

3. Eventually your friend gets far enough away that the proportional and integral error becomes large enough that it overrides your (incorrectly inverted) derivative term. At this point:

• Proportional error is large (still going zero and want to go 35).
• Integral error is very large (your friend is very, very far in front of you).
• Derivative term is still large (friend is still actively getting farther from you), but you are still forcing it to be negative.
• Actions: Proportional - press the gas a lot. Integral - Floor the gas pedal. Derivative - Should be push the gas pedal, but you inverted it, so instead you press the brake.

4. After some time, you get to 34.999 mph. Proportional error is still (slightly) positive because you want to go 35 and you're actually at 34.999, so proportional error is 0.001

• Proportional error is barely positive (still going slower than 35mph)
• Integral error is at its largest (you are farthest from your friend thus far because your friend has been going 35 the whole time)
• Derivative error is roughly zero (you're almost the same speed your friend is, so now the proportional error stabilizes)
• You force signs: Force integral error to be positive (already is) and derivative error to be negative (it's almost zero, so negligible change).
• Action: Proportional - No action because you're almost at 35mph. Integral - You are now really far from your friend, so you floor the gas. Derivative - No action because the proportional error is almost stable.

4. Now, because you were flooring the gas, you pass 35mph and hit 35.01mph. Now, your proportional error becomes negative (want 35, are going 35.01, so error is -0.01).

• Proportional error is almost zero (going just over the speed limit)
• Integral error is very large and still very positive because you are actually very far behind your friend.
• Derivative error is almost zero (because proportional error is still almost zero).
• You force signs: Force derivative error to be positive - little change because it's almost zero anyways. The problem here comes when you force integral error to be negative - it was very large and very positive! Now you're forcing it to be negative. This means Mr. Integral was telling you you're very far behind your friend, but you invert that and assume integral error meant to say that you are very far in front of your friend.
• Action: Proportional - No action because you're going 35mph. Integral - You are very far behind your friend and should floor the gas pedal, but you inverted that and now think that you are very far ahead of your friend, so you stomp the brake pedal instead! Derivative - No action because proportional error is pretty stable.

At this point you hit a loop - you slam on brakes when your speed just passes 35mph because you invert integral error, then you floor the gas when you fall below 35mph because you un-invert integral error. This should (violently!) jerk the car (aircraft) around and prevent you from ever eliminating any steady state error.

Worse, I'm not sure how this would behave once you get to the set position, but I think the constant sign flipping might prevent you from stopping anywhere close to where you wanted (if it ever stabilizes at all).