I want to show the progression I have done with my DronePilot framework, and in this video I'm showing how to do a trajectory controller.

I want to start by saying what I'm using to make this work:

  • Motion capture laboratory (optitrack)
  • Normal quadcopter 
  • Companion computer (raspberry pis and odroids)
  • Flight controller (I'm using a naze32)
  • Some python code (open source)

In my past blog post, I describe how a hover controller worked, and the video, in case you missed it, is here: https://www.youtube.com/watch?v=oN2S1qJaQNU ;

In this part we are focused on the Guidance (using the GNC argo). This refers to the determination of the desired path of travel (the “trajectory”) from the vehicle’s current location to a designated the target, as well as desired changes in velocity, rotation and acceleration for following that path.

There is several steps before we can achieve this. Mainly the next ones:

  1. Fly the vehicle using the flight stack
  2. Design a controller that will track/hold a specified position
  3. Create a trajectory, based on time and other factors

For the first part, in this blog we will use Altax Flight Stack, that compromises a companion computer and a flight controller. In this particular case I’m using a naze32 as flight controller, and two companion computers: Raspberry Pi 2 and Odroid U3.


The naze32 is connected to the Odroid U3 via a usb cable (a very short one). The vehicle is a 330mm rotor to rotor fiber glass frame, with 7×3.8in propellers, 1130kv motors, 15amps ESCs and a 3000mah 10C battery. It will fly for 11-13 minutes.

The Odroid U3 is running Ubuntu 14.04.1 in a eMMC module, which makes it boot and run generally faster. Its being powered by a BEC that is connected to the main battery.

The companion computer will “talk” a special language (multiwii serial protocol) in order to send commands to the vehicle, this one is described here. And the most important part is that it will run the DronePilot framework. This framework is the one in charge of piloting the vehicle. You can check it out here: https://github.com/alduxvm/DronePilot

And now the trajectory part…

We need to generate certain X and Y coordinates that then it will be “fed” to the position controller at a specific time. We are going to create two types of trajectories, circle and a infinity symbol. Why this ones? because this ones are easy to generate and perfect to excite all the multi-rotor modes.


This one is very simple… there is basically two parameters needed… Radius and angle. In our case the angle part we are going to combine it with the step time of the main loop of the controller and pi… basically the angle one will go from 0 to 360 degrees (in radians of course). The code looks like this:


So, if we declare “w” like this: (2*pi)/12 it means that the trajectory will take 12 seconds to complete a full revolution, and then start over. This is the parameter that we will change if we want the vehicle to travel faster. Its better to start with a slow value, and then progress to faster trajectories.

The next step is to “fed” this coordinates to the position controller inside the control loop. That is done in this script.

The infinity trajectory is a special one! this one is called in several ways: Inifity trajectory, figure of eight… And there is several ways of how to calculate the coordinates, you can see in the next gif the posibilites of how to create a figure of eight:


The one I like the red dot one! why is this?? that one is called the Lemniscate of Bernoulli, which is constructed as a plane curve defined from two given points F1 and F2, known as foci, at distance 2a from each other as the locus of points P so that PF1·PF2 = a2.


This lemniscate was first described in 1694 by Jakob Bernoulli as a modification of an ellipse, which is the locus of points for which the sum of the distances to each of two fixed focal points is a constant. We can calculate it as a parametric equation:


And then the rest is feeding that information to the position controller which will try to follow that trajectory as the dots on the plots. Magic. The next gif is a replay of the flight (DronePilot logs data for further analysis...).


The trajectory above is the fastest my vehicle can do (without getting unstable and or very shaky), it can complete a figure of eight in 6 seconds... does anyone know a human pilot that can do this?? I don't think so. hahaha.

Art that can be created using this:


The picture above is a long exposure of the vehicle doing the figure of eight trajectory.

What's next?? I want to change the flight controller, from a simplistic naze32 to a pixhawk! Why?? because the inner loop is faster, therefore I could create faster and more aggressive trajectories. You can check my blog post here: https://altax.net/blog/trajectory-controller/

E-mail me when people leave their comments –

You need to be a member of diydrones to add comments!

Join diydrones


  • Developer
    @aldo have you posted your question on http://discuss.ardupilot.org?
  • I'm sorry for the delay in replaying to this post...

    I have several blog posts where I talk and discuss what I call "flight stack", and also the vehicle that I use for this research, take a look here:



    I'm currently trying to do it for pixhawk, but I have some issues, and I'm having problems getting support from the dev team :(

    Thanks everyone!

    Building a quadcopter
    in under two hours!!!   This is a small post on how to build a quadcopter. I know there is tons of similar posts on the web, but anyhow I wanted to s…
  • Wow! Beautiful work, congratulations!

    that's exactly was I was seaching for, please update us regarding pixhawk implemetation.

    So now you are planning to do it with Arducopter platform using a serial port in the FC?

    Are you planning to speak with pixhawk with mavlink ??


  • Hi, it is great thing!

    would you like list the all detail components of yout flying  test platform? i also want to build the same one for experiment.

  • Developer

    I like the theme of combining a microcontroller FC for the sensors with the RPi companion. As a cheaper and smaller high performance option than pixhawk I like the look of the Openpilot revolution FWIW. 

  • Hello and nice work!

    Will read ur blog further but do you use external position feedback? If not cgtz!

    You could speed the Bernouilli's "eight" by transforming the equation from constant angular speed to constant forward speed : you could get lower overall travel time :)

This reply was deleted.