WPNav: add yaw control for straight line wp nav
Copter; integrate WPNAV's get_yaw
Copter: remove original_wp_bearing

To-Do: extend wpnav yaw control to only repoint nose towards waypoint if
it is more than 5m or 10m away