Added waypoint yaw control