Sonar fixes, controller tests, abstract waypoint