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
}
}
}


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)

• how accurate are you attitude estimates? how are you generating attitudes? Apr 4 '16 at 1:58
• I'm using MPU 6050 as my sensor. I wrote the fro IMU drivers using registers from the datasheet. The angle measurements are decent but very sluggish. On a Horizontal surface, the reading for the roll is -2 degrees and the pitch reads -3 degrees. These values sometimes over long period of time. When I bring the quad at 90 degrees, the actual readings are around 78-82 degrees. When the motors speed up, the angles start to fluctuate noticeably (around 4-5 degrees from idle state). Also, I should mention that I used a 10ms sample time, that I just simply guessed, for integrating the gyro speed. Apr 4 '16 at 4:26
• I'm using complimentary filter to fuse accelerometer angle and gyroscope angles. Apr 4 '16 at 4:51
• Use a nonlinear kalman filter. Complimentary filters should not be trusted. They are used by computer people who want to get into drones. Apr 4 '16 at 5:45
• @holmeski- I will look more into the statistical signal processing, but for now I need to make sure im implementing my PID correctly. Non-linear kalman filter is probably better suited for my quadcopter since my system it produces a lot of vibrations. Hopefully it could also make my angle change less sluggish. If not, then I will probably have to look at the mpu-6050 configuration. Apr 4 '16 at 21:22

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.
1. 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.
1. 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.
1. 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).

• Are suggesting I should change my motor speed using: motor speed1 = motor speed 1 + PitchPID motor speed 2 = motor speed 2 - PitchPID motor speed 3 =motor speed 3+ RollPID motor speed 4 = motor speed 5 - RollPID, Instead of using the speed adjustments to pairs or motors like the one I posted in the second function? Apr 4 '16 at 4:02
• Also, I should mention that I didnt take any sample time for any of the PID gains calculations as I was not sure how its done or why its important. Would it be possible to accurately record sample time using only a oscilloscope/LA? Also, would I only need to measure the length of the period it takes to complete one of the PID functions? Apr 4 '16 at 4:14
• It will probably be better if i used timer interrupt since the time it takes to complete a loop will vary depending on conditions. Apr 4 '16 at 5:26
• @SaeedMolaie - Yes, using an interrupt is the best route because you know for a fact what the interval is, you don't need to spend time measuring it, and it's stable and unaffected by logic branching or data transmission in your base code. Apr 4 '16 at 18:31
• Regarding your other comment, you can add them all together. The PID output will give you some value (thrust) to use to modify your motor speeds. So motor1 = altitudeSpeed/4 + pitchPID/4 -rollPID/4 +yawPID/4. Flip the signs as appropriate for all the motors. The sum of all four motors should be altitudeSpeed - there should be two positive pitchPID/4 and two negative pitchPID/4, etc., such that the two forward motors combine for pitchPID/2 and the two aft make -pitchPID/2, and then those combine as (+x)*pitchPID/2 + (-x)*(-pitchPID/2) for pitchPID. Apr 4 '16 at 18:35