YMFC-32| Altitude hold implementation| The Arduino - STM32 DIY drone

This page contains the full script that I used for making this video.

Hello and welcome to this video where I will explain the workings of the altitude hold function of the YMFC-32 quadcopter.

Altitude hold, as the name implies, is a quadcopter function that will keep the quadcopter at the same altitude.

Even if the quadcopter is pushed away from its position as shown in this demonstration.

The sensor that is used to measure the altitude is a very sensitive pressure sensor. When the quadcopter is gaining altitude the air pressure will slightly drop as the air pressure is higher close to the ground.

The math is very simple. Near the surface of the Earth, atmospheric pressure decreases almost linearly with increasing altitude. With every meter in altitude change the air pressure changes .117 mbar. So, if the quadcopter is flying at 30 meters the surrounding air pressure is approximately 3.5 millibars lower than when it's on the ground.

This is the YMFC-32 quadcopter that you saw in the video. My goal is to make it fly autonomous and make the complete code available for free on my website. The pressure sensor is placed inside this special housing under the GPS module. But more on that later.

The pressure sensor that I'm using is this MS5611 on a breakout board. As you can see in the datasheet it has a very high resolution of 10cm and a pressure resolution of .012 mbar.

Ok, before we start we need to know something about the hardware that we are dealing with.

To make testing easier I made this quadcopter test model. The pressure sensor is on the bottom plate as you can see her.

So this thing is sensitive, and I mean really sensitive. And that's great when you need high precision measurements as in altitude. But this sensitivity also has quite some downsides.

One of the main downsides is that this sensor is light sensitive. Here I have a test program running that shows the altitude in centimeters. And for those who do not use the metric system, every centimeter is approximately .39 inch.

When I change the altitude you can see the change on the screen.

When I point my flashlight at the sensor you can see that the altitude changes significant. So this is only a flash light. Imagine what the sun will do with the altitude hold function.

And that is why I made this special case on the CNC machine.

On the outside I made several holes where air can flow in and out. On the inside there is a light border. When light or sun is entering the holes, its reflected and adsorbed by the black paint. This way the sensor reacts very fast to changes in height but is not sensitive to light.

Another downside of the high sensitivity is turbulence. When flying behind obstacles the wind will roll over these obstacles and create small changes in air pressure. This also happens when the quadcopter is rapidly slowing down. Small changes in air pressure can create relatively large changes in altitude. As you can see in this example the altitude of the quadcopter changes significant due to the turbulence caused by the wind over an obstacle.

And now let's have a look at the breakout board. The MS5611 runs on 3.3V as you can see here in the datasheet. That is the same voltage as the STM32. However, this board is designed for 5V microcontrollers like the Arduino. That is why there is a 3.3V voltage regulator on the board including a level shifter that will shift the I2C clock and data lines between 0 and 5V instead of 0 and 3.3V.

To overcome this problem I could simply connect the 3.3V of the STM32 to the supply voltage of the MS5611 breakout board. I have tested this on a breadboard and it works fine. However, if you have seen my videos about electromagnetic interference on quadcopters you might remember that the voltage regulator on the breakout board is an awesome noise filter. I will put a link to this video in the description.

So I would like to keep using the voltage regulator on the breakout board if possible. Well, as it turn out, this is possible. The only change that I made are the location of the clock and data lines. As you can see here I connected the wires on the 3.3V side of the level shifter.

This is no easy soldering and it might ruin your board when the sensor is getting to hot or you accidentally desolder a component. So please thing twice before you do this.

Again, you can also provide 3.3V to the breakout board and it should also work fine. The rest of the wiring is straight forward as you can see on this schematic.

Next thing is to get useful pressure data from it that we can use for the altitude control. And luckily this is all described in the datasheet of the MS5611.

First we need to retrieve some calibration values that are stored in the MS5611. After that we can poll the pressure and temperature data and calculate the pressure in millibars as shown in this section.

But as always there is a challenge.

To get pressure or temperature data we first need to request it. For the YMFC-32 we will use the maximum precision with a 4096 oversampling rate . After the request the MS5611 will sample the requested pressure or temperature data.

