fiexed starting at nearest waypoint issue

fixes #442