Signed-off-by: GAFletcher <gafletcher@att.net>
 Lots of changes to the attitude.pde, only to calc_nav_roll function.
Also lots of stuff added to set_next_WP and set_guided_WP.
The way it works is by doing a fixed turn radius on a wp change and then reverting back to pid nav control at the proper bearing. All turns are contoled and strait line navigation is with the normal pid controller. After porting from AP 2.65 to the latest master AP-HAL some thing happened to the loiter controler. So, needs more work.