Fix on waypoint time

Time checking for a wp was working only for first calling because of static "wp_reached" flag, fixed now