Quadcopter stabilization algorithm

kakou19:
Can you explain exactly how PID works or post some pseudo code please. Im a bit confused...

If you really want to understand PID - I suggest that you take Udacity's CS373 class; in it, PID is described and implemented (in Python) in a way that makes it very clear how it works and why. This is only one small portion of the course, though; you can't just jump to that part and hope to understand it (unless you have a good grounding in the earlier parts of the course). Depending on your skill levels, the course may be considered from "easy" to "very difficult" (the latter being true if your understanding of linear algebra and probability/statistics is low to non-existant) - but in the end is well worth the struggle.

On a similar note:

Udacity recently announced that they have brought back the original AI course that Stanford ran last fall/winter (I am contemplating enrolling in it - I was in it last fall, and had to drop out halfway through due to personal issues - I am so glad they brought it back!)...

Here's how simple it can be using all 3 P,I,D:

loop
error = setpoint - measured_value
integral = integral + errordt
derivative = (error - previous_error)/dt
output = Kp
error + Kiintegral + Kdderivative
delay(dt)

While experimenting you can assign dt,gain to the value of R/C channel 5,6.
Let's start by controlling altitude only using the throttle channel, P term only:

void loop()
ft=get_altitude();
set_throttle((wasft-ft)*gain+wasthrottle); //gain is Kp
wasft=ft;
delay(dt);

A few more lines gives us I term:
const int NZEROSLOW=3;
const int NZEROSTOP=15;

ft=get_altitude();
chg=wasft-ft;
if last is in correct direction toward setpoint and j-lastj > NZEROSLOW then chg=0;
set_throttle(chg*gain+wasthrottle);
if chg then
last=chg;
lastj=j';
j++;
delay(dt);

It's not perfect, oversimplified a bit to make it easier to understand.
Another improvement in case it's moving too slow, not near the setpoint:

if ft-setpoint > 10ft and j-lastj > NZEROSTOP then chg= toward setpoint;

You can use the same method for pitch, roll, yaw using magnetic compass and GPS to correct towards setpoint.

Simple? Questions? Improvement? It works!

PS- Oh great library BTW! It's not needed for my solution here.

Can you tell me why it won't work properly before I make these changes?
Ideas for improvement?
Thanks for taking the time to read it.

sbright33:
Ideas for improvement?

How are you measuring altitude, and what sort of accuracy in the altitude hold are you trying to achieve?

I'm using the BMP085. Over a short time period it can detect changes of 1-2ft precision. I'd like to keep it within 10ft of the setpoint, where I activate it. Using the P term only loop, you have to sample often to keep it stable, between 1-5 samples/sec. At 2 s/sec it does not change very often when it's stable. That's why there are so many 0 values in chg. Instead of looking at the magnitude of chg, I have to count the number of 0's in a row for the I term. Am I making sense? Or am I in my own world?

Thank you everybody, my stabilization algorithm works well !

I'm from Missouri. Show me? I'd like to see how yours is different from mine. Since mine is so short, I'm sure you can offer me some improvements.

sbright33:
Ideas for improvement?

In the pseudo-code you seem to be controlling the timing by a fixed delay(dt) and then assuming that the sample interval is dt. Firstly I'd suggest using the actual loop interval in your calculation (either by explicitly controlling the loop interval, or by measuring it). Secondly, I'd have thought that reducing the sample interval would reduce the integration errors which suggests it might be desirable to run this control loop as fast as possible.

I appreciate your help! Let's talk about this. Without this interaction I'm too close to the project and will miss something. Talking about it out loud helps me think outside the box and become more creative. Since dt is about 500ms, the delay is about equal to the total loop time. In my code I don't use delay, I simplified it here to make it easier to read. You're right it is a fixed delay. During experiments I can vary dt by turning Channel 5 on my transmitter. Once I have chosen a value that works without oscillation, I can make it a constant which seems to work in all situations. I could measure the total interval it would be about 501ms. The way it is written without delay, I am actually doing what you said already. I can tell you are paying attention, because you noticed the problem in my simplified pseudocode.

I do not use dt anywhere in my calculations. So I'm confused about that comment. I don't think lowering dt would help. Since we're talking about the I term, let's look at the 3rd piece of code in my posting. Even using 200ms, chg is often 0. Because the sensor resolution is 1ft. There is no change for many seconds once it has stabilized. That's why I count the number of 0's instead of using the magnitude of chg for the I term. Simply: If it's moving slowly in the correct direction, don't chg throttle, until it arrives. I can tweek NZEROSLOW to keep it from oscillating. Maybe your comments apply to the first piece of code using all 3 PID. That's just an example showing what I'm NOT doing.

sbright33:
I do not use dt anywhere in my calculations.

The integral and differential elements both require dt, and the psuedocode you posted seemed to use it correctly; isn't that the algorithm being discussed?

