Navigation formulas and ideas

I have been working on some navigation formulas and below you can see the results running in a small simulator i wrote. The algorithm makes the plane follow a line between two wayoints by minimizing XTE in a PD-loop. If the angle to the target WP is to more than say 75 degrees tha plane will head directly to the WP ignoring the line. Below is a bad video showing what it looks like. As soon as i have converted the code to C that can run on the Mega168 i will post it here.I also thouht this could be a start of a discussion about navigation formulas that can be implemented in the AurdinoPilot./MagnusHardware in the loop simulation now working!I have now my line following code working in my ATMega128. I think it will run in the Ardupilot as well when i receive it.The simulation works as follows:The world is simulated in the PC. A simple GPS simulator calculates the planes position based on the airframes speed vector, last position and a wind vector. NMEA data is sent to the AP.The Autopilot handles waypoints and navigation based on the NMEA data. A PID regulator calculates how much the plane should turn to follow the line to the next WP. Target turn angle is sent to the PC which simulates the airframe. The airframe simulation is very simple and just adds a delay to the turn angle and calculates a new heading. The heading and airspeed gives the airframes speed vector which is used in the word simulation... Loop closed.Right now the code is quite ugly, but it works as expected except when reaching WP0. I am not sure why it turns away from the line, but it makes a circle before it sets out on the right direction.If anyone is interested in looking at the source, please send me a message.In the video the plane is flying in a 5m/s wind from the southeast. It is actually quite fascinating to see the plane holding up against the wind to hold the line./Magnus

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

Join diydrones

Email me when people reply –


  • How can you control the plane's attitude? I can not see any pitch and roll information on your vedio.
    By the way that is a wonderfull work.One of my friends are working on the same field. He plans to build the simulator in 3D type with OpenGL. Next, his work will be implemented in a PC104 based UAV.
    Could you send me the code? Thanks!
  • So far i only introduced a delay in response in roll rate and a small random roll disturbance. Next step is to add a wind vector. Hopefully later tonight. I plan to make my simulator work with the software running in the hardware. I'm waiting for my AurdinoPilot but for now i am planning to run the code on my Robostix which has a similar cpu.

  • Magnus it will be interesting to see how this works in the simulator work Michal is doing. Looks like good stuff - what types of errors in tracking did you introduce to make the simulator work to keep the aircraft on line?

This reply was deleted.