Added new code to fly with vicon, no auto take off implemented