Improved Heading Controller

Note** these are 100meter orbits with really strong winds to provide a setting for this test. Hence the reason why the autopilot has a hard time holding a perfect circle. However the improved algorithm is holding so much better in the high wind scenario than the original algorithm!

This is an example of how to eliminate the oscillation in ground track using a heading error>bank angle type proportional controller. The groundspeed of the vehicle determines its turn rate for a given bank angle. Typically you tune the PID (only P is needed) for a given trim airspeed and assume that the ground speed is fairly constant and something close to the airspeed. However when there is high winds, this isn't the case. The ground speed can change +-50% in some cases in and out of phase with the wind if not more.

This causes oscillations on the "upwind" side because the turn rate is so much higher. To take this nonlinearity out of the system, I added the turn rate equation into the mix and solved this problem. The largest contributor to this problem is the GPS lag. So using this turn rate equation in the middle, it makes the controller more predictable with the changing ground speed.

I made an attempt at a video so hopefully it will better explain what is going on here. Especially beings a lot of people are having this problem!


E-mail me when people leave their comments –

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

Join diydrones


  • Developer
    Here is the quick documentation I came up with today. Hopefully this helps.

  • Developer
    I thought I hadn't actually done any documentation here.....Sorry, I'll write it up here soon and send it your way.
  • Developer
    Hi Ryan,
    Checking this out again and I still don't follow it. Do you have any code or pseudo code?
  • Developer
    No it was your stock airplane P.m
  • Pretty interesting study. Did you model your own airplane ?
  • Developer
    Hey Ryan,

    Did the "old" algorithm in the simulation have the de-weighting? IE something like

    nav_gain_scaler = ground_speed / TARGET_AIRSPEED;
    nav_gain_scaler = constrain(nav_gain_scaler,0.6,1.3);

    I assumed from the oscillation pattern that it did not or was turned off, but in the video you said it was used. If the old one did have the de-weighting scheme in place then this is a fantastic improvement ;)

    It would be interesting, since you now have an easy tool to see it, what effect changing the constraint values would have on the old algorithm and the new algorithm.
  • Developer
    Sorry the sound is so bad on the video, Youtube really pulled a fast one on the "processing" phase!
This reply was deleted.