Navigation Control

Tridge, go ahead and look this over. I need to add some more documentation, but mostly I need to set the proper defaults for the new params. maybe i was not quit ready for a pullreq, but at least you can look it over. I have it flying very well in hitl (x-plane) and it's very robust even in winds were you shouldn't be flying. 
Oh and btw i just found the push button in tortise git ;) 
Cheers, Greg