More work towards waypoint nav.