Improve Route waypoint arrival criterion logic.
(cherry picked from commit e0261b83737fb4bcf9f4eaf8f83e03c36c56f189)

Conflicts:
	include/routeman.h
	src/routeman.cpp