Working hovering and waypoint navigation.
Fixing errors