Added destination change upon waypoint arrival