VTOL improved RTH

This begins implementing a framework where there are FSMs
defined to achieve certain goals (e.g. fly to location and
land), which provides a layer of indirection between the
PathDesired UAVO and the actual navigation.

This is used to improve RTH so that it initially hovers, then ascends
to a minimum altitude, flies home, hovers, then lands at the home
location.

Here is a demo of doing RTH https://vimeo.com/79603584