0216: mode2: fix waypoint finding when starting and already on course