added debugging output to waypoint control