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