Add gps-waypoint capabilities based on https://github.com/AutonomyLab/ardrone_autonomy/tree/gps-waypoint