When we have a look at this part in the datasheet you can see that, after a pressure or temperature request, it takes approximately 9 milliseconds before the requested pressure or temperature is available. So we have to wait.

But waiting is not an option because the quadcopter program must keep its 250Hz refresh rate. If we suddenly wait 9 milliseconds the quadcopter will become seriously unstable.

So, how to get useful pressure values when the main program loop is running at 250Hz.

As already explained we can't do anything without the calibration variables. Polling these values can be done in the setup part of the flight controller program as we only need to read it once. After that we can start with the main program loop.

First let's request the pressure data as shown in the datasheet. Because the conversion of the data takes approximately 9 milliseconds we will do nothing in the following 2 program loops of 4 milliseconds.

After that we can poll the pressure data and immediately request the temperature. And again we need to wait for 2 loops before the temperature data becomes available.

When the temperature is available we can poll this data from the MS5611 and do the pressure calculation. And now we can request the pressure data and the whole thing starts all over again.

With this method we can get a pressure reading every 24 milliseconds, or 41.7 times per second and it works fine. As always I was wondering if it's possible to increase the refresh rate so we get more readings per second.

After some trial and error I found the following method that gave me the best results.

We start as before and read the pressure and the temperature data. But instead of reading the temperature every cycle it's also possible to read the temperature every 20 cycles. This is possible because the temperature change of the sensor is relatively small. The great benefit of this method is that it almost doubles the refresh rate.

Ok, here's the catch, when I now plot 200 calculated values you can see that there is a repeating pattern. Every 20 outputs the pressure spikes and slowly decreases. As it turns out this is caused by the temperature reading. After reading the temperature the following pressure readings somehow result is a higher value.

This might sound very undesirable but look what happens when I take the average of 20 readings. I becomes a very nice and usable line. This is because in every average there is only one pressure spike.

Ok, very important, the average must be the same as the temperature reading interval or a multiple. So 20, 40 and 60 will work fine. But as you can see 30 will not work as there are two pressure spikes in one average.

And with this we get a nice and stable value that reacts very fast. However, if I now feed this data to the Arduino plotter you can see that it is still a little bumpy. It reacts very fast but if we feed this into a PID controller the quadcopter reacts very jumpy.

So we need a cleaner signal. Next thing that I did in the code is applying a complementary filter that will use the 20 data point average as a base.

When I plot this line you can see that the result is a very clean signal. However, the downside is that this line reacts very slow to changes.

In summary, the fast reacting line is bumpy and the slow reacting line is perfect, but way to slow.

So, how did I fix this problem. Well, it's very simple actually. The slow reacting line gets a small dead band. If the fast reacting line stays within this dead band there are no actions.

When the fast reacting line is outside the dead band it will increase or decrease the slower value so it stays accurate.

This way I get the best of both worlds as you can see in this example.

And that is how the YMFC-32 is getting its pressure data.

But how is this data used to create a stable altitude hold function. Well, it's basically a PID controller that will do the magic. However there are a few modifications that you can find in the code.

One of the changes is that the P-gain is increasing when the error between the setpoint and actual value increases. This will make the quadcopter react soft around the setpoint and more aggressive when there is a large error.

The other modification is the D-controller. Basically what happens is this. The D-controller is fed by the 20 data points average that we calculated earlier. The difference between the current data point and the previous is stored in an rotating memory. This creates and average sum of 30 pressure changes that can be used for the D-controller function.

To really understand the code you need to study it yourself. For this video I made a simple program for the STM32 that you can download. A link is in the description. The full code for the YMFC-32 will be available on my website when all parts are explained.

And finally, how to use the YMFC-32 altitude control. When I start the motors with the throttle down nothing happens. When I increase the throttle to over half way the motors increase their rpm and the quadcopter takes off.

When the quadcopter takes off and accelerates, the hover throttle is registered with the use of the accelerometers. This way the throttle should be almost half way when hovering.

After that I can activate the altitude hold and the quadcopter tries to maintain it's altitude.

When I now increase the throttle the altitude hold function is disabled and the quadcopter can gain altitude. When I center the stick the altitude hold function is activated again.

And basically that's the altitude hold function of the YMFC-32. I hope you enjoyed this video and if so please take the time to give this video a thumbs up.

Thank you for watching and see you next time.