|Terms of service|
|Let's keep it simple
YMFC-32| GPS hold part 2| The STM32 - Arduino DIY autonomous drone
This page contains the full script that I used for making this video.
Hello and welcome to the second and way to long video about the GPS hold function of the YMFC-32 flight controller. But there is a lot to talk about so let's get started.
In the previous video about the GPS hold function I explained the workings of the GPS system and how the module is setup for using it with the YMFC-32 flight controller software. In short the output is set at 57.6 kilo bauds per second with a refresh rate of 5Hz. Meaning that we get 5 new latitude and longitude values per second. And last we disabled some unnecessary lines to offload the microcontroller.
If you haven't seen this video I highly recommend to watch it first as it holds valuable information. Another benefit is that this video will make more sense.
So, 5 times per second the STM32 receives a data block like this from the GPS module. As you can see here, the GGA line holds the latitude, longitude and the number of satellites that was used for calculating the position information.
Important to know is that the number of satellites that is used in this line will have a maximum value of 12. If we have a look at this screen we can see that more than 12 satellites are green. But the number in the GGA line still shows 12.
Ok, Finally the SA line holds the fix type that we can use as a quality indicator.
Because the NMEA protocol has a fixed format, the specific data field can always be found at the same location. This makes it possible to easily filter the latitude, longitude, number of satellites and fix type from de NMEA lines.
Now let's have a look at the YMFC-32 software. The program will check the incoming bytes from the GPS module in search of the dollar sign. This is the start indicator for every new line. When a dollar sign is received the incoming_message array is fully erased and the message_counter variable is set to zero.
After that it will store all the incoming bytes in the incoming_message array up to the point that an asterisk is received. This marks the end of the NMEA line.
Because NMEA lines are standardized we can look at predefined locations for needed information. For example, this line in the code will look for the LL signature. When this NMEA line signature is found it will also look for a comma at the data location 7.
This line is only true when there is no valid GPS signal available. So, when this line is true the LED status is changed and some parameters are set back to zero. This way we will make sure that the latitude and longitude are erased when the GPS has lost its signal.
Because the GPS is set at 5Hz the LED on the STM 32 will blink at 2.5Hz when it receives valid serial GPS information. In short, if the LED on the STM32 does not blink at 2.5Hz or does not blink at all when you power the quadcopter you know that there is something wrong with the GPS module as it is probably not accepting the u-blox configuration protocol.
In the setup part you can find this line: #define STM32_board_LED PC13 If your LED on the STM32 is not connected to pin PC13 you can change it here. Otherwise it will never blink.
Ok, a blinking LED means that the GPS is sending valid data. After a while the GPS module should get a fix. Meaning that this line isn't true anymore.
However, the GGA line will now holds a valid latitude, longitude and the number of satellites. Meaning that this line will be true.
And the latitude and longitude are filtered from the NMEA line with these two program parts.
Interesting side note is that the latitude and longitude are given in degrees and minutes as you can see here in this u-blox document.
Because minutes are not very usable for calculations they will be convert into degrees. That is all done in this part.
So at the end of the calculations the lat_gps_actual and lon_gps_actual will hold the latitude and longitude in degrees.
And of course the variable number_used_sats will hold the used satellites.
Ok, now it's getting interesting. The output of the GPS is 5Hz. Meaning that we get 5 new values per second. For the GPS hold function we want to use a simple PD controller.
In short the P output is the result of the difference between the actual GPS position and the stored GPS hold position multiplied by the gain.
So, when the distance between the actual position and the set GPS hold position is increasing the output of the P controller also increases. As a result the quadcopter will tilt more and more as you can see in this example.
However, if only a P controller is used it will constantly overshoot its GPS hold position because it's not slowed down by some form of friction. Maybe there is some wind resistance but that's not enough to get a stable GPS hold function.
So we need some sort of parachute function to slow down the oscillating motion. And this is where the D controller fits in.
The output of the D controller is the difference between the previous position and the actual position multiplied by the gain. Because we have a fixed GPS output interval of 5Hz the difference between the previous position and the actual position is an indication of speed.
So, the faster the quadcopter moves, the larger will be the output of the D controller. This also makes it completely independent of the GPS hold position.
The goal of the D controller is to act as a parachute that will slow down the movement of the quadcopter.
In this example I only set the D-gain. Look what happens when I activate the GPS hold function.
The change in position in the beginning is large and the D-controller reacts very strong to slow down the quadcopter. When the speed is decreasing the D controller outputs a smaller correction until there is a balance in speed and correction. So it will never come to a complete stop with only the D controller.
And when the P and D controller work together you get a very stable GPS hold function.
So, sounds cool and right. Well as always there is a challenge. Let me explain.
Every degree in latitude equals approximately 111 kilometers.
And the same is true for the longitude around the equator.
As you can see the distance between the longitude degrees is decreasing closer to the north and south pole.
With the YMFC-32 we have 6 numbers after the full degree. Now it's possible to create an higher accuracy but for now 6 digit's will work fine. So 111 kilometers divided by one million equals 11 centimeters.
In short every last digit change of the latitude for example equals approximately 11 cm.
When the quadcopter is moving at 20 kilometers per hour or 5.5 meters per second in the latitude direction the quadcopter needs some serious corrections from the D-controller to slow it down efficiently. Let's say a simulated stick movement of 60%. This equals a servo pulse of 300 microseconds because the full stick deflection results in a 1000 till 2000 microsecond pulse.
Divide 5.5 meters by the refresh rate of 5 times per second and we get the a traveled distance of 1.1 meters per 200 milliseconds. Meaning that the latitude will change 10 points every 200 milliseconds.
So, the D-controller should output 300 when the quadcopter is traveling at 20 kilometers per hour. This means that the D gain should be 300 divided by 10 equals 30.
Now imagine that the quadcopter travels at 5 kilometers per hour. At this speed the D controller should still try to slow down the quadcopter. 5 kilometers per hour equals 1.4 meters per second or 28 cm per 200 milliseconds.
This means 3 or 2 points of latitude change per 200 milliseconds. Multiplied by a gain of 30 we get an output of 90 or 60.
And that's a problem. Because an ESC change of 30us is clearly noticeable and gives the idea that the multicopter is unstable.
In this example the pulse for the ESC is changed between 1500 and 1530. And you can clearly hear the difference.
Very long story short, we need a much higher refresh rate of the latitude and longitude for the D controller.
Now it's possible to increase the refresh rate of the GPS module from 5Hz to 10Hz. But I discovered that this is still not fast enough for a smooth D controller output.
Now what if we take the previous latitude value from 200 milliseconds ago and the current latitude value and divide the difference by 10. It's now possible to add this result to the previous latitude value every 20 milliseconds. This way we can simulate a GPS refresh rate of 50Hz that is only 200 milliseconds slower than the original GPS output.
And you guessed it, a 50Hz output gives us a good base for a stable D controller.
If we have a quick look at the code you can see that the division by 10 is done here.
In this part the difference is added every 20 milliseconds.
And now that we have a 50Hz refresh rate it's also possible to use a rotating buffer to create an average change is distance. this is the same technique that was used for the altitude control.
And that is all done in this part of the code.
Finally we can calculate the P and D controller output in these two lines. And that's it. We have just created an amazing GPS hold algorithm that works very well.
So, now that we have our PD controller working it is time to have another look at this map.
Here we have north and south. In between there is the equator. As you can see the equator represents 0 degrees. When the quadcopter is flying from south to north the latitude is decreasing. When the quadcopter passes the equator and still flying north the latitude increases.
And the same is true for west and east where the prime meridian represents 0 degrees.
Ok, here is the problem. I build the PD controller for this part of the world as I'm living in the Ntherlands. Meaning that, when the quadcopter is pushed north the latitude is increasing and the PD controller will command the quadcopter to move south.
However, when I fly in this part of the world the quadcopter should correct in the opposite direction. In short the output of the PD controller should be inverted.
And again the same is true for the east and west part of the world.
Luckily the GPS module has a very convenient indicator so we know in what part of the world we are. It's called the north south indicator and the east west indicator.
So, we can simply check these indicators and invert the output of the PD controller. And that is done in this part of the code.
Ok, next challenge: the calculated PD controller output is orientated with the nose of the quadcopter north. When the quadcopter is flying with its nose in another direction the calculated PD controller output must be virtually rotated so the corrections are still in the right direction.
This virtual rotation is done in these two lines and are based on the current heading of the quadcopter.
And finally we will limit the controller output to plus or minus 300. And finally we have the roll and pitch control signals for the GPS hold function.
Only thing left to do is to have these signals control the quadcopter.
In this part of the code you can see that the GPS roll and pitch adjust signals are simply added to the basic control signals that are coming from the pitch and roll stick of the transmitter. And to make sure that the quadcopter doesn't go bananas we will limit the final control signals.
This way I still can override the GPS hold function with the control sticks as their output is 500 and the GPS is limited to 300.
And that's it.
Now let's have a look at how it works. The left switch on my transmitter switches between flight modes 1, 2 and 3 as you can see on the telemetry receiver.
Always make sure that you have enough satellites indicated on your telemetry receiver before you start flying. When this s changes into a capital S you have a 3D fix. A capital S in combination of more than 10 satellites should give good GPS hold results.
If you don't have a telemetry system you can also check the LED on the STM32. On startup the LED blinks at 2.5Hz. When the GPS is receiving valid position information the LED will blink at 25 Hz. And finally, when there are 8 or more satellites in use the LED will be solid on.
During flight, I can now switch to flight mode 3 and the quadcopter will try to maintain its position and altitude.
Another option is to activate flight mode 3 before the start. When I now start the quadcopter with the start sequence I only have to move the stick to the center position and the quadcopter will automatically take-off, gain altitude and hold its position fully autonomous.
And that's the GPS hold function of the YMFC-32 flight controller. I hope you enjoyed this video and that this project motivates you to start building you own multicopter flight controller.
Thank you for watching and see you next time.