Quadcopter stabilization algorithm

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.

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

Can you explain how you're trying to control yaw?

sbright33:
I am considering to make a similar demonstration using the same 2 sensors.

Please do! I haven't got a quad yet but want to make my own and write all the code for it myself. So I've done a lot of research and wrote a simulator to test some ideas.

Since you only want an accuracy of 10ft or so, you may not need to do complex sensor fusion. But I'm thinking you'll need the accelerometer to limit the vertical speed, in the cases where the baro gives incorrect readings. I've got to buy a baro sensor so I can play.

BTW what are planning to do with the fixed wing? I was thinking of making an autonomous glider which would detect and stay in thermals. But alas it's already been done:

Sorry that I haven't had a chance to look at your actual code - it's been a busy couple of days. Since you say it is achieving a more-or-less stable height and not oscillating or drifting, I assume that you either have implemented the I & D components, or don't need them.

I am probably teaching Granny to suck eggs (I hope that expression translates to your culture!) but here are my thoughts:

If you are working at the limits of the resolution of your barometer then I suggest you design the algorithm to hover at the transition between two values, rather than sitting in the middle of a value (iyswim). So, rather than saying the target height is 'X' and the height error is (current height - X), you say that the target height is the boundary between X and X+1 (X + 0.5, in other words). When you are at that height, you should see relatively frequent transitions in the baro readings between X and X+1 and the proportion of time at X and X+1 should be roughly equal. By averaging multiple readings of a signal with noise present you can get better resolution than the sensor's nominal resolution for a single sample. I suspect you are already doing something of the sort, but the algorithm needs to be designed to find and stay at the boundary between adjacent sensor values for this to work well.

If you have a vertical accelerometer then you can use this to interpolate between the barometer readings, and the barometer can be used to compensate for accelerometer drift and cumulative integration errors. I have a motorsport data logger (I didn't make it!) which provides a resolution down to a few inches using the combination of GPS and an accelerometer; you should be able to obtain far better resolution if your barometer alone gives you resolution down to a few feet. The way I'd approach this is similar to the algorithm used for NTP - periodically compare the position implied by the accelerometer against the position implied by the barometer. If there is a consistent drift between them then skew the accelerometer accel/speed/position integration formulae to reduce that drift to zero. I'm sure you'll see the potential for a PID algorithm to control the drift. :slight_smile:

@jabbdo
We should share ideas! I suggest that you start with a KK board for $12 including gyros. There's plenty of work to do besides that, why reinvent the wheel? At least you can see how it's supposed to work with your 'copter hardware. My autonomous glider will also have a motor. I fly paragliders for a living so I'm familiar with thermals. I have seen that R/C is far easier to keep up in the same thermals. Especially for ridge soaring. I think that is overly ambitious for cross-country. Instead I will try to optimize my flight time by flying faster during a down draft, and slower when going up, even turning off the motor.

@Peter
This granny likes eggs so thanks! Boundary transitions, wonderful idea! Will update my code. I knew I posted here for a reason. I will listen to you from now on. I need to make some improvements to reduce oscillating and drifting. I'm thinking about your last paragraph. Sounds complicated, I wish it were simpler. Maybe I will come up with something.

I will keep you updated here as my projects progress!

PeterH:
The way I'd approach this is similar to the algorithm used for NTP

Is that Marzullo's algorithm you're talking about?

sbright33:
I fly paragliders for a living so I'm familiar with thermals.

That's not a job. It's getting paid to have fun :slight_smile:
I was looking at the KK boards since you mentioned them several days ago. But I can't find much info on firmware etc.

Be careful there is more than one version of the hardware.
Within each, there is more than one version of the firmware.
Look here under the Files tab:

Or Google kk board firmware v2.1?
I cannot see a reason to change the firmware that comes loaded when you buy it. But you can look at the code to learn how it works. Unless you want to download X configuration.

I think Marzullos algorithm is overkill for our needs. I'm not even sure how it is relevant. But you do raise an interesting question. It's not that the Ameter data is better than barometer. It has more precision, and less delay. That's the important part. But it also has more noise. How to use both together?

jabbado:

PeterH:
The way I'd approach this is similar to the algorithm used for NTP

Is that Marzullo's algorithm you're talking about?

Not really.

A lot of NTP is about determining propagation delays to find the consensus time, but the part that seems most relevant here is the algorithm used to steer the local clock towards the consensus time. Rather than simply adjust the local clock to correct for any discrepancies, the algorithm aims to control the local clock by skew adjustments so that the discrepancies tend to zero. That seems analogous to the current problem of detecting and correcting long-term errors in the integrated speed/position calculations by comparing them against the low resolution, but believed accurate, barometric height measurements.

sbright33:
I need to make some improvements to reduce oscillating and drifting.

Not to labour the point, but this is precisely the purpose of the D and I terms in PID. Your reluctance to use a conventional PID algorithm still puzzles me; it strikes me as the simplest and most appropriate solution to this problem.

I am using the I term, it just looks different. The D term is harder to implement because of the large dt. This problem is odd because of the large resolution of the sensor, the large dt compared to the changes in direction/speed, and the large number of 0's in my variable chg (even at a larger dt than I've chosen). Please show me how to implement the conventional PID within these limits? Are you suggesting my pressure sensor is inappropriate? Then why does it nearly work with only 3 lines of pseudocode? I don't mean to be difficult. Please teach me or show me some untested code that may help?