0260: fixed mode2 nearest waypoint issue rev2

#253