I´ve been working some weeks in a code for improvement of LOITER since I found not good enough for me (IMHO) the one of AC2.1.1.r8 (and previous up to 2.0.49).
I´ve started with the AP-NG that I´ve remembered quite good. I´ve translated to AC2 with some modifications.
Now I have a version that works quite well at least in HIL (need to real flight test).
I´ve implemented in AC2.1.1.r8 in a very simple way: by adding two new functions in "Navigation.pde".
static void calc_loiter_AFD(int x, int y)
static void calc_loiter_pitch_roll_AFD()
The rest of the code is quite the same (except the call of this funtions in Arducopter.pde and variables naming). PID´s has been tuned in HILL with AeroSim-RC and works right for light to hard wind.
This is under Dev-Team consultance for the moment, but you are free to test