fix waypoint movement