Added waypoint code; fixed UAVTalk bug