We all love drones, and we love to just buy one and go outside and make it fly by itself, this is great. But what is actually going on inside the drone?
In this post I'm going to explain a bit how a loiter controller works, with the difference is that I'll show my controller, share the python code and that I'm using a motion capture system inside my lab. The great MAST Lab.
First things first, you can check the code here. And secondly, I need to explain our setup. We are using Altax Flight Stack which is a tuple of computers connected with each other and sharing information. The flight controller is a very cheap naze32, running baseflight (but cleanflight will work as well), and the companion computer is a Raspberry Pi (any version will do the work...). The entire script does not consume too much CPU.
The connection diagram is showed above, the motion capture system is connected to a desktop computer and this computer sends the mocap data and the joystick information via a common wireless network (UDP), this information is used by the raspberry pi to know the position and attitude of the vehicle, then the rpi calculates a set of commands (roll angle, pitch angle, yaw rate and throttle) using simplistic PID controllers and then it sends the information to the flight controller.
This outer loop control is working at 100hz, but it can be configured to go slower.
Important to notice that we have crashed lots of times when starting to test/debug this system. Most of the crashes are due to the desktop computer "hanging out"... then the vehicle stops receiving the information and will keep the last command. A auto-landing feature is needed, this feature will be added on version 2.
In the part of the control, we are using angle mode on the inner loop (naze32) and then we calculate the appropriate angle commands (pitch and roll) from desired accelerations (outputs of the controllers) to make the vehicle hold the commanded position in the space. The most important part of the code is when we calculate desired angle commands from the desired accelerations coming from the PID controllers:
And the proper math:
The rest of the code is just to deal with data, vehicle and make everything work on threads. One thread for the control and another for receiving the data. The code is extremely easy to understand and to tweak (I hope...). With this setup, the joystick is the one that activates the automatic behavior, if the proper switch is on manual, then you will be able to fly the vehicle using the joystick. This is by no means the same technique used by Pixhawk in loiter mode. But perhaps is a nice way to start learning about flight modes (and controlling aerial vehicles) so that then you can learn how advanced flight modes developed by the team of PX4 and ArduPilot work.
With a couple of changes, this same script and controllers should work with dronekit+pixhawk... I'll try that soon.
You can see the post here: http://altax.net/blog/hover-controller/