loop
  error = setpoint - measured_value
  integral = integral + error*dt
  derivative = (error - previous_error)/dt
  output = Kp*error + Ki*integral + Kd*derivative
  delay(dt)

That is an example of using all 3 terms, which I am not doing. See the code below it instead. I am not using the D term at all. I have handled the I term differently since dt is a constant. My algorithm is quite different from the original example as you can see.

Struggling to follow what the code does, because of very terse logic and undefined variables. It might be clearer to post real code.

The first thing that jumps out at me is that this involves noticeably more code than the simple PID example above it; in what respect do you see this as being better?

I agree with PeterH that the sampling rate should be higher. But then you say 10ft accuracy is all your after so who knows.

If you have a look at the Aeroquad code you'll see getting decent altitude hold isn't as simple as taking readings from the baro sensor. They run the sampling rate at something like 50Hz, filter the readings, adjust with temperature compensation, and combine the baro with accelerometer readings. Maybe you're already doing this in your code? It's hard to tell from the pseudocode you posted.

I'd like to see your code @kakou19.

I apologize that it is difficult to understand. I'm sorry, I can see why. Maybe I shouldn't have included the PID example first. Can we discuss my code instead of the theoretical example?

int ft, wasft, gain, wasthrottle, dt;

ft=get_altitude();
set_throttle((wasft-ft)*gain+wasthrottle); //gain is Kp
wasft=ft;
delay(dt);

Let me define my variables:
ft is altitude in feet from BMP085 sensor.
wasft is last time around loop
wasthrottle is last time around 1000-2000us
gain is usually about 10
dt is around 500ms

It's better than the example because I implemented it in the real world with a sensor and servos instead of just unitless theory.

The sampling rate would need to be higher if I wanted it to stay level, but the KK board is already doing that. The temp comp is already handled by my sensor on board. An Accelerometer doesn't help much with altitude only leveling? I'm not sure how sampling the barometer at 50hz would help since it's usually the same reading for many seconds. The code I listed is complete for altitude hold except obviously get_altitude() and set_throttle() which do exactly what they say. set_throttle sends the specified parameter to the throttle input of the KK board. I'm not sure if filtering the sensor would help because it is already clean data. It stays at the same value 99% of the time when it's not moving. Every once in awhile it'll tick up or down 1ft. Over time it will change as the weather changes but that is not relevant here. I appreciate your input which makes me think of things I haven't considered yet. Please continue. I encourage you to disagree that's how we learn.

Maybe a single Accellerometer in the vertical direction would help for altitude? I hate typing that long word. I can save the value of gravity when it's at rest. When it's higher it means I will soon be going up if not already. When it's lower I will soon be going down. In other words higher means my vertical velocity is increasing. The Ameter does not tell me what the current velocity is. When it's stable it will be near zero for many samples anyway. With an Ameter I COULD sample more often, as you suggested. With Ameter only I could keep it near gravity by adjusting the throttle. But then it would slowly drift, and I wouldn't know it. I have to combine it with the barometer for this idea to work. I've seen complex code for this here. Anyone know how to do it simply?

Combining or fusing Ameter and Gyro:

@sbright33

Im using the same algorithm for the PID as describe here Improving the Beginner’s PID – Introduction « Project Blog

Its working well for roll and pitch, but I have a lot of problem to stabilize on the yaw axis, did you succeed?

I think you found the best solution for your goal of roll and pitch. How often do you sample? Have you worked on altitude hold yet? The yaw will drift without a compass. Do you have one connected? How did you choose tour tuning variables?

How about this code to fuse Ameter and pressure? I could sample at 20hz instead of 2hz. wasft-ft would often be 0.

set_throttle((wasft-ft)*gain1 +(gravity-accel)*gain2 +wasthrottle);

I know it's too simple, but would it work?

sbright33:
How about this code to fuse Ameter and pressure?

Have a read of this. It's not that simple and the Aeroquad guys still haven't got it working reliably. You can get it working on a test bench:
this video

But it's a totally different story on an actual flying quad.

Since it already works with my original code, I think the 2nd term in my formula may prevent/reduce any bumps, if I choose the proper value for gain2. Will experiment. Coming soon is Yaw. Also working on a fixed wing.

You can see the red line in the video does not perfectly match the movement of the sensor. There is still a delay. The peaks don't line up chronologically with reality. Sometimes the line is going up, when there is no movement etc. I admit it is better than I can do with pressure alone, because I cannot even detect 6" of movement with my cheap sensor. I have a delay also. But what good is such amazing resolution when it is often wrong? It will cause the quadcopter to shoot up or down when these errors happen, as you admitted they will. For my application I would rather have a reliable algorithm that keeps the craft within 10ft, instead of one that sometimes detects 6" of motion, other times screws up by a few feet. I am considering to make a similar demonstration using the same 2 sensors.