Working waypoint system now aligned to map