0236: fix for nearest waypoint issue in mode2

#240