Go Down

Topic: Quadcopter stabilization algorithm (Read 50530 times) previous topic - next topic


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? 


Dec 02, 2012, 09:57 am Last Edit: Dec 02, 2012, 10:19 am by kakou19 Reason: 1
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.


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.


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?

Code: [Select]

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


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;

set_throttle((wasft-ft)*gain+wasthrottle);  //gain is Kp

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:



Im using the same algorithm for the PID as describe here http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/

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


Dec 03, 2012, 03:36 pm Last Edit: Dec 03, 2012, 03:43 pm by sbright33 Reason: 1
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?


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.

Go Up