navigator: check if yaw reached only when position reached