Fix initial waypoint offset